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()
class TestMovementFactory(TestCase):
A_POSITION = MagicMock()
ANOTHER_POSITION = MagicMock()
A_DIRECTION = Direction.FORWARD
A_DISTANCE = Distance(10)
AN_ORIENTATION = Orientation(90)
def setUp(self) -> None:
self.movement_factory = MovementFactory()
def test_givenSingleLinePath_whenCreateMovements_thenSingleMovementIsCreated(
self):
a_path = Path(
[Position(1, 0),
Position(1, 1),
Position(1, 2),
Position(1, 4)])
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
self.assertEqual(1, len(movements))
def test_givenPathWithNinetyDegreeTurn_whenCreateMovements_thenTwoMovementsAreCreated(
self, ):
a_path = Path([
Position(3, 0),
Position(3, 1),
Position(3, 2),
Position(3, 3),
Position(2, 3),
Position(1, 3),
Position(0, 3),
])
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
self.assertEqual(2, len(movements))
def test_givenPathWithNinetyDegreeTurnAndGoingBackwards_whenCreateMovements_thenTwoMovementsAreCreated(
self, ):
a_path = Path([
Position(1, 3),
Position(1, 2),
Position(1, 1),
Position(1, 0),
Position(2, 0),
Position(3, 0),
Position(4, 0),
])
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
self.assertEqual(2, len(movements))
def test_givenStraightPathWithFivePoints_whenCreateMovements_thenASingleMovementWithDistanceFiveIsCreated(
self, ):
a_path = Path([
Position(1, 0),
Position(1, 1),
Position(1, 2),
Position(1, 4),
Position(1, 5),
])
expected_distance = Distance(distance=5)
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
movement_distance = movements[0].get_distance()
self.assertEqual(expected_distance, movement_distance)
def test_givenPathWithTurn_whenCreateMovements_thenTwoMovementsWithRightDistancesAreCreated(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
Position(4, 2),
Position(4, 3),
Position(4, 4),
])
expected_first_distance = Distance(distance=4)
expected_second_distance = Distance(distance=3)
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
first_movement_distance = movements[0].get_distance()
second_movement_distance = movements[1].get_distance()
self.assertEqual(expected_first_distance, first_movement_distance)
self.assertEqual(expected_second_distance, second_movement_distance)
def test_givenStraightPathWithXIncreasing_whenCreateMovementsWithWestOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
])
forward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)[0]
self.assertEqual(Direction.FORWARD, forward_movement.get_direction())
def test_givenStraightPathWithXIncreasing_whenCreateMovementsWithSouthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
])
right_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.SOUTH.value)[0]
self.assertEqual(Direction.RIGHT, right_movement.get_direction())
def test_givenStraightPathWithXIncreasing_whenCreateMovementsWithNorthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
])
left_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.NORTH.value)[0]
self.assertEqual(Direction.LEFT, left_movement.get_direction())
def test_givenStraightPathWithXIncreasing_whenCreateMovementsWithEastOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
])
backward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.EAST.value)[0]
self.assertEqual(Direction.BACKWARDS,
backward_movement.get_direction())
def test_givenStraightPathWithXDecreasing_whenCreateMovementsWithWestOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(4, 1),
Position(3, 1),
Position(2, 1),
Position(1, 1),
])
backward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)[0]
self.assertEqual(Direction.BACKWARDS,
backward_movement.get_direction())
def test_givenStraightPathWithXDecreasing_whenCreateMovementsWithSouthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(4, 1),
Position(3, 1),
Position(2, 1),
Position(1, 1),
])
left_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.SOUTH.value)[0]
self.assertEqual(Direction.LEFT, left_movement.get_direction())
def test_givenStraightPathWithXDecreasing_whenCreateMovementsWithNorthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(4, 1),
Position(3, 1),
Position(2, 1),
Position(1, 1),
])
right_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.NORTH.value)[0]
self.assertEqual(Direction.RIGHT, right_movement.get_direction())
def test_givenStraightPathWithXDecreasing_whenCreateMovementsWithEastOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(4, 1),
Position(3, 1),
Position(2, 1),
Position(1, 1),
])
forward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.EAST.value)[0]
self.assertEqual(Direction.FORWARD, forward_movement.get_direction())
def test_givenStraightPathWithYIncreasing_whenCreateMovementsWithWestOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(1, 2),
Position(1, 3),
Position(1, 4),
])
right_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)[0]
self.assertEqual(Direction.RIGHT, right_movement.get_direction())
def test_givenStraightPathWithYIncreasing_whenCreateMovementsWithSouthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(1, 2),
Position(1, 3),
Position(1, 4),
])
backward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.SOUTH.value)[0]
self.assertEqual(Direction.BACKWARDS,
backward_movement.get_direction())
def test_givenStraightPathWithYIncreasing_whenCreateMovementsWithNorthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(1, 2),
Position(1, 3),
Position(1, 4),
])
forward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.NORTH.value)[0]
self.assertEqual(Direction.FORWARD, forward_movement.get_direction())
def test_givenStraightPathWithYIncreasing_whenCreateMovementsWithEastOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(1, 2),
Position(1, 3),
Position(1, 4),
])
left_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.EAST.value)[0]
self.assertEqual(Direction.LEFT, left_movement.get_direction())
def test_givenStraightPathWithYDecreasing_whenCreateMovementsWithWestOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
left_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)[0]
self.assertEqual(Direction.LEFT, left_movement.get_direction())
def test_givenStraightPathWithYDecreasing_whenCreateMovementsWithSouthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
forward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.SOUTH.value)[0]
self.assertEqual(Direction.FORWARD, forward_movement.get_direction())
def test_givenStraightPathWithYDecreasing_whenCreateMovementsWithNorthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
backward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.NORTH.value)[0]
self.assertEqual(Direction.BACKWARDS,
backward_movement.get_direction())
def test_givenStraightPathWithYDecreasing_whenCreateMovementsWithEastOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
right_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.EAST.value)[0]
self.assertEqual(Direction.RIGHT, right_movement.get_direction())
def test_givenOrientationCloseToCardinalOrientationSouth_whenCreateMovements_thenReturnMovementWithRightDirection(
self, ):
a_close_orientation = Orientation(91)
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
expected_direction = Direction.FORWARD
movement = self.movement_factory.create_movements(
a_path, a_close_orientation)[0]
self.assertEqual(movement.get_direction(), expected_direction)
def test_givenOrientationFarFromCardinalOrientation_whenCreateMovements_thenRaiseInvalidOrientationException(
self, ):
a_far_orientation = Orientation(188)
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
creating_movements = lambda: self.movement_factory.create_movements(
a_path, a_far_orientation)
with self.assertRaises(InvalidOrientationException):
creating_movements()
@patch(
"infra.utils.GeometryUtils.GeometryUtils.calculate_distance_between_two_positions"
)
def test_givenTwoPositions_whenCreateMovementToGetToPointWithDirection_thenCalculateDistanceBetweenTwoPoints(
self, geometryUtils_mock):
self.movement_factory.create_movement_to_get_to_point_with_direction(
self.A_POSITION, self.ANOTHER_POSITION, self.A_DIRECTION)
geometryUtils_mock.assert_called_with(self.A_POSITION,
self.ANOTHER_POSITION)
@patch(
"infra.utils.GeometryUtils.GeometryUtils.calculate_distance_between_two_positions"
)
def test_givenCalculatedDistance_whenCreateMovementToGetToPointWithDirection_returnMovementWithDirectionAndDistance(
self, geometryUtils_mock):
geometryUtils_mock.return_value = self.A_DISTANCE
expected_movement = Movement(self.A_DIRECTION, self.A_DISTANCE)
actual_movement = (self.movement_factory.
create_movement_to_get_to_point_with_direction(
self.A_POSITION, self.ANOTHER_POSITION,
self.A_DIRECTION))
self.assertEqual(expected_movement, actual_movement)
def test_givenPathOfOnePosition_whenCreateMovement_thenReturnSingleMovementPath(
self, ):
a_one_position_path = Path([Position(0, 0)])
movements = self.movement_factory.create_movements(
a_one_position_path, self.AN_ORIENTATION)
self.assertEqual(len(movements), 1)
def test_givenExperimentallyFoundBreakingPath_whenCreateMovement_thenDoNotThrow(
self, ):
pass
a_breaking_path = Path([
Position(148, 238),
Position(149, 238),
Position(150, 238),
Position(151, 238),
Position(152, 238),
Position(153, 238),
Position(154, 238),
Position(155, 238),
Position(156, 238),
Position(157, 238),
Position(158, 238),
Position(159, 238),
Position(160, 238),
Position(161, 238),
Position(161, 239),
])
an_orientation = CardinalOrientation.EAST.value
try:
movements = self.movement_factory.create_movements(
a_breaking_path, an_orientation)
except Exception as e:
self.fail(
f"MovementFactory.create_movements threw exception '{e}'")
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)