def _drive_absolute(self, movement: AbsoluteMovement,
drivable: IDrivable) -> None:
position = self._position_service.get_position()
next_movement = AbsoluteMovement(movement.start, movement.stop)
while next_movement.length > self.close_position or \
not self._orientation_within_limits(position.orientation, movement.stop.orientation):
self._direct_driver.drive(next_movement, drivable)
position = self._position_service.get_position()
next_movement = AbsoluteMovement(position, movement.stop)
def test_whenPathingFromSpecificPosition_thenPathIsRight(self) -> None:
self.grassfire_pathable_0_0 = GrassfirePathable(
SOME_TABLE_WHICH_GOAL_IS_0_0)
unaligned_position = AbsoluteCoordinate(1, 3,
Orientation(15 * pi / 16))
path = self.grassfire_pathable_0_0.path_from(unaligned_position)
expected_path = AbsolutePath([
AbsoluteMovement(unaligned_position,
AbsoluteCoordinate(0, 2, Orientation(pi))),
AbsoluteMovement(AbsoluteCoordinate(0, 2, Orientation(pi)),
AbsoluteCoordinate(0, 0, Orientation(0)))
])
self.assertEqual(expected_path, path)
def test_givenMovement_whenCalculating_thenStopIsUsedToCalculateRobotCoordinate(self) -> None:
start = AbsoluteCoordinate(0, 0, Orientation(0))
stop = AbsoluteCoordinate(0, 0, Orientation(0))
movement = AbsoluteMovement(start, stop)
self.direct_driver.drive(movement, self.drivable)
self.robot_coordinate_transform.from_absolute.assert_called_with(stop)
def test_givenMovement_whenCalculating_thenStartIsUsedToInstantiateRCT(self) -> None:
start = AbsoluteCoordinate(0, 0, Orientation(0))
stop = AbsoluteCoordinate(0, 0, Orientation(0))
movement = AbsoluteMovement(start, stop)
self.direct_driver.drive(movement, self.drivable)
self.RobotCoordinateTransform.assert_called_with(start)
def test_givenNonZeroDistanceMovement_whenCalculating_thenTranslateOperationReturned(self) -> None:
start = AbsoluteCoordinate(0, 0, Orientation(0))
stop = AbsoluteCoordinate(0, 0, Orientation(0))
movement = AbsoluteMovement(start, stop)
self.robot_coordinate.length = 10
self.direct_driver.drive(movement, self.drivable)
expected_calls = [call.translate(Angle(0), Distance(10))]
actual_calls = self.drivable.method_calls
self.assertListEqual(expected_calls, actual_calls)
def path_from(self, position: AbsoluteCoordinate) -> AbsolutePath:
previous_position = position
current_position = self._get_grid_alignment(position)
previous_movement = AbsoluteMovement(previous_position, current_position)
# path_list = [previous_movement]
path_list = []
while self._table[current_position] != 0:
current_position = self._get_adjacent_cell(current_position)
current_movement = AbsoluteMovement(previous_position, current_position)
if self._movement_follows_previous(current_movement, previous_movement) \
and previous_movement.length < self.max_movement_length:
previous_movement.stop = current_movement.stop
else:
path_list.append(current_movement)
previous_movement = current_movement
previous_position = current_position
if len(path_list) == 0:
path_list.append(previous_movement)
path_list[-1].stop.orientation = self._table.target_orientation
return AbsolutePath(path_list)