def test_given_start_of_position_correction_for_translation_change_translation_status_and_send_translation_vector_towards_current_target_node( ): position = (40, 50) robot_status = RobotStatus((30, 30), 90) robot_status.target_position = (60, 65) wheels_controller = SimulatedWheelsController() servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger()) servo_wheels_manager.translating_start_position_correction( position, robot_status, wheels_controller) vector_x, vector_y = robot_status.get_translation_vector() assert int(vector_x) == 20 assert int(vector_y) == 15 assert servo_wheels_manager.translation_status == TranslationStatus.CORRECTING_POSITION position = (50.1, 54.9) robot_status = RobotStatus((45, 50), 90) robot_status.target_position = (50, 55) pathfinder = Pathfinder(ExecutionLogger()) pathfinder.robot_status = robot_status pathfinder.nodes_queue_to_checkpoint.append((100, 150)) servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger())
def test_generate_new_vector_returns_correct_vector(): pathfinder = Pathfinder(ExecutionLogger()) pathfinder.robot_status = RobotStatus((20, 20), 90) pathfinder.robot_status.target_position = (20, 20) real_robot_position = (30, 30) assert (pathfinder.robot_status. generate_new_translation_vector_towards_current_target( real_robot_position) == (-10, -10))
def test_if_not_close_enough_to_next_node_carry_on_to_next_node_without_changing_anything( ): pathfinder = Pathfinder(ExecutionLogger()) pathfinder.robot_status = RobotStatus((20, 20), 90) pathfinder.robot_status.target_position = (30, 30) pathfinder.nodes_queue_to_checkpoint.append((20, 40)) path_status, new_vector = pathfinder.get_vector_to_next_node((25, 25)) assert path_status == PathStatus.MOVING_TOWARDS_TARGET assert new_vector is None assert pathfinder.robot_status.target_position == (30, 30)
def test_if_close_enough_to_next_node_and_it_is_checkpoint_return_checkpoint_reached_status( ): pathfinder = Pathfinder(ExecutionLogger()) pathfinder.robot_status = RobotStatus((20, 20), 90) pathfinder.robot_status.target_position = (30, 30) pathfinder.nodes_queue_to_checkpoint.clear() path_status, new_vector = pathfinder.get_vector_to_next_node( (29.91, 29.91)) assert path_status == PathStatus.CHECKPOINT_REACHED assert new_vector is None assert pathfinder.robot_status.target_position == (29.91, 29.91)
def test_if_close_enough_to_next_node_switch_to_new_vector_and_return_moving_to_checkpoint_status( ): pathfinder = Pathfinder(ExecutionLogger()) pathfinder.robot_status = RobotStatus((20, 20), 90) pathfinder.robot_status.target_position = (30, 30) pathfinder.nodes_queue_to_checkpoint.append((20, 40)) path_status, new_vector = pathfinder.get_vector_to_next_node( (29.91, 29.91)) assert path_status == PathStatus.INTERMEDIATE_NODE_REACHED assert new_vector == (-9.91, 10.09) assert pathfinder.robot_status.target_position == (20, 40)
def test_given_at_checkpoint_when_finishing_translation_then_translation_status_is_updated_and_no_new_vector_is_generated( ): position = (50.1, 54.9) robot_status = RobotStatus((45, 50), 90) robot_status.target_position = (50, 55) pathfinder = Pathfinder(ExecutionLogger()) pathfinder.robot_status = robot_status servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger()) path_status, new_vector = servo_wheels_manager.finish_translation_and_get_new_path_status_and_vector( position, pathfinder) assert servo_wheels_manager.translation_status == TranslationStatus.MOVING assert new_vector is None assert path_status == PathStatus.CHECKPOINT_REACHED
def __init__(self, telemetry, interfacing_controller, logger, onboard_vision, movement_strategies, translation_lock, rotation_lock): self.logger = logger self.pathfinder = Pathfinder(logger) self.antenna_information = AntennaInformation() self.current_status = Step.STANBY self.base_station = telemetry self.servo_wheels_manager = ServoWheelsManager(translation_lock, rotation_lock, logger) self.capture_repositioning_manager = CaptureRepositioningManager() self.interfacing_controller = interfacing_controller self.dispatcher = CommandDispatcher( movement_strategies, self.interfacing_controller, self.pathfinder, logger, onboard_vision, self.antenna_information, self.servo_wheels_manager, self.capture_repositioning_manager)
def test_given_at_intermediary_node_when_finishing_translation_then_translation_status_is_updated_and_new_vector_towards_next_node_is_generated( ): position = (50.1, 54.9) robot_status = RobotStatus((45, 50), 90) robot_status.target_position = (50, 55) pathfinder = Pathfinder(ExecutionLogger()) pathfinder.robot_status = robot_status pathfinder.nodes_queue_to_checkpoint.append((100, 150)) servo_wheels_manager = ServoWheelsManager(None, None, ExecutionLogger()) path_status, new_vector = servo_wheels_manager.finish_translation_and_get_new_path_status_and_vector( position, pathfinder) new_vector_x, new_vector_y = new_vector assert servo_wheels_manager.translation_status == TranslationStatus.MOVING assert round(new_vector_x, 1) == 49.9 assert round(new_vector_y, 1) == 95.1 assert path_status == PathStatus.INTERMEDIATE_NODE_REACHED
class Brain(): def __init__(self, telemetry, interfacing_controller, logger, onboard_vision, movement_strategies, translation_lock, rotation_lock): self.logger = logger self.pathfinder = Pathfinder(logger) self.antenna_information = AntennaInformation() self.current_status = Step.STANBY self.base_station = telemetry self.servo_wheels_manager = ServoWheelsManager(translation_lock, rotation_lock, logger) self.capture_repositioning_manager = CaptureRepositioningManager() self.interfacing_controller = interfacing_controller self.dispatcher = CommandDispatcher( movement_strategies, self.interfacing_controller, self.pathfinder, logger, onboard_vision, self.antenna_information, self.servo_wheels_manager, self.capture_repositioning_manager) def main(self): main_sequence_has_started = False ready_packet = Packet( PacketType.NOTIFICATION, "STANDBY - Robot ready to roll! Cycle start when GAME_MAP is recieved." ) self.base_station.put_command(ready_packet) while True: telemetry_recieved = self.base_station.fetch_command() if telemetry_recieved: if telemetry_recieved.packet_type == PacketType.GAME_MAP: cycle_start_notification = Packet(PacketType.COMMAND, "START_CHRONOGRAPH") self.base_station.put_command(cycle_start_notification) main_sequence_has_started = True if main_sequence_has_started: telemetry_data = None if telemetry_recieved: telemetry_data = telemetry_recieved.packet_data command = self.dispatcher.get_relevant_command( self.current_status) next_status, exit_telemetry = command.execute(telemetry_data) self.current_status = next_status if exit_telemetry: if isinstance(exit_telemetry, list): for exit_packet in exit_telemetry: self.base_station.put_command(exit_packet) elif isinstance(exit_telemetry, Packet): self.base_station.put_command(exit_telemetry) if self.current_status == Step.STANBY: self.base_station.put_command(ready_packet) main_sequence_has_started = False self.reinitialize_for_next_cycle() def reinitialize_for_next_cycle(self): self.logger.log("Reinitializing for next cycle") self.pathfinder.reinitialize() self.servo_wheels_manager.reinitialize() self.interfacing_controller.antenna.reinitialize() self.interfacing_controller.wheels.reinitialize() self.capture_repositioning_manager.reinitialize() self.antenna_information = AntennaInformation()
print("Initializing potential field") graph.initialize_graph_matrix((0, 0), (111, 230), obstacle_list) hsv_img = np.zeros((graph.matrix_width, graph.matrix_height, 3), np.uint8) hsv_img[:, :] = (255, 255, 255) for i in range(graph.matrix_width): for j in range(graph.matrix_height): if graph.matrix[i][j] == math.inf: hsv_img[i, j] = (0, 0, 0) else: hsv_img[i, j] = (120 - (120 / MAXIMUM_GRID_NODE_HEIGHT) * graph.matrix[i][j], 255, 255) img = cv2.cvtColor(hsv_img, cv2.COLOR_HSV2BGR) pathfinder = Pathfinder(None) robotStatus = RobotStatus(starting_point, 90) pathfinder.graph = graph pathfinder.robot_status = robotStatus print("Calculating path") try: pathfinder.generate_path_to_checkpoint(destination) for i in range(len(pathfinder.nodes_queue_to_checkpoint)): x, y = pathfinder.graph.get_grid_element_index_from_position(pathfinder.nodes_queue_to_checkpoint[i]) cv2.rectangle(img, (y, x), (y + 1, x + 1), (114, 37, 116), 1) if i < len(pathfinder.nodes_queue_to_checkpoint) - 1: x2, y2 = pathfinder.graph.get_grid_element_index_from_position( pathfinder.nodes_queue_to_checkpoint[i + 1]) cv2.line(img, (y, x), (y2, x2), (114, 37, 116), 1) except CheckpointNotAccessibleError: