Layered Costmap Implementation

Although the algorithm and data structure for layered costmaps are system-agnostic, due to the ubiquity of the platform, we focused on implementing the system to work with the ROS Navigation stack in order to demonstrate the capabilities of the approach. The layered costmap implementation keeps the Costmap2DROS API mostly intact and like the rest of the navigation code, is implemented in C++, as are each of the layers. Costmap2DROS remains the facade and provides access to the master costmap for path planning. The Costmap2D class no longer has logic for interpreting the sensor data or any other data source, and instead just acts as the data structure for holding the grid of values. All of the data source-specific logic now resides in the individual layers.

Implementing a layer requires a few simple steps. First, a new class must be created which extends the costmap 2d::Layer class. This means implementing the initialize function (where the layer can independently subscribe to any data sources in the ROS ecosystem), the updateBounds function and the updateCosts function. Independently compiled layers can be plugged in to the layered costmap with simple run-time parameter changes. This removes the need for having to recompile the entire navigation stack with every change to how the costmaps work.

Each of the layers can be implemented individually, but since many of them share the func- tionality of maintaining a private costmap which is then combined with the master costmap in some fashion, many of the layers are implemented using the template class CostmapLayer, which handles the basic bounds bookkeeping and the combining of the costmaps, leaving the subclassed layer to only need to populate the private costmap.

Whereas previously the costmap class had special cases to deal with whether there was a static map or not, and whether to track the obstacles in three dimensions, these cases are instead handled by configuring the global and local costmaps with different layers. Such changes can be made by either instantiating the static layer or not, or by instantiating either the obstacles layer or the voxels layer. This greatly reduces the amount of special case code within the Costmap2DROS class.

The implementation fixes the problems enumerated in the previous section, some just by virtue of having the layered costmap. With the layered costmap’s update algorithm, the exact bounds of the updated areas of the costmap are tracked, allowing the inflation layer to

only update the required areas of the costmap. This also reduces the bandwidth of publishing the costmap, since only the changes are published. Since how each layer combines with the previous layers is determined on a layer-by-layer basis, the policy is customizable per layer, allowing for any number of combination policies to be enacted. The previously rigid types of data the costmap could take as input become looser since the layers can be customized for any particular data type, thus allowing us to introduce nonlethal values wherever we like. Also, encompassing each data types functionality in a layer forces an enhanced level of encapsulation of related logic, and as a result, the update process becomes much clearer.

7.5

Discussion

In this chapter, we have discussed the benefits of the new layered costmap model over the previous monolithic model. Due to its efficiency and extensibility, an implementation of it has been adopted as the default navigation algorithm for the latest Hydro release of ROS, the source code for which can be found at http://github.com/ros-planning/navigation. All of the code for the additional layers can found linked to from http://wiki.ros.org/ costmap_2d. Furthermore, based on the plugin-based layer structure, we hope that the new developments in creating additional costmap rules will be implemented as layers and tested within the ROS navigation framework, allowing for more open exchange of algorithmic behavior and more accessible comparisons between them.

The layered costmap and the associated layers open up the possibility for a wide range of additional robot behaviors. As more layers are assimilated into the planning algorithms, the robots will become more aware of different facets of their environment, and take those contexts into account while navigating. The current state of the practice is just to ignore the additional contexts, or tackle them one at a time. While the layered costmap does enable the contexts to be integrated, we predict that the future challenge will be to find a way to dynamically manage the collections of layers in order to ensure the right contexts are prioritized at the right times. Proxemic behavior dictates that personal space should be respected, but precisely how much less efficient the robot’s path should be as a result is an open question. Half of the problem is designing costmap layers such that the mathematically optimal path is the desired distance away. The other half is a social question with unclear answers, of how to balance the needs of robots against the needs of people. While we can

offer no concrete answers to such a question, we believe that having a highly customizable data structure for customizing robot behavior will make answering such a question much easier.

Chapter 8

Layers in the Costmap

In this chapter, we examine the different layers that can be added to the costmap. This includes layers whose values were already included in the ROS costmap, new layers to repli- cate the costmap adjustments of previous works and entirely new layers. Each layer gives us the ability to integrate environmental contexts into the costmap and have them treated in an equal manner to the other layers. With the power of the layered costmap, we can enable and disable any subset of these layers to produce the desired behavior in the robot.