A New Local Planner

This is another blog post that has sat in draft for 2+ years. I’m pushing it out ahead of ROSCon this year where I’ll be talking about migrating the UBR-1 mobile manipulator to ROS2.

For a long time I’ve wanted a better open-sourced local controller for ROS navigation. It’s finally implemented, and has been running on the robot fleet at Cobalt Robotics for about two years now. The full code of the graceful_controller is available on GitHub.

Motivation

There are a number of local controllers in the ROS1 and ROS2 navigation stacks, but most of them are based on either the Dynamic Window Approach, Trajectory Rollout, or optimization approaches such as Time Elastic Bands.

Both DWA and Trajectory Rollout suffer from a series of challenges:

For this reason, many people replace the default controllers - especially corporate users of ROS. Sadly most of these improved controllers don’t get released into the open source.

Goals

The goals for the new controller were quite simple:

Underlying Algorithms

A number of years ago at Fetch Robotics, I wrote a package for our robots to autonomously connect to their charge dock. Sometime later, Fetch open sourced the package under an LGPL license. I used this same underlying control law for the first part of the new controller.

This underlying control law is based on a paper called A Smooth Control Law for Graceful Motion of Differential Wheeled Mobile Robots in 2D Environments by Park and Kuipers, which was presented at ICRA 2011.

This control law implements a closed-form solution to find a kinematically feasible linear and angular velocity for a differential drive robot to approach a pose, based on just a few parameters that are actually fairly robot-independent. The image below (from the original paper) shows some example trajectories:

The underlying control law has a number of nice features:

The underlying control law really only works for approaching a single pose - which we call the target pose. This is exactly what docking with a charger entails. For something like navigation, we typically need to approach a series of poses in succession (since following the control law directly towards the final pose in the path is usually not collision free). Section IV.B of the paper describes a fairly complex approach to do this, however, our controller takes a simpler approach.

Our controller attempts to use as the target pose the farthest pose in the path which is both A) less than some maximum lookahead distance away from the robot, and B) reachable with our current control law parameters without collision.

The reachability portion is implemented through a forward simulation. Unlike DWA, the forward simulation is not time-based, but rather terminates when we have reached the target pose. Additionally, since the target pose is the farthest that we have collision checked, we will actually forward simulate based on stopping at the target pose.

The new controller is built out of two ROS packages. The first package includes the underlying control law, which is licensed under the LGPL. The second package implements a ROS1 or ROS2 plugin for navigation, and is licensed under BSD since it is loosely based on the DWA codebase.

Additional Features

While the basic control law with target pose simulation works fairly well, there are a number of additional improvements implemented:

ROS2 Support

As it turns out, porting to ROS2 was fairly straight forward. Improvements to the controller API in ROS2 mean that the following features are far easier to implement:

Future Work

There have been quite a few developments since I first started working on this controller in early 2021.

The Nav2 project added the MPPI controller, which addresses many of my concerns with both DWA and Trajectory Rollout. My only issue with this controller is that it uses special SSE/NEON instructions for parallelization, and so it doesn’t work on all CPUs (in particular, I found it wouldn’t work on the very low end Celeron processor in my FireBot).

An alternate implementation of the control law was also added to Nav2 as part of the docking server project. It’s not quite ready to be a full controller yet, but at some point I expect that version to supersede the ROS 2 implementation of my graceful controller package.

Kudos

This work was supported in part by Cobalt Robotics.