def arrowkeys_to_dir(up_arrow: bool, down_arrow: bool, left_arrow: bool,
right_arrow: bool) -> int:
"""
Function that converts arrowkey presses (at most 2 simultaneously) to a direction in range 0..7 including
"""
if not up_arrow and not down_arrow and not left_arrow and not right_arrow:
logger.error(
"Direction not clear --> No arrowkeys pressed. Try again.")
return Direction.NULL.value
if up_arrow:
if left_arrow:
return Direction.upleft.value
if right_arrow:
return Direction.upright.value
return Direction.up.value
if down_arrow:
if left_arrow:
return Direction.downleft.value
if right_arrow:
return Direction.downright.value
return Direction.down.value
if left_arrow:
return Direction.left.value
return Direction.right.value
def __init__(self, images_per_second: int, top_camera: str,
front_camera: str, no_cam_feed: bool) -> None:
if top_camera == "" and front_camera == "":
logger.error(
"No URL or Path to camera's were given --> Not able to provide visual feedback"
)
self.top_vid_link = "http://192.168.43.1:8080/video"
self.images_per_second = images_per_second
self.no_cam_feed = no_cam_feed
self.is_running = True # Bool to be modified from main loop, that ends loop
def async_event_handler(sig: int, frame: object) -> None:
"""
A Handler function for asynchronous KeyboardInterrupt events (SIGINT).
From the Python docs:
The handler is called with two arguments:
:param sig: int The signal number
:param frame: frame object The current stack frame
:return: None
"""
logger.error("\nInterrupted program execution...")
for thread in threading.enumerate():
print("Thread running on shutdown: ", thread.name)
sys.exit(sig)
def traverse(self):
"""traversing the folder structure"""
ctx = click.get_current_context().obj
if ctx.verbose:
logger.info(f'traversing \"{self.root_path}\"...')
for root, directories, filenames in os.walk(self.root_path, topdown=True):
for filename in sorted(filenames):
filepath = os.path.join(root, filename)
if filepath.endswith(".ctl"):
ctl = CTL()
ctl_file = open(filepath, 'r')
ctl_string = ctl_file.read()
transform_id = self.extract_tag(ctl_string, "ACEStransformID")
if transform_id is not None:
ctl.transform_id = transform_id
ctl.short_transform_id = re.sub(r'^urn:ampas:aces:transformId:v[0-9].[0-9]:', '', ctl.transform_id)
ctl.description = self.extract_tag(ctl_string, "ACESuserName")
relative_path = os.path.relpath(filepath, start=self.root_path)
ctl.relative_path = relative_path
if relative_path == "idt/vendorSupplied/pomfort/IDT.RED.log3G10.ctl":
logger.info("")
# FIXME: this is still under discussion
spec_prefixes = ("ODT", "IDT", "RRT", "LMT", "RRTODT", "ACEScsc",
"InvODT", "InvIDT", "InvRRT", "InvLMT", "InvRRTODT")
# spec_prefixes = ("ODT", "IDT", "RRT", "LMT", "RRTODT", "ACEScsc")
if not ctl.short_transform_id.startswith(spec_prefixes):
ignore_prefixes = ("ACESlib", "ACESutil", "utilities")
if not ctl.short_transform_id.startswith(ignore_prefixes):
logger.error("SKIPPING: wrong prefix \"{0}\"in {1}".format(ctl.short_transform_id, filepath))
else:
self.transforms.ctls.append(ctl)
else:
if ctx.verbose is True:
logger.error("ERROR: no <ACEStransformID> found in {0}".format(filepath))
def resolve_relative_paths(self, ctl_transforms):
for transform in self.input_transforms:
relative_path = ctl_transforms.relative_path_for_transform_id(
transform.short_transform_id())
if relative_path is None and transform.applied is False:
logger.error(
"WARNING: transformId \"{0}\" not found in CTLs".format(
transform.short_transform_id()))
relative_path = "???"
transform.relative_path = relative_path
for transform in self.look_transforms:
relative_path = ctl_transforms.relative_path_for_transform_id(
transform.short_transform_id())
if relative_path is None:
logger.error(
"Cannot find a transform for lookTransform {0}!".format(
transform.short_transform_id()))
exit(amfutil_error_cannot_find_transform)
else:
transform.relative_path = relative_path
for transform in self.output_transforms:
relative_path = ctl_transforms.relative_path_for_transform_id(
transform.short_transform_id())
if relative_path is None:
logger.error(
"Cannot find a transform for outputTransform {0}!".format(
transform.short_transform_id()))
exit(amfutil_error_cannot_find_transform)
else:
transform.relative_path = relative_path
def ctls(ctx, **kwargs):
"""Parse a folder with CTL files and print mapping between transformId and filepath."""
ctx.load_args(**kwargs)
traverser = TransformsTraverser(ctx.ctl_root_path)
if ctx.relativectlpath is None:
# logger.info("Mappings:")
ctx.verbose = True # show errors while traversing
traverser.log_ctl_mappings()
else:
# logger.info("... looking for {0}".format(ctx.relativectlpath))
# traverser.log_ctl_mappings()
transformId = traverser.transforms.transform_id_for_relative_path(
ctx.relativectlpath)
if ctx.description:
if transformId is None:
if ctx.verbose is True:
logger.error(
"Couldn't find description for relative path {0}".
format(ctx.relativectlpath))
exit(203)
else:
description = traverser.transforms.description_for_relative_path(
ctx.relativectlpath)
logger.info(description)
else:
if transformId is None:
if ctx.verbose is True:
logger.error(
"Couldn't find transformId for relative path {0}".
format(ctx.relativectlpath))
exit(203)
else:
logger.info(transformId)
def retrieve_current_image(self,
needle_pos_feed: multiprocessing.Queue) -> None:
""" Retrieves and processes video feed, then sends needle pos/ori back to main Process.
Parameters
----------
needle_pos_feed : multiprocessing.Queue
Queue to pass needle position and orientation to main Process.
Raises
------
OpenCV Error
Could not connect to video stream from file
Returns
-------
None
"""
# Create Queue of size 10 that is used for image acq/proc
video_feed = Queue(maxsize=10)
top_vidcap = cv2.VideoCapture(self.top_vid_link)
process_frame = self.fps_to_images_per_second(
top_vidcap.get(cv2.CAP_PROP_FPS))
while self.is_running:
# Image Acquisition
frame_nr = top_vidcap.get(1) # Current frame of the video feed
success, frame = top_vidcap.read(
) # Retrieve frame from video feed
if success:
# If camera feed is shown (nofeed == false)
if not self.no_cam_feed:
cv2.imshow("Top Camera Feed", frame)
if cv2.waitKey(1) == 27: # Escape Key exits loop
logger.error("Exiting Video Acquisition Process.")
top_vidcap.release()
cv2.destroyAllWindows()
# Sentinel Value to end main program loop
needle_pos_feed.put((None, None))
break
# Only send a certain few frames per second to processing
if frame_nr % process_frame == 0:
video_feed.put(frame)
else:
logger.error(
"Not able to retrieve image from VideoCapture Object. Exiting Process."
)
top_vidcap.release()
cv2.destroyAllWindows()
needle_pos_feed.put(
(None, None)
) # Can't open video feed, send Sentinel value to Main Process
break
# Image Processing
if not video_feed.empty(
): # Check queue if an image can be processed
current_frame = video_feed.get()
# Retrieve needle tip and orientation and measure processing time.
start = time.time()
tip_position, tip_ori = position_from_image(current_frame,
"config.ini",
flip='yes',
filtering='yes',
show='yes')
end = time.time()
logger.success("processing took {}".format((end - start)))
if tip_position is None or tip_ori is None: # No lines detected, so don't get pos and ori
continue
# Send Needle position and orientation back to main process.
needle_pos_feed.put((tip_position, tip_ori))
def move_to_dir_syncv2(self, gdo, report=0):
"""
Receive direction (gdo) --> Drive needle with motors synchronously
1) Find A and B such that gdo.stepsout = (stepsx, stepsy) = A(motor_u) + B(motor_Z)
(linear combination of 2 motor vectors to reach gdo.stepsout vector)
2a) view the push option (corresponds with run_forward() method) ELSE do 2b)
2b) view the pull option (corresponds with run_backward() method ELSE do *)
*) return error in case both options do not work
gdo = get direction output (an object of the class Output(direction, stepsout) )
"""
# 1) if push available then find the steps for motorpush0 and motorpush1
motor_matrix_push = []
for n in self.dirpush[gdo.direction]:
motor_matrix_push.append(self.motorvec[n])
a = np.array(motor_matrix_push, dtype=int)
b = np.array([
int(gdo.stepsout[0] * self.sensitivity),
int(gdo.stepsout[1] * self.sensitivity)
],
dtype=int)
x = np.linalg.solve(a.transpose(), b)
big_a = int(abs(x[0]))
big_b = int(abs(x[1]))
if report == 1:
print("NEEDLE->move_syncV2: PUSH system of equations: \n"
"| a = " + str(a.transpose()[0]) + "\n"
"| " + str(a.transpose()[1]) + "\n"
"| b = " + str(b) + "\n"
"| [big_a, big_b] = " + str([big_a, big_b]) + "\n")
motorpush0 = self.dirpush[gdo.direction][0]
motorpush1 = self.dirpush[gdo.direction][1]
if report == 1:
print("| motorpush0 = " + str(motorpush0) + " motorpush1 = " +
str(motorpush1))
# can we push? IF yes ==> then push ELSE continue to pull
if self.motors[motorpush0].get_count() + abs(
big_a) <= 400 and self.motors[motorpush1].get_count() + abs(
big_b) <= 400:
print("NEEDLE->move_syncV2: PUSH available, starting...")
if big_b > big_a:
if big_a == 0: # removing division by zero error
big_a = 1
ratio = int(big_b / big_a)
#print(" mod ratio = ", ratio)
for i in range(big_a + big_b):
if i % (ratio + 1) == 0 and i != 0:
#print("big_a at i = ", i)
self.motors[motorpush0].run_forward(1)
else:
self.motors[motorpush1].run_forward(1)
else:
if big_b == 0:
big_b = 1
ratio = int(big_a / big_b)
#print(" mod ratio = ", ratio)
for i in range(big_a + big_b):
if i % (ratio + 1) == 0 and i != 0:
#print("big_b at i = ", i)
self.motors[motorpush1].run_forward(1)
else:
self.motors[motorpush0].run_forward(1)
# report final motor positions to user
for motor_i in range(len(self.motors)):
self.motors[motor_i].get_count(report=1)
return 0
print("\nNEEDLE->move_syncV2: PUSH not available, trying PULL")
# 2) if pull available then find the steps for motorpull0 and motorpull1
motor_matrix_pull = []
for n in self.dirpull[gdo.direction]:
motor_matrix_pull.append(self.motorvec[n])
a = np.array(motor_matrix_pull, dtype=int)
b = np.array([
int(gdo.stepsout[0] * self.sensitivity),
int(gdo.stepsout[1] * self.sensitivity)
],
dtype=int)
x = np.linalg.solve(a.transpose(), b)
big_a = int(abs(x[0]))
big_b = int(abs(x[1]))
if report == 1:
print("NEEDLE->move_syncV2: PULL system of equations: \n"
"| a = " + str(a.transpose()[0]) + "\n"
"| " + str(a.transpose()[1]) + "\n"
"| b = " + str(b) + "\n"
"| [big_a, big_b] = " + str([big_a, big_b]) + "\n")
motorpull0 = self.dirpull[gdo.direction][0]
motorpull1 = self.dirpull[gdo.direction][1]
if report == 1:
print("| motorpull0 = " + str(motorpull0) + " motorpull1 = " +
str(motorpull1))
# can we pull? IF yes ==> then pull ELSE report the movement requested is not possible
if self.motors[motorpull0].get_count() - abs(
big_a) >= 0 and self.motors[motorpull1].get_count() - abs(
big_b) >= 0:
print("NEEDLE->move_syncV2: PULL available, starting...")
if big_b > big_a:
if big_a == 0: # removing division by zero error
big_a = 1
ratio = int(big_b / big_a)
#print(" mod ratio = ", ratio)
for i in range(big_a + big_b):
if i % (ratio + 1) == 0 and i != 0:
#print("big_a at i = ", i)
self.motors[motorpull0].run_backward(1)
else:
self.motors[motorpull1].run_backward(1)
else:
if big_b == 0:
big_b = 1
ratio = int(big_a / big_b)
#print(" mod ratio = ", ratio)
for i in range(big_a + big_b):
if i % (ratio + 1) == 0 and i != 0:
#print("big_b at i = ", i)
self.motors[motorpull1].run_backward(1)
else:
self.motors[motorpull0].run_backward(1)
# report final motor positions to user
for motor_i in range(len(self.motors)):
self.motors[motor_i].get_count(report=1)
return 0
else:
logger.error(
"\n NEEDLE->move_syncV2: neither PUSH nor PULL available; no movement of needle occurred"
)
def __init__(self, comport_arduino, startsteps, sensitivity, invertx: bool,
run_test: str):
# Handle input parameters
self.port = comport_arduino
self.startcount = startsteps
self.init_pos = 0
self.sensitivity = float(sensitivity)
if self.sensitivity > 1:
self.sensitivity = 0.5
logger.info("Invalid sensitivity entered: new value = {}".format(
self.sensitivity))
self.invert_x_axis = invertx
self.test = run_test
# Setup Arduino and Stepper Motors
self.board = pyfirmata.Arduino(self.port)
time.sleep(1)
self.motors = []
self.default_motor_setup()
self.dirpull = {
0: [0, 1],
1: [0],
2: [0, 3],
3: [3],
4: [2, 3],
5: [2],
6: [1, 2],
7: [1],
}
self.dirpush = {
0: [2, 3],
1: [2],
2: [1, 2],
3: [1],
4: [0, 1],
5: [0],
6: [0, 3],
7: [3],
}
# motor vectors that code for the directions in the x-y space of the needle
self.motorvec = {
0: np.array([1, 1]),
1: np.array([-1, 1]),
2: np.array([-1, -1]),
3: np.array([1, -1]),
}
"""
FESTO section
!!! to use FESTO: first upload "FESTO_controlv3.lua" to the T7 with Kipling 3 software
then close the connection with Kipling 3 software
"""
# config
self.config_object = ConfigParser()
self.config_object.read('config.ini')
festo = self.config_object["FESTO"]
self.init_FESTO_pos = int(festo["initial_pos"])
self.init_FESTO_speed = float(festo["initial_speed"])
self.FESTO_stepsize = int(festo["step_size"])
self.AIN0addr = 0 # position (0-10V)
self.DAC0addr = 1000 # speed ref.signal (2.5V)
self.DAC1addr = 1002 # speed out signal (-2.5 - 2.5V)
self.initialpos_addr = 46000
self.targetpos_addr = 46002
self.speed_addr = 46004
self.enable_addr = 46008
self.f_datatype = ljm.constants.FLOAT32
self.i_datatype = ljm.constants.UINT16
self.offsetV = 2.5 # (offsetV+2.5V on DAC1 = 25 mm/s)
self.offV = 0.0299544557929039 # low voltage that T7 can certainly output
self.maxpos = 50 # mm
self.minpos = 3 # mm
self.currentpos = self.init_FESTO_pos
try:
FESTO_handle = ljm.openS("ANY", "USB", "ANY")
except ljm.LJMError as error:
FESTO_handle = None
logger.error(
"No FESTO_handle: thus not able to use the FESTO functions \n Error presented: "
+ str(error))
if FESTO_handle is not None:
self.FESTO_handle = FESTO_handle
# Set initial positions (keep target pos at init_FESTO_pos at the start)
ljm.eWriteAddress(self.FESTO_handle, self.initialpos_addr,
self.f_datatype, self.init_FESTO_pos)
ljm.eWriteAddress(self.FESTO_handle, self.targetpos_addr,
self.f_datatype, self.init_FESTO_pos)
# Set speed
ljm.eWriteAddress(self.FESTO_handle, self.speed_addr,
self.f_datatype, self.init_FESTO_speed)
logger.success(
"FESTO connected, handle is available, init is set, current position ="
+ str(
ljm.eReadAddress(self.FESTO_handle, self.AIN0addr,
self.f_datatype)))
time.sleep(0.3)
# Enable init LUA program
ljm.eWriteAddress(self.FESTO_handle, self.enable_addr,
self.f_datatype, 1)
logger.success("FESTO moving to initial position")
else:
logger.error(
"Something went wrong when creating a FESTO Handle. Check if all adresses are correct in needle.py"
)
self.FESTO_handle = None
