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_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 direction(self) -> Orientation:
if self.y == 0 and self.x == 0:
return Orientation(0)
elif self.y == 0:
return Orientation(pi / 2) if self.x > 0 else Orientation(-pi / 2)
orientation = Orientation(atan(self.x / self.y))
if self.y < 0:
orientation = orientation + Orientation(pi)
return orientation - self.orientation_change
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 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_absolute(self, coord: AbsoluteCoordinate) -> RobotCoordinate:
homogeneous_coordinates = array([coord.x, coord.y, 1])
transformed_coordinates = inv(
self._transform_matrix) @ homogeneous_coordinates
orientation = Orientation(self._theta + coord.orientation.radians)
return RobotCoordinate(transformed_coordinates[0],
transformed_coordinates[1], orientation)
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 from_camera(self, coord: CameraCoordinate) -> RobotCoordinate:
homogeneous_coordinate = array([coord.x, coord.y, 1])
transformed_coordinate = inv(
self._transform_matrix) @ homogeneous_coordinate
orientation = Orientation(self._theta)
return RobotCoordinate(transformed_coordinate[0],
transformed_coordinate[1], orientation)
def setUp(self) -> None:
self.north = self.front = Orientation(0)
self.northEast = self.frontRight = Orientation(pi / 4)
self.east = self.right = Orientation(pi / 2)
self.southEast = self.backRight = Orientation(3 * pi / 4)
self.south = self.back = Orientation(pi)
self.southWest = self.backLeft = Orientation(-3 * pi / 4)
self.west = self.left = Orientation(-pi / 2)
self.northWest = self.frontLeft = Orientation(-pi / 4)
def setUp(self) -> None:
rct_patcher = patch('mobility.infrastructure.directDriver.RobotCoordinateTransform')
self.RobotCoordinateTransform = rct_patcher.start()
self.addCleanup(rct_patcher.stop)
self.robot_coordinate_transform = Mock()
self.RobotCoordinateTransform.return_value = self.robot_coordinate_transform
self.robot_coordinate = Mock()
self.robot_coordinate.length = 0
self.robot_coordinate.orientation_change = Orientation(0)
self.robot_coordinate.direction = Orientation(0)
self.robot_coordinate_transform.from_absolute.return_value = self.robot_coordinate
self.drivable = Mock()
self.direct_driver = DirectDriver()
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_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 deserialize(cls, data_string: str):
data = loads(data_string)
return Table(data['height'], data['width'], data['data'],
Orientation(data['orientation']))
def direction(self) -> Orientation:
angle = atan2(self.stop.x - self.start.x, self.stop.y - self.start.y)
return Orientation(angle)
def zero(cls):
return AbsoluteCoordinate(0, 0, Orientation(0))
float("inf"),
float("inf"), 6.0, 6.0, 6.0,
float("inf"),
float("inf"), 9.0, 9.0,
float("inf"),
float("inf"),
float("inf"), 5.0, 5.0, 5.0,
float("inf"),
float("inf"), 9.0, 8.0,
float("inf"),
float("inf"),
float("inf"), 4.0, 4.0, 4.0, 4.0, 4.0, 9.0, 8.0, 7.0, 6.0, 5.0, 4.0, 3.0,
3.0, 3.0, 3.0, 9.0, 8.0, 7.0, 6.0, 5.0, 4.0, 3.0, 2.0, 2.0, 2.0, 9.0, 8.0,
7.0, 6.0, 5.0, 4.0, 3.0, 2.0, 1.0, 1.0, 9.0, 8.0, 7.0, 6.0, 5.0, 4.0, 3.0,
2.0, 1.0, 0
], Orientation(pi / 2))
SOME_TABLE_WHICH_GOAL_IS_0_0 = Table(10, 10, [
0,
float("inf"),
float("inf"),
float("inf"), 8.0, 8.0, 8.0, 9.0, 10.0, 11.0, 1.0,
float("inf"),
float("inf"),
float("inf"), 7.0, 7.0, 8.0, 9.0, 10.0, 11.0, 2.0,
float("inf"),
float("inf"),
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"),
def deserialize(cls, data_string: str):
data = loads(data_string)
return AbsoluteCoordinate(data['x'], data['y'],
Orientation(data['orientation']))
