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 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 create_movement_to_get_to_point_with_direction(self, first_position,
second_position,
direction):
distance = GeometryUtils.calculate_distance_between_two_positions(
first_position, second_position)
return Movement(direction, distance)
def _go_back_a_bit(self):
movements = [
Movement(
Direction.BACKWARDS, Distance(0.2, unit_of_measure=UnitOfMeasure.METER)
)
]
self._move_robot(movements)
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 _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_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 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_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 _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 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_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 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
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)))
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 _create_movement_with_zero_distance(self):
return Movement(Direction.FORWARD, Distance(0))
def _move_to_the_left(self):
self._movement_service.move([Movement(Direction.LEFT, Distance(0.20))])
def _move_to_the_right(self):
self._movement_service.move(
[Movement(Direction.RIGHT, Distance(0.10))])
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()
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)