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_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_given_eight_turn_right_when_converting_absolute_then_coordinate_is_correct( self) -> None: orientation = Orientation(pi / 4) start = AbsoluteCoordinate(5, 3, orientation) stop = AbsoluteCoordinate(7, 1, orientation) transform = RobotCoordinateTransform(start) actual_coordinate = transform.from_absolute(stop) self.assertRobotCoordIsCorrect((0, sqrt(8)), actual_coordinate)
def test_given_quarter_turn_left_when_converting_absolute_then_coordinate_is_correct( self) -> None: orientation = Orientation(-pi / 2) start = AbsoluteCoordinate(5, 3, orientation) stop = AbsoluteCoordinate(7, 1, orientation) transform = RobotCoordinateTransform(start) actual_coordinate = transform.from_absolute(stop) self.assertRobotCoordIsCorrect((2, -2), actual_coordinate)
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 test_given_quarter_turn_right_from_south_when_converting_absolute_then_orientation_change_is_correct( self) -> None: orientation = Orientation(pi) start = AbsoluteCoordinate(0, 0, orientation) orientation_change = Orientation(pi / 2) stop = AbsoluteCoordinate(0, 0, orientation + orientation_change) transform = RobotCoordinateTransform(start) actual_coordinate = transform.from_absolute(stop) self.assertEqual(orientation_change, actual_coordinate.orientation_change)
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 from_robot(self, coord: RobotCoordinate) -> AbsoluteCoordinate: homogeneous_coordinates = array([coord.x, coord.y, 1]) transformed_coordinates = self._transform_matrix @ homogeneous_coordinates orientation = Orientation(self._theta + coord.orientation_change.radians) return AbsoluteCoordinate(transformed_coordinates[0], transformed_coordinates[1], orientation)
def _get_grid_alignment(self, position: AbsoluteCoordinate) -> AbsoluteCoordinate: orientation = position.orientation if orientation.radians % self.angle_precision <= self.angle_precision / 2: aligned_orientation = Orientation(orientation.radians // self.angle_precision * self.angle_precision) else: aligned_orientation = Orientation((orientation.radians // self.angle_precision + 1) * self.angle_precision) return AbsoluteCoordinate(position.x, position.y, aligned_orientation)
def test_whenPathingFromUnalignedPositionClosestToUpperAngle_thenRotationAlignsToUpperAngle( self) -> None: self.grassfire_pathable_0_0 = GrassfirePathable( SOME_TABLE_WHICH_GOAL_IS_0_0) unaligned_position = AbsoluteCoordinate(1, 3, Orientation(7 * pi / 16)) path = self.grassfire_pathable_0_0.path_from(unaligned_position) self.assertEqual(Orientation(pi / 2), path.movements[0].stop.orientation)
def _get_adjacent_cell(self, cell: AbsoluteCoordinate) -> AbsoluteCoordinate: possible_cells = [ AbsoluteCoordinate(cell.x, cell.y - 1, cell.orientation), # North AbsoluteCoordinate(cell.x, cell.y + 1, cell.orientation), # South AbsoluteCoordinate(cell.x + 1, cell.y, cell.orientation), # East AbsoluteCoordinate(cell.x - 1, cell.y, cell.orientation), # West AbsoluteCoordinate(cell.x + 1, cell.y - 1, cell.orientation), # North-east AbsoluteCoordinate(cell.x - 1, cell.y + 1, cell.orientation), # South-west AbsoluteCoordinate(cell.x + 1, cell.y + 1, cell.orientation), # South-east AbsoluteCoordinate(cell.x - 1, cell.y - 1, cell.orientation) # North-west ] for possible_cell in possible_cells: try: if self._table[possible_cell] < self._table[cell]: return possible_cell except KeyError: pass
float("inf"), 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 3.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 4.0, 4.0, 4.0, 5.0, 6.0, float("inf"), float("inf"), float("inf"), 10.0, 11.0, 5.0, 5.0, 5.0, 5.0, 6.0, float("inf"), float("inf"), float("inf"), 11.0, 11.0, 6.0, 6.0, 6.0, 6.0, 6.0, float("inf"), float("inf"), float("inf"), 10.0, 11.0, 7.0, 7.0, 7.0, 7.0, 7.0, 7.0, 8.0, 9.0, 10.0, 11.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 9.0, 10.0, 11.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 10.0, 11.0 ], Orientation(0)) POSITION_0_0 = AbsoluteCoordinate(0.0, 0.0, Orientation(0)) POSITION_2_2 = AbsoluteCoordinate(2.0, 2.0, Orientation(0)) POSITION_9_9 = AbsoluteCoordinate(9.0, 9.0, Orientation(pi / 2)) class TestPathableGrassfire(TestCase): def test_when_path_from_some_point_then_last_position_is_correct_one( self) -> None: self.grassfire_pathable_9_9 = GrassfirePathable( SOME_TABLE_WHICH_GOAL_IS_9_9) path = self.grassfire_pathable_9_9.path_from(POSITION_2_2) self.assertEqual(POSITION_9_9, path.movements[-1].stop)
def __init__(self, communication_service: CommunicationService) -> None: self._communication_service = communication_service self._position_received = Event() self._position = AbsoluteCoordinate.zero()
def set_position(self, position: str) -> None: self._position = AbsoluteCoordinate.deserialize(position) self._position_received.set()