Editing Queued Navigation Architecture

Sven Köhler
Navigation4.jpg (199503 bytes)
Pathcontroller.jpg (27403 bytes)

Proposal for Queued Navigation Architecture

Updated to include PathController:

Here is a version of the diagram that includes all the current and proposed interfaces:


The application constructs a pilot and an obstacle detector appropriate to the robot and calls the path planner, specifying the destination.

The [path planner] runs in its own thread. It consults the map, plans a path as a series of waypoints and feeds them into the waypoint queue.

The navigator runs in its own thread. It reads waypoints from the waypoint queue, determines what moves the robot can make from the pilot (e.g. arc or rotate), and feeds moves into the move queue.

The pilot and obstacle avoider run in parallel in their own threads or possibly in the same thread. The pilot knows about the robot design. It implements a PID or similar algorithm to generate Motor commands. It reads moves from the move queue, and informs the obstacle detector when each move stops and starts and what type it is. It also gives the same information to the pose provider and the pilot state estimator. The state estimator is given the current settings of velocity and angular velocity as well.

When the obstacle detector detects an obstacle, it informs the navigator and path planner. These flush their queues. The path planner then constructs a new path, avoiding the obstacle and feeds it to the waypoint queue.

The pose provider receives moves from the pilot and any amount of sensor data from sensor providers. It will typically be a localization Bayes filter, such as an MCL particle filter.

The pilot state estimator hold the current state of a move (position, velocity, angular velocity, acceleration etc.) in terms that are independent of the design of the robot. It is told about moves and velocity from the pilot and reads sensor data from any number of sensor providers. It will typically be a Bayes filter, such as a Kalman filter. The pilot consults it to get the current state of the move.

Proposed Interfaces

 * To perform "offline" navigation, the PathPlanner returns a complete set of points.
 * For "online" navigation (active) it is up to the PoseController (such as SLAMPoseController) to 
 * request smaller segments of a path to build up the larger complete path.
 * No WaypointListener is really needed in this object, because all waypoint paths are given on request.
 * Optional information for the PathPlanner is given in the constructor, and may include
 * a MoveController (to indicate the types of paths it can drive), a Map, etc...
public interface PathPlanner {
  public ArrayList<WayPoint> getPath(Point start, Point finish);

/** ALTERNATE NAME: PathController
 * PoseControllers are stackable, meaning a more advanced PoseController can use a simpler PoseController
 * in its constructor.
 * Other optional information given in the PoseController constructor:
 * PathPlanner, ObstacleDetector, MoveController
public interface PoseController {
  public void goTo(Waypoint destination , boolean immediateReturn); //go to a single waypoint
  public void followRoute(ArrayList<WayPoint> theRoute); //begin following the route
  public void flushQueue(); // deletes all waypoints
  public void interrupt(); // stops the robot immediately
  public void resume();  //following the route after an interruption;
  public void addWaypoint(WayPoint aWayPoint); // adds a WayPoint to the route.
  addListener(WayPointListener aListener);

public interface WayPointListener
  atWayPoint(Pose thePose);  //presumably called by WayPointController when a WayPoint is reached

public interface MoveProvider {
  public Move getMovement();
  public void addMoveListener(MoveListener listener);

public interface MoveListener {
  public void moveStarted(Move event, MoveProvider mp);
  public void moveStopped(Move event, MoveProvider mp);

public interface PoseProvider {
  public Pose getPose();    

public interface MoveController extends MoveProvider {
  public void flush();
  public void interrupt();
  public void forward();
  public void backward();
  public boolean stop();
  public boolean isMoving();
  public boolean travel(float distance);
  public boolean travel(float distance, boolean immediateReturn);
  public float getMovementIncrement();
  public void setMoveSpeed(float speed);
  public float getMoveSpeed();
  public float getMoveMaxSpeed();

public interface ArcMoveController extends MoveController {
  public float getMinRadius();
  public void setMinRadius(float radius);
  public boolean arcForward(float radius);
  public boolean arcBackward(float radius);
  public boolean arc(float radius, float angle);
  public boolean arc(float radius, float angle, boolean immediateReturn);
  public boolean travelArc(float radius, float distance);
  public boolean travelArc(float radius, float distance, boolean immediateReturn);}

public interface RotateMoveController extends MoveController {
  public boolean rotate(float angle);
  public boolean rotate(float angle, boolean immediateReturn);
  public float getAngleIncrement();
  public void setTurnSpeed(float speed);
  public float getTurnSpeed();
  public float getTurnMaxSpeed();

public interface ArcRotateMoveController extends ArcMoveController, RotateMoveController {

public interface PilotStateEstimation {
  public float getAngleTurned();
  public float getDistanceTraveled();
  public float getForwardVelocity();
  public float getAngularVelocity();
  public float getForwardAcceleration();
  public float getAngularAcceleratiion();

public void ObstacleDetector {
  public void obstacleDectected(Obstacle obstacle) ;
  public void addObstacleListener():

public void ObstacelListener {
  public void detected(Obstacle anObstacle);

public interface SensorConsumer() {
  addSensorProvider(Measurement quantity, SensorProvider provider);

public interface SensorProvider {

public interface RangeMap {
  public float range(Pose pose);
  public boolean inside(Point p);
  public Rectangle getBoundingRect();

public interface TachoMotorListener {
  public void rotationStarted(TachoMotor motor,int tachoCount, boolean stalled, long timeStamp);
  public void rotationStopped(TachoMotor motor,int tachoCount, boolean stalled,long timeStamp);

public interface TachoMotor extends Tachometer, DCMotor {
 // As now

Outline of calling application

// Construct a map of the environment
map = new LineMap(...);

// Construct an ObstacleDetector that uses an Ultrasonic sensor
obstacleDetector = new RangeObstacleDetector(sonic, THRESHOLD);

// Construct a state estimator for the pilot, using a Kalman filter with feedback from a Gyro Sensor
stateEstimator = new KalmanPilotStateEstimator();
stateEstimator.addSensorProvider(Measurement.ANGULAR_VELOCITY, gyro);

// Construct an MCL PoseProvider
poseProvider = new  MCLPoseProvider(map);
poseProvider.addSensorProvider(Measurement.RANGE_READINGS, rangeScanner);

// Construct the MoveController
moveController = new SteeringMoveController(Motor.A, Motor.C, stateEstimator, ...);

// Construct a path planner that uses the map
planner = new MapPathPlanner(map);

// Construct the PoseController
poseController = new SLAMPoseController(moveController, poseProvider, planner);

// Go to the destination