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)