def _grab_puck(self, puck: Puck, is_puck_in_a_corner: bool):
puck_position = puck.get_position()
if self._puck_is_close_to_center_middle(puck):
self._move_robot(
[
Movement(
Direction.BACKWARDS,
Distance(0.2, unit_of_measure=UnitOfMeasure.METER),
)
]
)
robot_pose = self._find_robot_pose()
orientation_to_puck = self._find_orientation_to_puck(puck_position, robot_pose)
self._rotation_service.rotate(orientation_to_puck)
distance_to_puck = (
Distance(0.2, unit_of_measure=UnitOfMeasure.METER)
if is_puck_in_a_corner
else Distance(0.1, unit_of_measure=UnitOfMeasure.METER)
)
movements_to_puck = [Movement(Direction.FORWARD, distance_to_puck)]
print(
movements_to_puck[0].get_direction(),
movements_to_puck[0].get_distance().get_distance(),
)
self._move_robot(movements_to_puck)
self._send_command_to_robot(
Topic.GRAB_PUCK, puck.get_color().get_resistance_digit()
)
self._wait_for_robot_confirmation(Topic.GRAB_PUCK)
def test_givenDistanceInPixels_whenCreateEquivalentDistanceInMeters_thenDistancesAreEqual(
self, ):
pixel_distance = Distance(128.6, unit_of_measure=UnitOfMeasure.PIXEL)
meter_distance = Distance(0.25, unit_of_measure=UnitOfMeasure.METER)
self.assertEqual(pixel_distance, meter_distance)
def test_givenNoUnitOfMeasureSpecified_whenCreateDistance_thenUnitOfMeasureIsPixel(
self, ):
expected_distance = Distance(200)
actual_distance = Distance(200, unit_of_measure=UnitOfMeasure.PIXEL)
self.assertEqual(expected_distance, actual_distance)
def test_givenPixelUnitOfMeasure_whenCreateDistance_thenDistanceIsConvertedToMeter(
self, ):
expected_distance_value = 2.5
actual_distance = Distance(1286, unit_of_measure=UnitOfMeasure.PIXEL)
self.assertEqual(expected_distance_value,
actual_distance.get_distance())
def calculate_horizontal_correction(
self,
image: np.ndarray,
) -> Movement:
time.sleep(1)
command_panel_letters = (self._command_panel_letter_extractor.
extract_letters_from_image(image))
if self._can_read_all_letters(command_panel_letters):
return Movement(Direction.STOP, Distance(0))
else:
return Movement(Direction.RIGHT, Distance(0.1))
def _go_forward_a_bit(self, starting_zone_index: int):
distance = Distance(0.25, unit_of_measure=UnitOfMeasure.METER)
starting_zone_corner = GameState.get_instance().get_starting_zone_corners()[
starting_zone_index
]
if (
starting_zone_corner == StartingZoneCorner.B
or starting_zone_corner == StartingZoneCorner.A
):
distance = Distance(0.22, unit_of_measure=UnitOfMeasure.METER)
movements = [Movement(Direction.FORWARD, distance)]
self._move_robot(movements)
def _go_back_a_bit(self):
movements = [
Movement(
Direction.BACKWARDS, Distance(0.2, unit_of_measure=UnitOfMeasure.METER)
)
]
self._move_robot(movements)
class TestCommandPanelAlignmentCorrector(TestCase):
AN_IMAGE = MagicMock()
ALIGNMENT_DISTANCE = Distance(0.1)
NUMBER_OF_COMMAND_PANEL_LETTERS = 9
COMMAND_PANEL_REFERENCE_POSITION = Position(20, 20)
ANY_POSITION = Position(178, 248)
A_POSITION_HORIZONTALLY_ALIGNED = Position(20, 20)
A_LEFT_POSITION = Position(10, 42)
A_RIGHT_POSITION = Position(200, 211)
A_LEFT_POSITION_WITHIN_THRESHOLD = Position(345, 332)
A_RIGHT_POSITION_WITHIN_THRESHOLD = Position(355, 332)
CONTINUOUS_COMMAND_DURATION = CommandDuration(0)
STOP_MOVEMENT = Movement(Direction.STOP, Distance(0))
NINE_LETTERS_READ = ["A", "A", "A", "A", "A", "A", "A", "A", "A"]
def setUp(self) -> None:
self.panel_detector = MagicMock()
self.command_panel_letter_extractor = MagicMock()
self.command_panel_alignment_corrector = CommandPanelAlignmentCorrector(
self.NUMBER_OF_COMMAND_PANEL_LETTERS,
self.COMMAND_PANEL_REFERENCE_POSITION,
self.panel_detector,
self.command_panel_letter_extractor,
)
def test_givenCanReadAllLettersOnCommandPanel_whenCalculateHorizontalAlignment_thenReturnStopMovement(
self, ):
self.command_panel_letter_extractor.extract_letters_from_image.return_value = (
self.NINE_LETTERS_READ)
actual_movement = (self.command_panel_alignment_corrector.
calculate_horizontal_correction(self.AN_IMAGE))
self.assertEqual(actual_movement, self.STOP_MOVEMENT)
def test_givenCommandPanelTooMuchOnTheRight_whenCalculateHorizontalAlignment_thenReturnRightMovement(
self, ):
self.panel_detector.detect_upper_left_corner.return_value = (
self.A_RIGHT_POSITION)
expected_movement = Movement(Direction.RIGHT, self.ALIGNMENT_DISTANCE)
actual_movement = (self.command_panel_alignment_corrector.
calculate_horizontal_correction(self.AN_IMAGE))
self.assertEqual(actual_movement, expected_movement)
def calculate_distance_between_two_positions(first_position: Position,
second_position: Position):
first_x, first_y = first_position.to_tuple()
second_x, second_y = second_position.to_tuple()
distance = math.sqrt((first_x - second_x)**2 + (first_y - second_y)**2)
return Distance(distance)
def _go_back_to_puck_zone(self):
movements_back_to_puck_zone = [
Movement(
Direction.BACKWARDS, Distance(0.3, unit_of_measure=UnitOfMeasure.METER)
)
]
self._move_robot(movements_back_to_puck_zone)
self._rotate_robot(CardinalOrientation.EAST.value)
self._go_to_puck_zone(rotate_west=False)
def test_whenCalculateFromDistanceAndDuration_thenReturnSpeedWithValueDistanceDividedByDuration(
self, ):
a_distance = Distance(self.A_DISTANCE_VALUE)
a_duration = CommandDuration(self.A_DURATION_VALUE)
expected_speed_value = self.A_DISTANCE_VALUE / self.A_DURATION_VALUE
actual_speed = Speed.calculate_from_distance_and_duration(
a_distance, a_duration)
self.assertEqual(expected_speed_value, actual_speed.get_speed())
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_givenLongMovement_whenCreateFromMovement_thenFirstCommandsUseMaximumRobotSpeed(
self, ):
a_long_movement = Movement(self.A_DIRECTION, Distance(1000))
movement_commands = self.movement_command_factory.create_from_movement(
a_long_movement)
first_movement, second_movement, *_ = movement_commands
self.assertEqual(self.A_ROBOT_MAXIMUM_SPEED,
first_movement.get_speed())
self.assertEqual(self.A_ROBOT_MAXIMUM_SPEED,
second_movement.get_speed())
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 _create_movement_from_straight_path(
self, path: Path, orientation: Orientation) -> Movement:
direction = None
if path[1].get_x_coordinate() - path[0].get_x_coordinate() == 1:
direction = self._calculate_movement_from_orientation(
orientation, Direction.FORWARD)
elif path[1].get_x_coordinate() - path[0].get_x_coordinate() == -1:
direction = self._calculate_movement_from_orientation(
orientation, Direction.BACKWARDS)
elif path[1].get_y_coordinate() - path[0].get_y_coordinate() == 1:
direction = self._calculate_movement_from_orientation(
orientation, Direction.RIGHT)
elif path[1].get_y_coordinate() - path[0].get_y_coordinate() == -1:
direction = self._calculate_movement_from_orientation(
orientation, Direction.LEFT)
return Movement(direction, Distance(len(path)))
def create_from_movement(self,
movement: Movement) -> List[MovementCommand]:
distance_left = movement.get_distance().get_distance()
movement_commands = list()
while distance_left > 0.001:
speed = min(
self._robot_maximum_speed,
Speed(self._servoing_constant * distance_left),
)
if speed * self._base_command_duration.get_duration(
) > distance_left:
speed = Speed.calculate_from_distance_and_duration(
Distance(distance_left), self._base_command_duration)
movement_command = MovementCommand(movement.get_direction(), speed,
self._base_command_duration)
movement_commands.append(movement_command)
distance_left -= speed * self._base_command_duration.get_duration()
movement_commands.append(self.create_stop_command())
return movement_commands
class TestStmMotorController(TestCase):
A_DIRECTION = Direction.FORWARD
A_DISTANCE = Distance(distance=10)
A_MOVEMENT = Movement(A_DIRECTION, A_DISTANCE)
A_MOVEMENT_COMMAND = MovementCommand(Direction.LEFT, Speed(0.01),
CommandDuration(0))
A_STOP_COMMAND = MovementCommand(Direction.STOP, Speed(0),
CommandDuration(0.5))
A_MOVEMENT_COMMAND_LIST = [
MovementCommand(Direction.FORWARD, Speed(10), CommandDuration(10)),
A_STOP_COMMAND,
]
def setUp(self) -> None:
self.serial_communication = MagicMock()
self.stm_motor_controller = StmMotorController(
self.serial_communication)
def test_givenMultipleMovementCommands_whenActuateWheels_thenWriteOnSerialOncePerCommand(
self, ):
commands = self.A_MOVEMENT_COMMAND_LIST
self.stm_motor_controller.actuate_wheels(commands)
self.assertEqual(
len(self.A_MOVEMENT_COMMAND_LIST),
self.serial_communication.write.call_count,
)
def test_givenSingleMovementCommand_whenActuateWheels_thenSerializeCommandBeforeWritingOnSerial(
self, ):
a_direction = Direction.LEFT
a_speed = Speed(0.147)
commands = [
MovementCommand(a_direction, a_speed, CommandDuration(100))
]
expected_serialization = b"\x03+\x87\x16>"
self.stm_motor_controller.actuate_wheels(commands)
self.serial_communication.write.assert_called_with(
expected_serialization)
def test_whenRotate_thenSerializeCommandBeforeWritingOnSerial(self):
a_direction = Direction.CLOCKWISE
an_angle = 15.4
rotation_command = RotationCommand(a_direction, an_angle)
expected_serialization = b"\x05ffvA"
self.stm_motor_controller.rotate(rotation_command)
self.serial_communication.write_and_readline.assert_called_with(
expected_serialization)
def test_whenRotate_thenWaitForRobotResponse(self):
a_direction = Direction.CLOCKWISE
an_angle = 15.4
rotation_command = RotationCommand(a_direction, an_angle)
self.stm_motor_controller.rotate(rotation_command)
self.serial_communication.write_and_readline.assert_called()
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 _move_to_the_left(self):
self._movement_service.move([Movement(Direction.LEFT, Distance(0.20))])
def calculate_from_distance_and_duration(
distance: Distance, duration: CommandDuration
) -> "Speed":
return Speed(distance.get_distance() / duration.get_duration())
class TestMovementCommandFactory(TestCase):
AN_ALIGNMENT_SPEED = Speed(0.01)
A_ROBOT_MAXIMUM_SPEED = Speed(0.25)
A_SEVOING_CONSTANT = Speed(5)
A_COMMAND_DURATION = CommandDuration(0.1)
A_DIRECTION = Direction.LEFT
A_DISTANCE = Distance(1)
AN_ANGLE = 43.1
ROTATING_SPEED = Speed(0.2)
ROBOT_RADIUS = 0.1075
SLOW_MOVEMENT_SPEED = Speed(ROBOT_ALIGNMENT_SPEED)
CONTINUOUS_MOVEMENT_DURATION = CommandDuration(0)
NULL_SPEED = Speed(0)
NULL_COMMAND_DURATION = CommandDuration(0)
def setUp(self) -> None:
self.movement_command_factory = MovementCommandFactory(
self.A_ROBOT_MAXIMUM_SPEED,
self.A_SEVOING_CONSTANT,
self.A_COMMAND_DURATION,
)
def test_whenCreateFromMovement_thenLastCommandHasStopDirection(self):
a_movement = Movement(self.A_DIRECTION, self.A_DISTANCE)
movement_commands = self.movement_command_factory.create_from_movement(
a_movement)
last_movement_command = movement_commands[-1]
self.assertEqual(Direction.STOP, last_movement_command.get_direction())
def test_givenMovementGoingForward_whenCreateFromMovement_thenAllCommandsExceptLastHaveForwardDirection(
self, ):
a_movement = Movement(Direction.FORWARD, self.A_DISTANCE)
expected_directions = {Direction.FORWARD}
movement_commands = self.movement_command_factory.create_from_movement(
a_movement)
actual_directions = {
command.get_direction()
for command in movement_commands[:-1]
}
self.assertEqual(expected_directions, actual_directions)
def test_givenMovementGoingBackwards_whenCreateFromMovement_thenAllCommandsExceptLastHaveBackwardsDirection(
self, ):
a_movement = Movement(Direction.BACKWARDS, self.A_DISTANCE)
expected_directions = {Direction.BACKWARDS}
movement_commands = self.movement_command_factory.create_from_movement(
a_movement)
actual_directions = {
command.get_direction()
for command in movement_commands[:-1]
}
self.assertEqual(expected_directions, actual_directions)
def test_givenLongMovement_whenCreateFromMovement_thenFirstCommandsUseMaximumRobotSpeed(
self, ):
a_long_movement = Movement(self.A_DIRECTION, Distance(1000))
movement_commands = self.movement_command_factory.create_from_movement(
a_long_movement)
first_movement, second_movement, *_ = movement_commands
self.assertEqual(self.A_ROBOT_MAXIMUM_SPEED,
first_movement.get_speed())
self.assertEqual(self.A_ROBOT_MAXIMUM_SPEED,
second_movement.get_speed())
def test_whenCreateFromMovement_thenLastCommandsHaveSpeedLessThanMaximumRobotSpeed(
self, ):
a_movement = Movement(self.A_DIRECTION, self.A_DISTANCE)
movement_commands = self.movement_command_factory.create_from_movement(
a_movement)
last_non_stop_command = movement_commands[-2]
self.assertLess(
last_non_stop_command.get_speed(),
self.A_ROBOT_MAXIMUM_SPEED,
)
def test_givenASetCommandDuration_whenCreateFromMovement_thenAllCommandsHaveThisDuration(
self, ):
a_movement = Movement(self.A_DIRECTION, self.A_DISTANCE)
expected_durations = {self.A_COMMAND_DURATION}
movement_commands = self.movement_command_factory.create_from_movement(
a_movement)
actual_durations = {
command.get_duration()
for command in movement_commands
}
self.assertEqual(expected_durations, actual_durations)
def test_givenNegativeAngle_whenCreateFromAngle_thenReturnCommandWithSameAngle(
self, ):
a_negative_angle = -self.AN_ANGLE
actual_command = self.movement_command_factory.create_from_angle(
a_negative_angle)
self.assertEqual(self.AN_ANGLE, actual_command.get_angle())
def test_whenCreateFromAngle_thenReturnCommandWithSameAngle(self):
actual_command = self.movement_command_factory.create_from_angle(
self.AN_ANGLE)
self.assertEqual(self.AN_ANGLE, actual_command.get_angle())
def test_givenAPositiveAngle_whenCreateFromAngle_thenReturnCommandWithCounterClockwiseDirection(
self, ):
a_positive_angle = 12.3
actual_command = self.movement_command_factory.create_from_angle(
a_positive_angle)
self.assertEqual(Direction.COUNTER_CLOCKWISE,
actual_command.get_direction())
def test_givenANegativeAngle_whenCreateFromAngle_thenReturnCommandWithClockwiseDirection(
self, ):
a_negative_angle = -12.3
actual_command = self.movement_command_factory.create_from_angle(
a_negative_angle)
self.assertEqual(Direction.CLOCKWISE, actual_command.get_direction())
def test_givenBackwardsDirection_whenCreateAlignmentMovementCommand_thenSlowContinuousBackwardsMovementCommandIsCreated(
self, ):
expected_movement_command = MovementCommand(
Direction.BACKWARDS,
self.SLOW_MOVEMENT_SPEED,
self.CONTINUOUS_MOVEMENT_DURATION,
)
actual_movement_command = (
self.movement_command_factory.create_alignment_movement_command(
Direction.BACKWARDS))
self.assertEqual(actual_movement_command, expected_movement_command)
def test_whenCreateStopMovementCommand_thenStopMovementCommandIsCreated(
self):
expected_movement_command = MovementCommand(Direction.STOP,
self.NULL_SPEED,
self.A_COMMAND_DURATION)
actual_movement_command = self.movement_command_factory.create_stop_command(
)
self.assertEqual(actual_movement_command, expected_movement_command)
def _move_to_the_right(self):
self._movement_service.move(
[Movement(Direction.RIGHT, Distance(0.10))])
STM_BAUD_RATE,
BASE_COMMAND_DURATION,
SERVOING_CONSTANT,
ROBOT_MAXIMUM_SPEED,
)
from domain.movement.CommandDuration import CommandDuration
from domain.movement.Direction import Direction
from domain.movement.Distance import Distance
from domain.movement.Movement import Movement
from domain.movement.MovementCommandFactory import MovementCommandFactory
from domain.movement.Speed import Speed
from infra.motor_controller.StmMotorController import StmMotorController
from service.movement.MovementService import MovementService
if __name__ == "__main__":
straight_movements = [Movement(Direction.FORWARD, Distance(1))]
turn_movements = [
Movement(Direction.FORWARD, Distance(0.5)),
Movement(Direction.LEFT, Distance(0.5)),
]
movement_command_factory = MovementCommandFactory(
Speed(ROBOT_MAXIMUM_SPEED),
Speed(SERVOING_CONSTANT),
CommandDuration(BASE_COMMAND_DURATION),
)
motor_controller = StmMotorController(
serial.Serial(STM_PORT_NAME, STM_BAUD_RATE))
movement_service = MovementService(movement_command_factory,
motor_controller)
def _create_movement_with_zero_distance(self):
return Movement(Direction.FORWARD, Distance(0))
