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