def find_movements_to_goal(self, robot_pose: RobotPose, goal_position):
if FIND_MOVEMENTS_FOR_REAL:
path = self._path_service.find_path(
robot_pose.get_position() + Position(140, 0),
goal_position,
)
return MovementFactory().create_movements(
path, robot_pose.get_orientation_in_degree())
else:
# TODO return hardcoded movements
pass
def _create_stage_handler_selector(self):
self._movement_factory = MovementFactory()
start_game_cycle_handler = self._create_start_game_cycle_handler()
go_to_ohmmeter_handler = self._create_go_to_ohmmeter_handler()
find_command_panel_handler = self._create_find_command_panel_handler()
transport_puck_handler = self._create_transport_puck_handler()
stop_handler = self._create_stop_handler()
return StageHandlerSelector(
start_game_cycle_handler,
go_to_ohmmeter_handler,
find_command_panel_handler,
transport_puck_handler,
stop_handler,
)
def setUp(self) -> None:
self.movement_factory = MovementFactory()
def find_movements_to_puck_zone(self, robot_pose: RobotPose):
path = self._path_service.find_path_to_puck_zone_center(
robot_pose.get_position())
return MovementFactory().create_movements(
path, robot_pose.get_orientation_in_degree())
def run(self):
print("Finding puck position")
current_puck_position = self.find_puck_position(PUCK_COLOR_TO_USE)
input("Put robot in the middle of the puck zone")
print("Finding robot position")
puck_zone_robot_pose = self.find_robot_pose()
print("Finding angle between puck and robot")
orientation_to_puck = self.find_orientation_to_puck(
current_puck_position, puck_zone_robot_pose
)
print("Rotating to face puck")
self._rotation_service.rotate(orientation_to_puck)
print("Calculating movement needed to get to puck")
movements_to_puck = [
MovementFactory().create_movement_to_get_to_point_with_direction(
puck_zone_robot_pose.get_gripper_position(),
current_puck_position,
Direction.FORWARD,
)
]
print("Sending movements to robot")
self.send_command_to_robot(Topic.MOVEMENTS, movements_to_puck)
print("Waiting for robot to move")
self.wait_for_robot_confirmation(Topic.MOVEMENTS)
print("Send command to grab puck to robot")
self.send_command_to_robot(
Topic.GRAB_PUCK, PUCK_COLOR_TO_USE.get_resistance_digit()
)
print("Waiting for robot to grab puck")
self.wait_for_robot_confirmation(Topic.GRAB_PUCK)
print("Finding robot position")
robot_with_puck_pose = self.find_robot_pose()
print("Calculating movement needed to go back to puck zone center")
movements_back_to_puck_zone = [
MovementFactory().create_movement_to_get_to_point_with_direction(
robot_with_puck_pose.get_gripper_position(),
current_puck_position,
Direction.BACKWARDS,
)
]
print("Sending movements to robot")
self.send_command_to_robot(Topic.MOVEMENTS, movements_back_to_puck_zone)
print("Waiting for robot to move")
self.wait_for_robot_confirmation(Topic.MOVEMENTS)
print("Rotating robot straight")
self._rotation_service.rotate(CardinalOrientation.EAST.value)
server = ApplicationServer(MagicMock(), MagicMock(),
context._vision_worker, MagicMock())
server.run()
input("Ready to start, press enter to continue")
game_table = context._vision_service.create_game_table()
context._path_service.set_game_table(game_table)
puck_position = Position(1400, 600)
path = (context._shortest_path_algorithm.
find_shortest_path_with_cartesian_coordinates(
GameState.get_instance().get_robot_pose().get_position(),
puck_position))
movement_factory = MovementFactory()
robot_orientation = (
GameState.get_instance().get_robot_pose().get_orientation_in_degree())
movements = movement_factory.create_movements(path, robot_orientation)
maze_array = context._vision_service.create_game_table().get_maze()
table_image = context._world_camera.take_world_image()
for i, column in enumerate(maze_array):
for j, row in enumerate(column):
if maze_array[i][j] == 1:
table_image[i][j] = [255, 0, 255]
for element in path:
table_image[element.get_y_coordinate()][element.get_x_coordinate()] = [
current_image, puck_color))
if vertical_movement_command.get_direction() == Direction.STOP:
self._movement_service.execute_movement_command(
vertical_movement_command)
break
if __name__ == "__main__":
image_center = Position(320, 340)
camera = FakeCamera(0)
vision_service = FakeVisionService(camera)
puck_detector = OpenCvPuckDetector()
serial = serial.Serial(port=STM_PORT_NAME, baudrate=STM_BAUD_RATE)
stm_motor_controller = StmMotorController(serial)
movement_service = MovementService(
MovementFactory(),
MovementCommandFactory(ROBOT_MAXIMUM_SPEED, SERVOING_CONSTANT,
BASE_COMMAND_DURATION),
stm_motor_controller,
)
puck_alignment_corrector = PuckAlignmentCorrector(image_center, 10,
puck_detector)
vertical_alignment_corrector = VerticalAlignmentCorrector(
movement_service, vision_service, puck_alignment_corrector)
puck_color_to_align = Color.RED
vertical_alignment_corrector.correct_vertical_alignment(
puck_color_to_align)