🌐 Costmap Filters#

Costmap Filter is a term introduced in Nav2 and refers to the capability of marking/annotating certain areas of the map with additional information that can be used to indicate, for example: restricted areas, speed zones, which are areas where the robot should move faster or slower, other than areas in which the robot should perform a certain action, etc.

The standard environmental representation used for the navigation of autonomous mobile robots is the Costmap 2D, also known as the Layered Costmap, in which each layer takes into account a different aspect of the environment.

For more information on how Layered Costmap works, you can check this video. In this layered representation of the environment, each layer is a plugin that updates the Costmap with specific sensor information or algorithms, in sequential order (each layer takes the Costmap of the previous layer as input, applies its changes, and provides it to the next layer).

Similarly, Costmap Filters are plugins that can be added to the Costmap providing the ability to mark certain areas with additional features or behavioral changes for the robot.

../_images/layered_costmap_w_filters.svg

The Costmap Filters plugins available in Nav2 (and that you can use for your Robot) are:

Keepout Filter that forces the robot to avoid certain areas or stay on preferred lanes.
Speed Filter that restricts the maximum velocity of the robot.
Binary Filter that toggles a certain action of the robot.

Map Masks#

To use Costmap Filters, one should create a new Map Mask using any Image Editor (ex. Gimp) and provide this Map Mask (a nav_msgs/msg/OccupancyGrid message) to the corresponding Costmap Filter Plugin, through a specified ROS 2 Topic. For more information on how to create a Map Mask using this manual approach, check this video.

PAL Robots are equipped with a more flexible way of creating Map Mask using an intuitive RViz Plugin which allows users to easily annotate a map and automatically have their changes reflected in the corresponding Costmap Filters.

Apart from offering a nice GUI, this PAL feature also offers a plugin-based architecture that allows users to programmatically create their own Map Masks and easily integrate them with the available Costmap Filters. By creating a new Map Mask Plugin, users can define new logics to annotate their maps and having it integrated with the Costmap Filters.

The custom Map Mask plugins available on our Robots are:

pal_map_masks::KeepoutMapMask that allows you to draw forbidden areas and preferred lanes for autonomous navigation.
pal_map_masks::SpeedMapMask that allows you to draw limited velocity areas for autonomous navigation.

Usage#

To add new Costmap Filters among the ones available in Nav2 Costmap Filters or change some of the parameters of the existing ones, it is necessary to edit the corresponding Costmap configuration file, and add it to the list of filters.

For example, to add a new Costmap Filter to the Local Costmap, you need to create a new YAML configuration file with the command:

mkdir -p ~/.pal/pal_navigation_cfg_params/params/nav2_controller && touch ~/.pal/pal_navigation_cfg_params/params/nav2_controller/my_local_costmap.yaml

Note

For the Global Costmap you need to insert your configuration file in a different folder structure that you can create with the command:

mkdir -p ~/.pal/pal_navigation_cfg_params/params/nav2_planner && touch ~/.pal/pal_navigation_cfg_params/params/nav2_planner/my_global_costmap.yaml

Then modify the newly created configuration file

vim ~/.pal/pal_navigation_cfg_params/params/nav2_controller/my_local_costmap.yaml

And add a new entry to the list of filters:

pal_navigation_cfg:
  ros__parameters:
    supported_robots:
    - some_pal_robot # e.g. tiago, omni_base, tiago_pro, etc.

local_costmap:  # or global_costmap
  ros__parameters:
      # Other Local Costmap Parameters

      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]  # costmap layers
      filters: ["keepout_filter", "inflation_layer", "speed_filter"]  # costmap filters

      # Plugins Configuration
      # Filters Configuration

Note

The order in which layers and filters are defined in the configuration file matters. The Costmap will apply the Keepout Filter first, then the Inflation Layer, and finally the Speed Filter. This means that the inflation algorithm will only be applied to the resulting Mask generated by the Keepout Filter, and will NOT inflate the areas marked by the Speed Filter.

To configure the remaining Costmap parameters, you can refer to the Costmap configuration file.

Otherwise, if you want to configure the parameters of a specific Costmap Filter Plugin, you can add it into the corresponding section of the configuration file:

pal_navigation_cfg:
  ros__parameters:
    supported_robots:
    - some_pal_robot # e.g. tiago, omni_base, tiago_pro, etc.

local_costmap:  # or global_costmap
  ros__parameters:
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]  # costmap layers
      filters: ["keepout_filter", "inflation_layer", "speed_filter"]  # costmap filters

      # Plugins Configuration

      keepout_filter:
          plugin: "nav2_costmap_2d::KeepoutFilter"
          # Configuration parameters for the keepout filter

      speed_filter:
          plugin: "nav2_costmap_2d::SpeedFilter"
          # Configuration parameters for the speed filter

      inflation_layer:
          plugin: "nav2_costmap_2d::InflationLayer"
          # Configuration parameters for the inflation layer

For the list of parameters of the different Costamp Filters, their meaning and their type, check the documentation of the specific Costmap Filter:

Once created, to start using your custom Costmap configuration, you need to set it in the navigation pipeline by modifying the <robot>_nav.yaml file in the <robot>_2dnav package.

sudo vi /opt/pal/$PAL_DISTRO/share/<robot>_2dnav/params/<robot>_nav.yaml

Attention

Make sure to replace the <robot> placeholder with the name of your robot (e.g. tiago, omni_base, tiago_pro, etc.).

Then, change the params field of the controller_server to the name of your custom Costmap configuration file.

nodes:
   # Other nodes
   controller_server:
     app: controller_server
     params: [dwb_w_progress_checker_and_goal_checker, my_local_costmap]

Attention

Make sure that the name of the custom Costmap configuration file you use in the params field (my_local_costmap) is the same as the name of the file you created in the .pal folder (my_local_costmap.yaml).

Finally, to apply the changes and start using the new Costmap configuration, you need to restart the navigation pipeline with the command:

pal module restart navigation

🌐 Costmap Filters#

Map Masks#

Usage#

See also#