def test_givenTwoDifferentOrientation_whenEqual_thenReturnFalse(self):
an_orientation = Orientation(self.AN_ORIENTATION_IN_DEGREE)
another_orientation = Orientation(self.ANOTHER_ORIENTATION_IN_DEGREE)
are_orientations_equal = an_orientation == another_orientation
self.assertFalse(are_orientations_equal)
def test_givenRadianValueOfPi_whenCreateOrientationFromRadian_thenReturnOrientationWith180Degrees(
self, ):
expected_orientation = Orientation(180)
actual_orientation = Orientation.from_radian(math.pi)
self.assertEqual(actual_orientation, expected_orientation)
def test_givenCalculatedAngleOf48A_whenFindAngleBetweenRobotAndPuck_thenReturn48(
self, geometryUtils_mock):
geometryUtils_mock.return_value = Orientation(48)
expected_orientation = Orientation(48)
actual_orientation = self._rotation_service.find_orientation_to_puck(
self.A_POSITION, self.A_ROBOT_POSE)
self.assertEqual(expected_orientation, actual_orientation)
def test_givenAnOrientationInDegree_whenGetOrientationInRadians_thenOrientationIsConvertedCorrectly(
self, ):
an_orientation = Orientation(self.AN_ORIENTATION_IN_DEGREE)
expected_orientation_in_radians = 0.9250245035569946
actual_orientation_in_radians = an_orientation.get_orientation_in_radians(
)
self.assertEqual(expected_orientation_in_radians,
actual_orientation_in_radians)
def test_givenCalculatedAngleOf90AndRobotOrientationOfZero_whenFindAngleBetweenRobotAndPuck_thenReturn90(
self, geometryUtils_mock):
geometryUtils_mock.return_value = Orientation(90)
self.A_ROBOT_POSE.get_orientation_in_degree.return_value = Orientation(
0)
expected_orientation = Orientation(90)
actual_orientation = self._rotation_service.find_orientation_to_puck(
self.A_POSITION, self.A_ROBOT_POSE)
self.assertEqual(expected_orientation, actual_orientation)
def _find_direction_from_orientation(
self, orientation: Orientation) -> Direction:
if orientation.get_orientation_in_degree() < 0:
orientation = Orientation(360 +
orientation.get_orientation_in_degree())
direction = self._find_closest_direction(orientation)
if direction is None:
raise InvalidOrientationException
return direction
def test_givenRobotNotCorrectlyOrientedAndRobotWithOrientationLargerThan180Degree_whenRotate_thenSendNegativeAdjustedOrientation(
self, ):
self._setup_communication_service(1)
self._setup_vision_service(self.ANOTHER_ORIENTATION)
expected_orientation = Orientation(-126)
self._rotation_service.rotate(self.AN_ORIENTATION)
self._communication_service.send_object.assert_called_with(
Message(Topic.ROTATION, expected_orientation))
def test_givenAnImage_whenDetectRobot_thenReturnCorrectRobotPose(self):
an_image = cv2.imread(self.AN_IMAGE)
expected_robot_position = Position(705, 664)
expected_robot_orientation = Orientation(11)
expected_robot_pose = RobotPose(
expected_robot_position, expected_robot_orientation
)
actual_robot_pose = self.robot_detector.detect(an_image)
self.assertEqual(expected_robot_pose, actual_robot_pose)
def calculate_angle_between_positions(
first_position: Position,
second_position: Position) -> Orientation:
first_x, first_y = first_position.to_tuple()
second_x, second_y = second_position.to_tuple()
angle = -math.atan2(second_y - first_y, second_x - first_x)
if angle < 0:
angle += 2 * math.pi
return Orientation.from_radian(angle)
class StartingZoneCorner(Enum):
A = Orientation(45)
B = Orientation(315)
C = Orientation(225)
D = Orientation(125)
@staticmethod
def get_next_corner(
current_corner: "StartingZoneCorner") -> "StartingZoneCorner":
next_corner = {
StartingZoneCorner.A: StartingZoneCorner.B,
StartingZoneCorner.B: StartingZoneCorner.C,
StartingZoneCorner.C: StartingZoneCorner.D,
StartingZoneCorner.D: StartingZoneCorner.A,
}
return next_corner.get(current_corner)
@staticmethod
def value_of_string(string: str) -> "StartingZoneCorner":
for corner in StartingZoneCorner:
if corner.name == string:
return corner
def test_givenOrientationFarFromCardinalOrientation_whenCreateMovements_thenRaiseInvalidOrientationException(
self, ):
a_far_orientation = Orientation(188)
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
creating_movements = lambda: self.movement_factory.create_movements(
a_path, a_far_orientation)
with self.assertRaises(InvalidOrientationException):
creating_movements()
def test_givenOrientationCloseToCardinalOrientationSouth_whenCreateMovements_thenReturnMovementWithRightDirection(
self, ):
a_close_orientation = Orientation(91)
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
expected_direction = Direction.FORWARD
movement = self.movement_factory.create_movements(
a_path, a_close_orientation)[0]
self.assertEqual(movement.get_direction(), expected_direction)
def _find_smallest_orientation_to_send(self,
orientation_delta: Orientation):
if orientation_delta.get_orientation_in_degree() < -180:
return Orientation(orientation_delta.get_orientation_in_degree() +
360)
if orientation_delta.get_orientation_in_degree() >= 180:
return Orientation(orientation_delta.get_orientation_in_degree() -
360)
return orientation_delta
def _create_robot_pose(x_position: int, y_position: int, orientation: int):
position = Position(x_position, y_position)
orientation = Orientation(orientation)
return RobotPose(position, orientation)
def _rotate(self, orientation: Orientation):
self._movement_service.rotate(orientation.get_orientation_in_degree())
class TestMovementFactory(TestCase):
A_POSITION = MagicMock()
ANOTHER_POSITION = MagicMock()
A_DIRECTION = Direction.FORWARD
A_DISTANCE = Distance(10)
AN_ORIENTATION = Orientation(90)
def setUp(self) -> None:
self.movement_factory = MovementFactory()
def test_givenSingleLinePath_whenCreateMovements_thenSingleMovementIsCreated(
self):
a_path = Path(
[Position(1, 0),
Position(1, 1),
Position(1, 2),
Position(1, 4)])
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
self.assertEqual(1, len(movements))
def test_givenPathWithNinetyDegreeTurn_whenCreateMovements_thenTwoMovementsAreCreated(
self, ):
a_path = Path([
Position(3, 0),
Position(3, 1),
Position(3, 2),
Position(3, 3),
Position(2, 3),
Position(1, 3),
Position(0, 3),
])
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
self.assertEqual(2, len(movements))
def test_givenPathWithNinetyDegreeTurnAndGoingBackwards_whenCreateMovements_thenTwoMovementsAreCreated(
self, ):
a_path = Path([
Position(1, 3),
Position(1, 2),
Position(1, 1),
Position(1, 0),
Position(2, 0),
Position(3, 0),
Position(4, 0),
])
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
self.assertEqual(2, len(movements))
def test_givenStraightPathWithFivePoints_whenCreateMovements_thenASingleMovementWithDistanceFiveIsCreated(
self, ):
a_path = Path([
Position(1, 0),
Position(1, 1),
Position(1, 2),
Position(1, 4),
Position(1, 5),
])
expected_distance = Distance(distance=5)
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
movement_distance = movements[0].get_distance()
self.assertEqual(expected_distance, movement_distance)
def test_givenPathWithTurn_whenCreateMovements_thenTwoMovementsWithRightDistancesAreCreated(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
Position(4, 2),
Position(4, 3),
Position(4, 4),
])
expected_first_distance = Distance(distance=4)
expected_second_distance = Distance(distance=3)
movements = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)
first_movement_distance = movements[0].get_distance()
second_movement_distance = movements[1].get_distance()
self.assertEqual(expected_first_distance, first_movement_distance)
self.assertEqual(expected_second_distance, second_movement_distance)
def test_givenStraightPathWithXIncreasing_whenCreateMovementsWithWestOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
])
forward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)[0]
self.assertEqual(Direction.FORWARD, forward_movement.get_direction())
def test_givenStraightPathWithXIncreasing_whenCreateMovementsWithSouthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
])
right_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.SOUTH.value)[0]
self.assertEqual(Direction.RIGHT, right_movement.get_direction())
def test_givenStraightPathWithXIncreasing_whenCreateMovementsWithNorthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
])
left_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.NORTH.value)[0]
self.assertEqual(Direction.LEFT, left_movement.get_direction())
def test_givenStraightPathWithXIncreasing_whenCreateMovementsWithEastOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(2, 1),
Position(3, 1),
Position(4, 1),
])
backward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.EAST.value)[0]
self.assertEqual(Direction.BACKWARDS,
backward_movement.get_direction())
def test_givenStraightPathWithXDecreasing_whenCreateMovementsWithWestOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(4, 1),
Position(3, 1),
Position(2, 1),
Position(1, 1),
])
backward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)[0]
self.assertEqual(Direction.BACKWARDS,
backward_movement.get_direction())
def test_givenStraightPathWithXDecreasing_whenCreateMovementsWithSouthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(4, 1),
Position(3, 1),
Position(2, 1),
Position(1, 1),
])
left_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.SOUTH.value)[0]
self.assertEqual(Direction.LEFT, left_movement.get_direction())
def test_givenStraightPathWithXDecreasing_whenCreateMovementsWithNorthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(4, 1),
Position(3, 1),
Position(2, 1),
Position(1, 1),
])
right_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.NORTH.value)[0]
self.assertEqual(Direction.RIGHT, right_movement.get_direction())
def test_givenStraightPathWithXDecreasing_whenCreateMovementsWithEastOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(4, 1),
Position(3, 1),
Position(2, 1),
Position(1, 1),
])
forward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.EAST.value)[0]
self.assertEqual(Direction.FORWARD, forward_movement.get_direction())
def test_givenStraightPathWithYIncreasing_whenCreateMovementsWithWestOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(1, 2),
Position(1, 3),
Position(1, 4),
])
right_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)[0]
self.assertEqual(Direction.RIGHT, right_movement.get_direction())
def test_givenStraightPathWithYIncreasing_whenCreateMovementsWithSouthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(1, 2),
Position(1, 3),
Position(1, 4),
])
backward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.SOUTH.value)[0]
self.assertEqual(Direction.BACKWARDS,
backward_movement.get_direction())
def test_givenStraightPathWithYIncreasing_whenCreateMovementsWithNorthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(1, 2),
Position(1, 3),
Position(1, 4),
])
forward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.NORTH.value)[0]
self.assertEqual(Direction.FORWARD, forward_movement.get_direction())
def test_givenStraightPathWithYIncreasing_whenCreateMovementsWithEastOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 1),
Position(1, 2),
Position(1, 3),
Position(1, 4),
])
left_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.EAST.value)[0]
self.assertEqual(Direction.LEFT, left_movement.get_direction())
def test_givenStraightPathWithYDecreasing_whenCreateMovementsWithWestOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
left_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.WEST.value)[0]
self.assertEqual(Direction.LEFT, left_movement.get_direction())
def test_givenStraightPathWithYDecreasing_whenCreateMovementsWithSouthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
forward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.SOUTH.value)[0]
self.assertEqual(Direction.FORWARD, forward_movement.get_direction())
def test_givenStraightPathWithYDecreasing_whenCreateMovementsWithNorthOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
backward_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.NORTH.value)[0]
self.assertEqual(Direction.BACKWARDS,
backward_movement.get_direction())
def test_givenStraightPathWithYDecreasing_whenCreateMovementsWithEastOrientation_thenReturnSingleMovementWithCorrectDirection(
self, ):
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
right_movement = self.movement_factory.create_movements(
a_path, CardinalOrientation.EAST.value)[0]
self.assertEqual(Direction.RIGHT, right_movement.get_direction())
def test_givenOrientationCloseToCardinalOrientationSouth_whenCreateMovements_thenReturnMovementWithRightDirection(
self, ):
a_close_orientation = Orientation(91)
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
expected_direction = Direction.FORWARD
movement = self.movement_factory.create_movements(
a_path, a_close_orientation)[0]
self.assertEqual(movement.get_direction(), expected_direction)
def test_givenOrientationFarFromCardinalOrientation_whenCreateMovements_thenRaiseInvalidOrientationException(
self, ):
a_far_orientation = Orientation(188)
a_path = Path([
Position(1, 4),
Position(1, 3),
Position(1, 2),
Position(1, 1),
])
creating_movements = lambda: self.movement_factory.create_movements(
a_path, a_far_orientation)
with self.assertRaises(InvalidOrientationException):
creating_movements()
@patch(
"infra.utils.GeometryUtils.GeometryUtils.calculate_distance_between_two_positions"
)
def test_givenTwoPositions_whenCreateMovementToGetToPointWithDirection_thenCalculateDistanceBetweenTwoPoints(
self, geometryUtils_mock):
self.movement_factory.create_movement_to_get_to_point_with_direction(
self.A_POSITION, self.ANOTHER_POSITION, self.A_DIRECTION)
geometryUtils_mock.assert_called_with(self.A_POSITION,
self.ANOTHER_POSITION)
@patch(
"infra.utils.GeometryUtils.GeometryUtils.calculate_distance_between_two_positions"
)
def test_givenCalculatedDistance_whenCreateMovementToGetToPointWithDirection_returnMovementWithDirectionAndDistance(
self, geometryUtils_mock):
geometryUtils_mock.return_value = self.A_DISTANCE
expected_movement = Movement(self.A_DIRECTION, self.A_DISTANCE)
actual_movement = (self.movement_factory.
create_movement_to_get_to_point_with_direction(
self.A_POSITION, self.ANOTHER_POSITION,
self.A_DIRECTION))
self.assertEqual(expected_movement, actual_movement)
def test_givenPathOfOnePosition_whenCreateMovement_thenReturnSingleMovementPath(
self, ):
a_one_position_path = Path([Position(0, 0)])
movements = self.movement_factory.create_movements(
a_one_position_path, self.AN_ORIENTATION)
self.assertEqual(len(movements), 1)
def test_givenExperimentallyFoundBreakingPath_whenCreateMovement_thenDoNotThrow(
self, ):
pass
a_breaking_path = Path([
Position(148, 238),
Position(149, 238),
Position(150, 238),
Position(151, 238),
Position(152, 238),
Position(153, 238),
Position(154, 238),
Position(155, 238),
Position(156, 238),
Position(157, 238),
Position(158, 238),
Position(159, 238),
Position(160, 238),
Position(161, 238),
Position(161, 239),
])
an_orientation = CardinalOrientation.EAST.value
try:
movements = self.movement_factory.create_movements(
a_breaking_path, an_orientation)
except Exception as e:
self.fail(
f"MovementFactory.create_movements threw exception '{e}'")
def _shrink_rotation(self, orientation_delta: Orientation):
return (Orientation(-1)
if orientation_delta.get_orientation_in_degree() < 0 else
Orientation(1))
def _is_correctly_oriented(self, desired_orientation: Orientation) -> bool:
_, robot_pose = self._vision_service.get_vision_state()
return (robot_pose.get_orientation_in_degree() -
desired_orientation) == Orientation(0)
class CardinalOrientation(Enum):
WEST = Orientation(0)
SOUTH = Orientation(90)
EAST = Orientation(180)
NORTH = Orientation(270)
def run(self):
self._send_start_signal()
self._route_robot_response()
for dir in test_directions:
self._rotation_service.rotate(Orientation(dir))
calibrator = OpenCvCalibrator(CALIBRATION_FILE_PATH)
camera = OpenCvWorldCamera(LAPTOP_CAMERA_INDEX, calibrator)
robot_detector = OpenCvRobotDetector(
DICT_4X4_50,
ROBOT_ARUCO_MARKER_ID,
ROBOT_ARUCO_MARKER_SIZE,
CAMERA_MATRIX,
DISTORTION_COEFFICIENTS,
ROBOT_HEIGHT,
)
if __name__ == "__main__":
while True:
image = camera.take_world_image()
robot_pose = RobotPose(Position(800, 600), Orientation(0))
try:
robot_pose = robot_detector.detect(image)
except:
pass
x, y = robot_pose.get_position().to_tuple()
cv2.circle(
image,
robot_pose.get_gripper_position().to_tuple(),
3,
[255, 0, 0],
thickness=10,
)
cv2.imshow("robot", image)
class TestRotationService(TestCase):
AN_IMAGE = MagicMock()
A_POSITION = Position(100, 100)
ANOTHER_POSITION = Position(200, 200)
A_ROBOT_POSE = MagicMock()
AN_ORIENTATION = Orientation(20)
ANOTHER_ORIENTATION = Orientation(254)
def setUp(self) -> None:
self._communication_service = MagicMock()
self._vision_service = MagicMock()
self._rotation_service = RotationService(self._vision_service,
self._communication_service)
def test_whenRotate_thenGetVisionState(self):
self._setup_vision_service()
self._rotation_service.rotate(self.AN_ORIENTATION)
self._vision_service.get_vision_state.assert_called()
def test_givenRobotNotCorrectlyOriented_whenRotate_thenSendRotationToRobot(
self):
self._setup_communication_service(1)
self._setup_vision_service(self.ANOTHER_ORIENTATION)
self._rotation_service.rotate(self.AN_ORIENTATION)
self._communication_service.send_object.assert_called()
def test_givenRobotNotCorrectlyOriented_whenRotate_thenReceiveRotationCompletedConfirmationFromRobot(
self, ):
self._communication_service(1)
self._setup_vision_service(self.ANOTHER_ORIENTATION)
self._rotation_service.rotate(self.AN_ORIENTATION)
self._communication_service.receive_object.assert_called()
def test_givenRobotCorrectlyOriented_whenRotate_thenDontSendRotationToRobot(
self):
self._setup_vision_service()
self._rotation_service.rotate(self.AN_ORIENTATION)
self._communication_service.send_object.assert_not_called()
def test_givenRobotCorrectlyOriented_whenRotate_thenDontReceiveAnythingFromRobot(
self, ):
self._setup_vision_service()
self._rotation_service.rotate(self.AN_ORIENTATION)
self._communication_service.receive_object.assert_not_called()
def test_givenRobotAlreadyInCorrectOrientation_whenRotate_thenShouldGetVisionStateOneTime(
self, ):
self._setup_communication_service(1)
self._setup_vision_service()
self._rotation_service.rotate(self.AN_ORIENTATION)
self.assertEqual(1, self._vision_service.get_vision_state.call_count)
def test_givenRobotNotInitiallyInCorrectOrientation_whenRotate_thenGetVisionStateShouldBeCalledTwoTimesForValidation(
self, ):
self._setup_communication_service(2)
self._setup_vision_service(self.ANOTHER_ORIENTATION)
self._rotation_service.rotate(self.AN_ORIENTATION)
self.assertEqual(2, self._vision_service.get_vision_state.call_count)
def test_givenRobotNotCorrectlyOrientedAndRobotWithOrientationLargerThan180Degree_whenRotate_thenSendNegativeAdjustedOrientation(
self, ):
self._setup_communication_service(1)
self._setup_vision_service(self.ANOTHER_ORIENTATION)
expected_orientation = Orientation(-126)
self._rotation_service.rotate(self.AN_ORIENTATION)
self._communication_service.send_object.assert_called_with(
Message(Topic.ROTATION, expected_orientation))
@patch(
"infra.utils.GeometryUtils.GeometryUtils.calculate_angle_between_positions"
)
def test_whenFindOrientationToPuck_thenFindAngleBetweenRobotAndPuck(
self, geometryUtils_mock):
self.A_ROBOT_POSE.get_position.return_value = self.A_POSITION
self._rotation_service.find_orientation_to_puck(
self.ANOTHER_POSITION, self.A_ROBOT_POSE)
geometryUtils_mock.assert_called_with(self.A_POSITION,
self.ANOTHER_POSITION)
@patch(
"infra.utils.GeometryUtils.GeometryUtils.calculate_angle_between_positions"
)
def test_givenCalculatedAngleOf90AndRobotOrientationOfZero_whenFindAngleBetweenRobotAndPuck_thenReturn90(
self, geometryUtils_mock):
geometryUtils_mock.return_value = Orientation(90)
self.A_ROBOT_POSE.get_orientation_in_degree.return_value = Orientation(
0)
expected_orientation = Orientation(90)
actual_orientation = self._rotation_service.find_orientation_to_puck(
self.A_POSITION, self.A_ROBOT_POSE)
self.assertEqual(expected_orientation, actual_orientation)
@patch(
"infra.utils.GeometryUtils.GeometryUtils.calculate_angle_between_positions"
)
def test_givenCalculatedAngleOf48A_whenFindAngleBetweenRobotAndPuck_thenReturn48(
self, geometryUtils_mock):
geometryUtils_mock.return_value = Orientation(48)
expected_orientation = Orientation(48)
actual_orientation = self._rotation_service.find_orientation_to_puck(
self.A_POSITION, self.A_ROBOT_POSE)
self.assertEqual(expected_orientation, actual_orientation)
def _setup_communication_service(self,
number_of_rotation_message_send: int):
side_effect = []
for i in range(number_of_rotation_message_send):
side_effect.append(Message(Topic.ROTATION_COMPLETED, None))
self._communication_service.receive_object.side_effect = side_effect
def _setup_vision_service(self, first_robot_orientation=AN_ORIENTATION):
first_robot_pose = RobotPose(self.A_POSITION, first_robot_orientation)
a_robot_pose = RobotPose(self.A_POSITION, self.AN_ORIENTATION)
self._vision_service.get_vision_state.side_effect = [
(self.AN_IMAGE, first_robot_pose),
(self.AN_IMAGE, a_robot_pose),
]
def test_givenTwoEqualOrientations_whenEqual_thenReturnTrue(self):
an_orientation = Orientation(self.AN_ORIENTATION_IN_DEGREE)
self.assertEqual(an_orientation, an_orientation)