def _init_validators(self):
self._path_width_validator = PathWidthValidator(self._coordinate_factory, self._world_map_service.world_map)
self._final_destination_overflow_validator = FinalDestinationOverflowValidator(self._coordinate_factory, self._world_map_service.world_map)
self._wall_overflow_validator = WallOverflowValidator(self._coordinate_factory, self._world_map_service.world_map)
class PathNavigator(object):
STEP_BY_STEP_WAIT_CHECK_INTERVAL_SEC = 0.5
PAUSE_WAIT_CHECK_INTERVAL_SEC = 0.5
WAIT_TIME_AFTER_MOVEMENT_SEC = 0.5
WAIT_TIME_AFTER_ROTATION_SEC = 1
MIN_ROTATION_ANGLE = 1
MIN_MOVEMENT_CM = Vector2(1, 1)
REALIGNABLE_POSITION_DIFFERENCE_TRESHOLD_CM = Vector2.uniform(5)
@property
def step_by_step(self):
return self._step_by_step
@step_by_step.setter
def step_by_step(self, value):
self._step_by_step = value
@property
def simulation_mode(self):
return self._simulation_mode
@simulation_mode.setter
def simulation_mode(self, value):
self._simulation_mode = value
if self._simulation_mode == True:
self._world_map.robot_prioritization_mode = RobotPrioritizationMode.PRIORITIZE_ESTIMATED
else:
self._world_map.robot_prioritization_mode = RobotPrioritizationMode.PRIORITIZE_DETECTED
def __init__(self, movement_executor, world_map_service, coordinate_factory):
self._movement_executor = movement_executor
self._world_map_service = world_map_service
self._world_map = world_map_service.world_map
self._coordinate_factory = coordinate_factory
self._init_validators()
self._step_by_step = False
self._simulation_mode = False
self._completion = 0
self._init_state()
def _init_validators(self):
self._path_width_validator = PathWidthValidator(self._coordinate_factory, self._world_map_service.world_map)
self._final_destination_overflow_validator = FinalDestinationOverflowValidator(self._coordinate_factory, self._world_map_service.world_map)
self._wall_overflow_validator = WallOverflowValidator(self._coordinate_factory, self._world_map_service.world_map)
def navigate(self, path, move_speed, rotation_speed, target_location, robot_arrival_side = RobotSide.FRONT, centered_arrival = True):
self._path = path
self._move_speed = move_speed
self._rotation_speed = rotation_speed
self._target_location = target_location
self._centered_arrival = centered_arrival
self._robot_arrival_side = robot_arrival_side
self._init_state()
for segment in path.segments:
navigation_action = self._navigate_segment(segment)
if navigation_action.is_last_action:
break
self._perform_final_move(navigation_action)
def _init_state(self):
self._step_wait = False
self._stop_requested = False
self._pause_requested = False
def _navigate_segment(self, segment):
Logger.get_instance().log(self, "Follow Segment", LogEntryType.INFORMATIONAL, "Start: {0}, Destination: {1}".format(segment.point1, segment.point2))
navigation_action = PathNavigationAction(segment.point1, segment.point2.clone(), self._world_map.robot.angle)
self._realign_robot(navigation_action)
self._perform_validations(navigation_action)
self._rotate_robot(navigation_action.angle)
self._move_robot(navigation_action.destination)
return navigation_action
def _realign_robot(self, navigation_action):
realignable_position_difference_treshold = self._coordinate_factory.create(self.REALIGNABLE_POSITION_DIFFERENCE_TRESHOLD_CM, CoordinateSystem.PHYSICAL).get().length
while Segment(self._world_map.robot.get_center(), navigation_action.start_position).length > realignable_position_difference_treshold:
Logger.get_instance().log(self, "Robot Realign", LogEntryType.INFORMATIONAL)
self._move_robot(navigation_action.start_position)
def _perform_validations(self, navigation_action):
self._path_width_validator.validate(navigation_action)
self._wall_overflow_validator.validate(navigation_action)
self._final_destination_overflow_validator.validate(navigation_action, self._path, self._robot_arrival_side, self._target_location, self._centered_arrival)
def _rotate_robot(self, angle):
angle_difference, expected_angle = self._get_required_rotation_angle(angle)
if math.fabs(angle_difference) < self.MIN_ROTATION_ANGLE:
return
self._check_state()
if not self._simulation_mode:
self._movement_executor.rotate(angle_deg = angle_difference, speed_cmps = self._rotation_speed)
self._world_map_service.update_estimated_robot_position(angle = expected_angle)
time.sleep(self.WAIT_TIME_AFTER_ROTATION_SEC)
def _get_required_rotation_angle(self, expected_angle):
actual_angle = self._world_map.robot.angle
angle_difference = MathUtils.angle_difference(actual_angle, expected_angle)
Logger.get_instance().log(self, "Navigation Rotation", LogEntryType.INFORMATIONAL, "Expected: {0}, Actual: {1}, Difference: {2}, Speed (cm/s): {3}".format(expected_angle, actual_angle, angle_difference, self._rotation_speed))
return angle_difference, expected_angle
def _move_robot(self, destination):
relative_destination = self._coordinate_factory.create(destination, CoordinateSystem.GAME).get(CoordinateSystem.ROBOT, self._world_map.robot)
Logger.get_instance().log(self, "Navigation Movement", LogEntryType.INFORMATIONAL, "Destination: {0}, Relative Destination (cm): {1}, Speed (cm/s): {2}".format(destination, relative_destination, self._move_speed))
if relative_destination.length < self.MIN_MOVEMENT_CM.length:
return
self._check_state()
if not self._simulation_mode:
self._movement_executor.move_to(destination_cm = relative_destination, max_speed_cmps = self._move_speed)
self._world_map_service.update_estimated_robot_position(center = destination)
time.sleep(self.WAIT_TIME_AFTER_MOVEMENT_SEC)
def _perform_final_move(self, navigation_action):
self._move_robot(navigation_action.destination)
self._rotate_robot(Segment(navigation_action.destination, self._target_location).angle + self._robot_arrival_side.rotation_angle)
def _check_state(self):
self._check_stop_request()
self._pause_wait()
self._step_by_step_wait()
self._check_stop_request()
def _check_stop_request(self):
if self._stop_requested:
raise UserStopMotionError("Navigation stopped by user request.")
def _step_by_step_wait(self):
self._step_wait = True
while self._step_by_step and self._step_wait:
time.sleep(self.STEP_BY_STEP_WAIT_CHECK_INTERVAL_SEC)
self._check_stop_request()
self._step_wait = True
def _pause_wait(self):
while self._pause_requested:
time.sleep(self.PAUSE_WAIT_CHECK_INTERVAL_SEC)
self._check_stop_request()
def next_step(self):
self._step_wait = False
def stop_navigation(self):
self._stop_requested = True
def pause_navigation(self):
self._pause_requested = True
def resume_navigation(self):
self._pause_requested = False
def reset(self):
self.stop_navigation()
self._world_map_service.clear_estimated_robot_position()
