def _find_destination(self, objective: Objective) -> CameraCoordinate:
self._mobility_service.operate(
[TranslateOperation(Angle(pi), Distance(5))])
self._direction_cortex.reach_goal()
self._mobility_service.operate(
[TranslateOperation(Angle(pi / 2), Distance(1))])
return self._visual_cortex.find_drop_position(objective)
def test_whenAngleIsBetweenMinusPiAndPi_thenEffectiveAngleIsBetweenMinusPiAndPi(self) -> None:
angle = Angle(3 * pi / 4)
effective_angle = Angle.to_effective(angle)
self.assertEqual(angle, effective_angle)
self.assertGreaterEqual(effective_angle, Angle(-pi))
self.assertLessEqual(effective_angle, Angle(pi))
def test_givenZeroDistanceMovement_whenCalculating_thenRotateOperationReturned(self) -> None:
start = AbsoluteCoordinate(0, 0, Orientation(0))
stop = AbsoluteCoordinate(0, 0, Orientation(pi))
self.robot_coordinate.orientation_change = Angle(pi)
movement = AbsoluteMovement(start, stop)
self.direct_driver.drive(movement, self.drivable)
expected_calls = [call.rotate(Angle(pi))]
actual_calls = self.drivable.method_calls
self.assertListEqual(expected_calls, actual_calls)
def _find_matching_items(self,
objective: Objective,
move: bool = True) -> List[Item]:
if move:
self._mobility_service.operate(
[TranslateOperation(Angle(pi), Distance(5))])
self._direction_cortex.reach_source()
self._mobility_service.operate(
[TranslateOperation(Angle(pi / 2), Distance(1))])
items = self._visual_cortex.find_items()
return self._item_chooser.choose_from(objective, items)
def _calculate_camera(movement: CameraMovement) -> List[Operation]:
print("camera movement {}".format(movement))
camera_coordinate_transform = CameraCoordinateTransform(movement.start)
robot_coordinate = camera_coordinate_transform.from_camera(
movement.stop)
print("robot coordinate {}".format(robot_coordinate))
distance = Distance(robot_coordinate.length)
operations = []
if distance != Distance.zero():
angle = Angle(robot_coordinate.direction.radians)
angle = Angle.to_effective(angle)
operations.append(CarefulOperation(angle, distance))
return operations
def _calculate_absolute(movement: AbsoluteMovement) -> List[Operation]:
robot_coordinate_transform = RobotCoordinateTransform(movement.start)
robot_coordinate = robot_coordinate_transform.from_absolute(
movement.stop)
distance = Distance(robot_coordinate.length)
rotation = Angle(robot_coordinate.orientation_change.radians)
operations = []
if rotation != Angle.zero():
rotation = Angle.to_effective(rotation)
operations.append(RotateOperation(rotation))
if distance != Distance.zero():
angle = Angle(robot_coordinate.direction.radians)
angle = Angle.to_effective(angle)
operations.append(TranslateOperation(angle, distance))
return operations
def translate_to_rotate_operation(message: Message) -> RotateOperation:
try:
angle = float(message.get_data("angle"))
except ValueError:
logger.warning("cannot translate angle value to float")
raise TranslationError
return RotateOperation(Angle(angle * (2 * pi)))
def _orientation_within_limits(self, actual: Orientation,
referential: Orientation) -> bool:
ref_angle = Angle(referential.radians)
effective_ref_angle = Angle.to_effective(ref_angle)
lower_limit = Angle(effective_ref_angle.radians - self.angle_epsilon)
upper_limit = Angle(effective_ref_angle.radians + self.angle_epsilon)
actual_angle = Angle(actual.radians)
effective_actual_angle = Angle.to_effective(actual_angle)
return lower_limit <= effective_actual_angle <= upper_limit
from typing import Union
from communication.service.message import Message
from cortex.domain.objective.color import Color
from cortex.domain.objective.objective import Objective
from cortex.domain.objective.shape import Shape
from mobility.domain.angle import Angle
from mobility.domain.distance import Distance
from mobility.domain.operation.carefulOperation import CarefulOperation
from mobility.domain.operation.rotateOperation import RotateOperation
from mobility.domain.operation.translateOperation import TranslateOperation
from remote.translationError import TranslationError
logger = getLogger(__name__)
directionAngleMapping = {
"up": Angle(0),
"upRight": Angle(pi / 4),
"right": Angle(pi / 2),
"downRight": Angle(3 * pi / 4),
"down": Angle(pi),
"downLeft": Angle(-3 * pi / 4),
"left": Angle(-pi / 2),
"upLeft": Angle(-pi / 4),
}
shapes = {
"Shapes:triangle": Shape.Triangle,
"Shapes:square": Shape.Square,
"Shapes:pentagon": Shape.Pentagon,
"Shapes:circle": Shape.Circle
}
colors = {
def _align_on_charge_station_func(self) -> None:
self._charging.clear()
while not self._charging.is_set():
self._direction_cortex.reach_charge()
self._mobility_service.operate([TranslateOperation(Angle(pi), Distance(DISTANCE_FROM_BED))])
sleep(1)
def _wake_up(self):
self._mobility_service.operate([TranslateOperation(Angle(0), Distance(DISTANCE_FROM_BED))])