Waypoint queue mechanism

Sven Köhler

Waypoint queue mechanism

This page describes a mechanism for passing Waypoint information from the PathPlanner to the PoseController queue. The description starts with the WaypointListener interface, then describes the methods needed to accept Waypoints into a PoseController queue, then an explanation for the internal code of PathPlanner and finally a code example of how this would look for the user application level. There are also many utility methods we could enhance these classes with.

Requirements and Goals

The overall goal of this proposal is to make the PathPlanner less of a controller and more of a provider/reporter in a consultant role (similar to Google Maps), while still allowing the option of allowing the PathPlanner to take control of the PoseController (e.g. SLAM) and give it a limited number of moves at a time, and erase/retry a path if required (the queuing mechanism).

  • Simple, easy to use interface for users
  • Intuitive for first time users but deep/complex behavior when needed


The methods for WaypointListener are:

  • receiveWaypoint(Waypoint) - notifies listener of the next Waypoint in the sequence
  • pathComplete() - only called when the PathPlanner has sent the last Waypoint in the path.

Multiple listeners are allowed. In a typical example, the first listener is a PoseController which adds waypoints to its own queue. A second listener is a GUI that displays the path realtime as it becomes available.


Lawrie, Roger and Brian expressed a desire to retain the basic goTo(destination) functionality of PoseController which allows it to operate independent of a PathPlanner. However, is it okay if the PoseController API and internal code reference a PathPlanner object?

  • PoseController.addWaypoint(destination) - adds a waypoint to the end of the queue.
  • PoseController.goTo(destination) - If a queue exists already and the user calls this, what behavior should it produce? Options are:
    1. Stop movement and wipe out the existing queue.
    2. Add this point to the end of the queue
    3. Add this point to the start of the queue
    4. Return some error (boolean) that a queue exists and it is currently driving the queue and do nothing (ignore command)
    5. Throw an exception
  • PoseController.goTo(destination, PathPlanner) - Goes to a destination using a certain PathPlanner to plot that route. Internally this method tells the pathplanner to start coming up with a path, and the PathPlanner starts feeding waypoints to PoseController via addWaypoint(). Is it okay to reference PathPlanner in the PoseController interface? The code for this is not burdensome for developers making PoseControllers (2-3 lines at most for this method).


There is one basic method for the PathPlanner interface called addWaypointListener() to work with the listener interface. There can be two variations of this method in the API. Also I believe we should expand this (see Utility Methods below). For now, the variations of this method are:

  • addWaypointListener(WaypointListener, start, destination) - Adds listener that will receive waypoints for the path between start and destination.
  • addWaypointListener(WaypointListener) - Any waypoints the PathPlanner generates are sent to all listeners, such as a GUI.

Some implementations of PathPlanner, such as SLAM, will need to interact with the PoseController doing things such as clearing the queue and remaking paths, and interacting with ObstacleDetectors. The SLAMPathPlanner implementation will likely accept a PoseController in the constructor, but it will not require any special methods in the PathPlanner interface to deal with this (it is done internally). Most PathPlanners will only require Map data, a start pose, and a destination in the constructor.

Alternate PathPlanner interface

With the two methods described above, when the PathPlanner is asked by the application to generate a path, and then before it completes it is asked to generate another path, it seems like it could get confused:

addWaypointListener(WaypointListener, start1, destination1);
addWaypointListener(WaypointListener, start2, destination2);

How should it behave?

  1. immediately stop generating the previous path and start working on the newest one
  2. return false to indicate it is currently busy
  3. start a new thread each time and only send waypoints to the listener object provided by that particular method call (this makes it ambiguous which set of waypoints to send to listeners that don't specify a particular start and destination)

Or we could try an alternate API with these methods:

  • addWaypointListener(WaypointListener)
  • startPathfinding(start, destination)

This is a style decision.

Application Code Sample

MoveController pilot = new DifferentialMoveController(..);
PoseProvider locator = new MCLPoseProvider(..); 
PoseController navigator = new PoseController(pilot, locator);

PathPlanner planner = new MapPathPlanner(..);
navigator.goTo(destination, planner);

Alternate Architecture

NOTE: We could add an alternate constructor in PoseController that accepts a PathPlanner, and then the PoseController is bound with it for all moves that are asked via PoseController.goTo(destination). This would eliminate the need for PoseController.goTo(destination, PathPlanner):

MoveController pilot = new DifferentialMoveController(..);
PoseProvider locator = new MCLPoseProvider(..); 
PathPlanner planner = new MapPathPlanner(..);
PoseController navigator = new PoseController(pilot, locator, planner); // Alternate constructor


This is largely an API style decision.

Utility Methods

Some of these utility methods will be needed, others are optional.

  • PathPlanner.getPath(start, destination) - returns a complete path all in one go. Internally it just creates an inner class of WaypointListener and then listens to itself (adding waypoitns to a collection) until a complete collection of waypoints is done. Not all path-planning algorithms will be able to support this (SLAM, rangefinder pathfinder with no preexisting map data). In this case it returns NULL right away.
  • PoseController.getWaypoints() - returns the current queue. Copy of queue or actual collection of waypoints? Note: If a reference to the actual live collection is returned, it might allow the methods below to be called directly on the collection object rather than on PoseController. In fact that might be preferred.
  • PoseController.addPath(collection of waypoints) - adds a larger collection to the queue rather than one at a time (Roger suggested this yesterday).
  • PoseController.insertWaypoint(int position, Waypoint) - insert a waypoint at any position in the queue. I'm a little worried that the queue would be changing live, so it might be better to insert after or before a certain waypoint that is somewhere in the queue: PoseController.addWaypointAfter(Waypoint index, Waypoint destination) Returns false if it failed to insert it (due to index Waypoint being consumed).
  • PoseController.eraseWaypoints() - erase the entire waypoint queue
  • PoseController.eraseWaypoints(int start, int end) - erase a section of the waypoint queue. Again, there are concerns the live waypoint queue might change, so we might want the start position to be an actual Waypoint. Returns false if it failed to insert it.

Code Sample using Utility Methods

These helper methods allow users to use PathPlanner similar to Google maps (consultant only). This means applications can do things such as get the entire route, display it in a GUI, play around with the waypoints, then feed it to the PoseController:

MoveController pilot = new DifferentialMoveController(..);
PoseProvider locator = new MCLPoseProvider(..); 
PoseController navigator = new PoseController(pilot, locator);

PathPlanner planner = new MapPathPlanner(..);
path = planner.getPath(start, destination)

// Application displays waypoints on GUI:
// GUI allows user to tailor waypoints in path
// When done user clicks "Go" button:
path = gui.getPath();



This is the basic outline of how this could work but it is not a complete or final architecture. All method and even class names are provisional, to be finalized later.

I'm a little worried this over-complicates the architecture. However, as the code above shows, the actual listener interface is not really used by users. Most of the complexity is on the developer end (us) and not the user end, which is a plus.


NXT Wiki: Home