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()