Hi all,
It’s your friendly neighborhood navigator here. In a cross collaboration with RoboTech Vision, we’re looking at abstracting out the smoother in the Smac Planner into its own server and adding in a few different path smoothing options. This will enable users to take any path from any planner and request it to be smoothed in accordance with a set of smoothing algorithm plugins (analogous to the design of the controller and planner servers). This will mean we have now 4 primary task servers: Recovery, Smoothing, Planning, and Control.
We wanted your comments / questions / ideas / suggestions on the topic of path smoothing so we make sure to take into account all of the different criteria users might have on the subject. Below is a brief description of our current thoughts as a basis but we’d appreciate anything on this subject you care to share or suggest, including algorithm ideas, use-cases, or more.
We are currently looking at making an abstract nav2_smoother package containing an action server lifecycle node. This action server will expose a new SmoothPath action (defined roughly below) that can be called to smooth a path. This server will contain a map of named plugins that can be configured independently and utilized using the smoother_id parameter. The server will contain a costmap and footprint subscribers to the global costmap to use if cost information is valuable to your smoother plugin, given in the configuration stage of the lifecycle node.
#goal definition
nav_msgs/Path path #input path
string smoother_id #plugin to use
builtin_interfaces/Duration max_smoothing_duration #max time for smoothing
---
#result definition
nav_msgs/Path path #output smoothed path
builtin_interfaces/Duration smoothing_duration #how much time was spent smoothing
bool was_completed #did smoothing complete successfully in the time allotted
---
#feedback
While we haven’t discussed at length the API for the plugins, it would likely be something like:
- void configure(costmap subscriber, footprint subscriber, tf, node, name): get parameters, setup the plugin
- path_and_time smooth(path_and_time): do the major path smoothing action on the path within the time window provided. Returns smoothed path in used time struct.
- activate / deactivate / etc: filling in lifecycle requirements
In terms of techniques, our current thinking is to support the current Smac Planner smoother which is a more basic optimization-based solution with collision checking to stop smoothing if it hits something. We will also add in an improved version of the old Smac Planner smoother which is a much more intelligent Ceres based cost-aware smoother that is curvature constained. While this definitely works better, also takes much longer to compute. So its up to a designer what they prefer but they each have their niche.
Other thoughts of methods would be spline-based solutions such as b-splines or bezier curves, as well as elastic-band based techniques. We do not have implementations of those at hand to plugin-ize but if there was serious desire for them over the optimization-based solutions, that would be something we would consider.
Thanks for your time, please let me know if you have any questions / comments / suggestions / requests so we can incorporate them in our design and review process for this work.
Happy smoothing,
Steve