def handle_hotkeys(keycode):
"""
Handle the keyboard key pressed event
:param keycode: keycode of the pressed key
:type keycode: pynput.keyboard.KeyCode
"""
if keycode == keyboard.KeyCode(char='s') or keycode == keyboard.KeyCode(char='S'):
preview_obj = InstanceManager.get_instance("PreviewUtils")
preview_obj.save_cam_screenshot()
def __init__(self):
self.robot = InstanceManager.get_instance("Robot")
self.drive_controller = InstanceManager.get_instance("DriveController")
self.preview_utils = InstanceManager.get_instance("PreviewUtils")
self.corr_calculator = InstanceManager.get_instance(
"CorrectionCalculator")
self.sign_handler = InstanceManager.get_instance("SignHandler")
self.navigator = InstanceManager.get_instance("Navigator")
self.last_timestamp = datetime.datetime.now()
def analyze_frame(image):
"""
Analyzes a frame to check it it contains a crossing. A crossing type needs to stay unchanged for 2 frames
for it to be confirmed as valid and returned.
:param image: The image as captured by Cozmos camera
:return: The last confirmed crossing type
"""
correction_calculator_obj = InstanceManager.get_instance(
"CorrectionCalculator")
# If lane correction is too much the crossing may be invalid and should be discarded
correction_points = correction_calculator_obj.last_points
if correction_points is None or correction_points[0] is None or \
correction_points[0][0] < Settings.crossing_correction_min_dist_to_edge or \
correction_points[0][0] > image.shape[1] - Settings.crossing_correction_min_dist_to_edge:
CrossingTypeIdentifier.last_confirmed_crossing_type = None
return CrossingTypeIdentifier.last_confirmed_crossing_type
# Crop out relevant area
image = ImagePreprocessor.crop_image(image, Settings.crossing_top_crop,
Settings.crossing_right_crop,
Settings.crossing_bottom_crop,
Settings.crossing_left_crop)
# Obtain pixel rows from shape
row_patterns = CrossingTypeIdentifier.create_row_patterns(image)
# Filter out invalid patterns
row_patterns = CrossingTypeIdentifier.filter_invalid_row_pattern(
row_patterns)
# Set last crossing type for preview window
crossing_type = CrossingTypeIdentifier.row_patterns_to_crossing_type(
row_patterns)
# Confirm crossing type if at least on two frames
if crossing_type == CrossingTypeIdentifier.last_crossing_type:
CrossingTypeIdentifier.last_confirmed_crossing_type = crossing_type
else:
CrossingTypeIdentifier.last_confirmed_crossing_type = None
CrossingTypeIdentifier.last_crossing_type = crossing_type
return CrossingTypeIdentifier.last_confirmed_crossing_type
def navigate():
"""
Navigates the robot from start to endpoint
"""
drive_controller = InstanceManager.get_instance("DriveController")
# If track is none it has not been loaded yet
if Navigator.current_track is None:
raise Exception("Route not set")
# Execute next turn
if Navigator.current_track[Navigator.route_turn_index] == "L":
drive_controller.crossing_turn_left()
elif Navigator.current_track[Navigator.route_turn_index] == "R":
drive_controller.crossing_turn_right()
elif Navigator.current_track[Navigator.route_turn_index] == "S":
drive_controller.crossing_go_straight()
else:
raise Exception("Invalid turn operation")
# Update route turn index
Navigator.route_turn_index += 1
def __init__(self):
self.robot = InstanceManager.get_instance("Robot")
def __init__(self):
self.robot = InstanceManager.get_instance("Robot")
self.correction_calculator = InstanceManager.get_instance("CorrectionCalculator")
self.drive_controller = InstanceManager.get_instance("DriveController")
self.behavior_controller = InstanceManager.get_instance("BehaviorController")
def run(robot_obj: cozmo.robot.Robot):
"""
Main script for the lane following algorithm. Starts all necessary components.
:param robot_obj: Reference to the robot
:type robot_obj: cozmo.robot.Robot
"""
global last_frame
# Create necessary instances and add them to instance manager
InstanceManager.add_instance("Robot", robot_obj)
corr_calculator_obj = CorrectionCalculator()
InstanceManager.add_instance("CorrectionCalculator", corr_calculator_obj)
preview_obj = PreviewUtils()
InstanceManager.add_instance("PreviewUtils", preview_obj)
drive_obj = DriveController()
InstanceManager.add_instance("DriveController", drive_obj)
behavior_obj = BehaviorController()
InstanceManager.add_instance("BehaviorController", behavior_obj)
trackloader_obj = TrackLoader()
InstanceManager.add_instance("TrackLoader", trackloader_obj)
navigator_obj = Navigator()
InstanceManager.add_instance("Navigator", navigator_obj)
sign_handler_obj = SignHandler()
InstanceManager.add_instance("SignHandler", sign_handler_obj)
core_engine_obj = CoreEngine()
InstanceManager.add_instance("CoreEngine", core_engine_obj)
# Setup robot with presets
robot_obj.enable_stop_on_cliff(False)
robot_obj.camera.image_stream_enabled = True
robot_obj.camera.color_image_enabled = False
robot_obj.set_head_light(False)
behavior_obj.move_head_down()
behavior_obj.move_lift_up()
robot_obj.wait_for_all_actions_completed()
# Setup camera event handler
# noinspection PyTypeChecker
robot_obj.add_event_handler(cozmo.camera.EvtNewRawCameraImage, save_last_frame)
# Start driving engine
drive_obj.start()
s = sched.scheduler(time.time, time.sleep)
# runs the frame analysis in the specified frequency
def run_analysis(sc):
if last_frame is not None:
TimingUtils.run_all_elapsed()
core_engine_obj.process_frame(image=last_frame)
s.enter(0.05, 1, run_analysis, (sc,))
s.enter(0.05, 1, run_analysis, (s,))
s.run()
# Setup hotkey listener
with keyboard.Listener(on_press=handle_hotkeys) as listener:
listener.join()
def __init__(self):
self.robot = InstanceManager.get_instance("Robot")
self.drive_controller = InstanceManager.get_instance("DriveController")
def __init__(self):
self.correction_calculator_obj = InstanceManager.get_instance("CorrectionCalculator")
self.robot_obj = InstanceManager.get_instance("Robot")