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 _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 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 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 _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))])
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))