Layered costmaps for context-sensitive navigation

Many navigation systems, including the ubiquitous ROS navigation stack, perform path-planning on a single costmap, in which the majority of information is stored in a single grid. This approach is quite successful at generating collision-free paths of minimal length, but it can struggle in dynamic, people-filled environments when the values in the costmap expand beyond occupied or free space. We have created and implemented a new method called layered costmaps, which work by separating the processing of costmap data into semantically-separated layers. Each layer tracks one type of obstacle or constraint, and then modifies a master costmap which is used for the path planning. We show how the algorithm can be integrated with the open-source ROS navigation stack, and how our approach is easier to fine-tune to specific environmental contexts than the existing monolithic one. Our design also results in faster path planning in practical use, and exhibits a cleaner separation of concerns that the original architecture. The new algorithm also makes it possible to represent complex cost values in order to create navigation behavior for a wide range of contexts.