Paul Senatillaka
May 3, 2012

Waypoint Planning


Waypoint planning such is in the IGVC competition should be adaptive. Rather then statically presetting the order of waypoints (goals in this case), my aim is to adaptively select the shortest path across all remaining waypoints. An A* search is performed on an occupancy grid of the robot's current map from every waypoint to every other waypoint to get distances that take into account paths around obstacles. The resulting distances are built into a graph and Dijkstra's search is used to obtain the order of waypoint traversal which yields the shortest total path.


Concepts Demonstrated

  • Stage is used to simulate the IGVC competition
  • ROS Navigation Stack is used to build and publish an occupancy grid of the map
  • A* Search' is used to find the path distances between waypoints in the occupancy grid
  • Dijkstra's Search is used to find the shortest path of waypoints in a undirected graph


Adaptively choosing the shortest path of waypoints to visit which takes into account obstacles in the map. Also, not just finding the closest waypoint from the robot by doing a single shortest path search, but finding the shortest total path of waypoints. This could occur if for example waypoint B is further away then A but by visiting B first, subsequent waypoints would be closer.

Technology Used Block Diagram

Create a simple block diagram of your system that illustrates the major technical components and how they interact; e.g.:

Additional Remarks

Built a Dijkstra's search algorithm. Played around with the Navigation stack in ROS a lot to try to get a costmap occupancy grid built and visualized along with the SLAM node that comes with it. Though unsuccessful at fully implementing the nav stack, learned a lot.