../_images/tiagopro-icon.png ../_images/kangaroo-icon.png ../_images/tiago-icon.png ../_images/ari-icon.png ../_images/talos-icon.png ../_images/mobile-bases-icon.png

๐Ÿงฉ Costmap Filters Plugins#

In this section, you are going to learn how to create a Costmap Filter plugin and a Map Mask plugin. Before diving into the code, letโ€™s first clarify the difference between these two concepts and how you can use them to improve the navigation capabilities of your robot.

A Costmap Filter Plugin defines WHAT the robot should do with the environmental annotations it is given. For example, a Costmap Filter Plugin can be used to publish on a certain ROS 2 topic or call a certain ROS 2 action when the robot is in a certain area of the environment.

A Map Mask Plugin is only available for PAL Robots and defines WHERE and HOW the environmental annotations should be placed. For example, a Map Mask Plugin can be used to draw a certain shape on the map using some vertexes that were stored in a Database.

How to create your Costmap Filter Plugin#

In this tutorial, you are going to learn how to create a Costmap Filter Plugin that will publish a log message when the robot is in a certain area of the environment, whose cost is bigger than a certain threshold. The steps for creating a Costmap Filter Plugin are very similar to the ones for the creation of a Costmap Layer Plugin. For this reason, before starting this tutorial, make sure you are already familiar with the creation of a Costmap Layer Plugin.

For this demonstration, we are going to use a new ROS 2 package called costmap_filters_tutorial, in which we are going to create a new C++ source file called dummy_costmap_filter.cpp.

#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"

#include "nav2_util/occ_grid_values.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"

#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav2_msgs/msg/costmap_filter_info.hpp"

namespace costmap_filters_tutorial
{
/**
 * @class DummyCostmapFilter
 * @brief Reads a map mask and prints a log message
 * when the robot is in a cell with a value higher than a threshold
 */
class DummyCostmapFilter : public nav2_costmap_2d::CostmapFilter
{
public:
  /**
   * @brief DummyCostmapFilter plugin of type CostmapFilter
   */
  DummyCostmapFilter()
  : filter_info_sub_(nullptr), mask_sub_(nullptr),
    filter_mask_(nullptr), global_frame_("")
  {
  }

  /**
   * @brief Initialize the filter and subscribe to the info topic
   */
  void initializeFilter(
    const std::string & filter_info_topic)
  {
    std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

    rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
    if (!node) {
      throw std::runtime_error{"Failed to lock node"};
    }

    // Declare parameters specific to DummyCostmapFilter only
    declareParameter("mask_threshold", rclcpp::ParameterValue(50.0));
    node->get_parameter(name_ + "." + "mask_threshold", mask_threshold);

    filter_info_topic_ = filter_info_topic;
    // Setting new costmap filter info subscriber
    RCLCPP_INFO(
      logger_,
      "DummyCostmapFilter: Subscribing to \"%s\" topic for filter info...",
      filter_info_topic_.c_str());
    filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
      filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
      std::bind(&DummyCostmapFilter::filterInfoCallback, this, std::placeholders::_1));

    // Get global frame required for binary state publisher
    global_frame_ = layered_costmap_->getGlobalFrameID();
  }

  /**
   * @brief Process the given map mask at the current pose
   */
  void process(
    nav2_costmap_2d::Costmap2D &,
    int, int, int, int,
    const geometry_msgs::msg::Pose2D & pose)
  {
    std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

    if (!filter_mask_) {
      // Show warning message every 2 seconds to not litter an output
      RCLCPP_WARN_THROTTLE(
        logger_, *(clock_), 2000,
        "DummyCostmapFilter: Filter mask was not received");
      return;
    }

    geometry_msgs::msg::Pose2D mask_pose;  // robot coordinates in mask frame

    // Transforming robot pose from current layer frame to mask frame
    if (!transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
      return;
    }

    // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j)
    unsigned int mask_robot_i, mask_robot_j;
    if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) {
      // Robot went out of mask range. Set "false" state by-default
      RCLCPP_WARN(
        logger_,
        "DummyCostmapFilter: Robot is outside of filter mask. Resetting binary state to default.");
      return;
    }

    // Getting filter_mask data from cell where the robot placed
    int8_t mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j);
    if (mask_data == nav2_util::OCC_GRID_UNKNOWN) {
      // Corresponding filter mask cell is unknown.
      // Warn and do nothing.
      RCLCPP_WARN_THROTTLE(
        logger_, *(clock_), 2000,
        "DummyCostmapFilter: Filter mask [%i, %i] data is unknown. Do nothing.",
        mask_robot_i, mask_robot_j);
      return;
    }
    // Check mask threshold
    if (base_ + mask_data * multiplier_ > mask_threshold) {
      RCLCPP_INFO(logger_, "Robot is Inside the mask");
    } else {
      RCLCPP_INFO(logger_, "Robot is Outside the mask");
    }
  }

  /**
   * @brief Reset the costmap filter / topic / info
   */
  void resetFilter()
  {
    std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

    filter_info_sub_.reset();
    mask_sub_.reset();
  }

  /**
   * @brief If this filter is active
   */
  bool isActive()
  {
    std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

    if (filter_mask_) {
      return true;
    }
    return false;
  }

private:
  /**
   * @brief Callback for the filter information
   */
  void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg)
  {
    std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

    rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
    if (!node) {
      throw std::runtime_error{"Failed to lock node"};
    }

    if (!mask_sub_) {
      RCLCPP_INFO(
        logger_,
        "DummyCostmapFilter: Received filter info from %s topic.", filter_info_topic_.c_str());
    } else {
      RCLCPP_WARN(
        logger_,
        "DummyCostmapFilter: New costmap filter info from %s topic. Updating old filter info.",
        filter_info_topic_.c_str());
      // Resetting previous subscriber each time when new costmap filter information arrives
      mask_sub_.reset();
    }

    // Set base_ and multiplier_
    base_ = msg->base;
    multiplier_ = msg->multiplier;
    // Set topic name to receive filter mask from
    mask_topic_ = msg->filter_mask_topic;

    // Setting new filter mask subscriber
    RCLCPP_INFO(
      logger_,
      "DummyCostmapFilter: Subscribing to \"%s\" topic for filter mask...",
      mask_topic_.c_str());
    mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
      mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
      std::bind(&DummyCostmapFilter::maskCallback, this, std::placeholders::_1));
  }

  /**
   * @brief Callback for the filter mask
   */
  void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
  {
    std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());

    if (!filter_mask_) {
      RCLCPP_INFO(
        logger_,
        "DummyCostmapFilter: Received filter mask from %s topic.", mask_topic_.c_str());
    } else {
      RCLCPP_WARN(
        logger_,
        "DummyCostmapFilter: New filter mask arrived from %s topic. Updating old filter mask.",
        mask_topic_.c_str());
      filter_mask_.reset();
    }

    filter_mask_ = msg;
  }

  // Working with filter info and mask
  rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;
  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_sub_;

  nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_;

  std::string global_frame_;  // Frame of currnet layer (master_grid)

  double base_, multiplier_;
  double mask_threshold;
};

}  // namespace costmap_filters_tutorial

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(costmap_filters_tutorial::DummyCostmapFilter, nav2_costmap_2d::Layer)

Examine the code#

The example plugin inherits from the nav2_costmap_2d::CostmapFilter class. This base class provides three virtual functions that need to be implemented by the plugin class.

Virtual method

Description

Requires override?

initializeFilter()

Method that is called when the Costmap Filter is initialized. It should create subscriptions to filter-related topics and declare and retrieve all the nodeโ€™s parameters to configure its functionalities. This method takes a std::string as input with the name of the Costmap Filter info topic.

Yes

process()

Method that develops the algorithm that defines how to use the mapโ€™s information. It fills the Costmap2D with calculated data and makes an action based on processed data. This method takes six input parameters: a nav2_costmap_2d::Costmap2D referring to the master 2D Costmap with applied masks, the minimum and maximum map boundaries, and the robotโ€™s current pose.

Yes

resetFilter()

Method that is called when the Costmap Filter is reset. It clears all the data and stops all subscriptions.

Yes

The initializeFilter() function is used to declare and read a plugin parameter called mask_threshold

declareParameter("mask_threshold", rclcpp::ParameterValue(50.0));
node->get_parameter(name_ + "." + "mask_threshold", mask_threshold);

This parameter indicates the minimum value that a cell in the mask should have in order to trigger the log message. In other words, if the robot is in a cell with a value higher than the threshold, the plugin will publish a log message saying that the robot is inside the mask.

The process() function is where the real logic of the plugin is implemented. It uses many utility functions declared in the nav2_costmap_2d::CostmapFilter base class to get the current robot pose, transform it to the mask frame, and get the mask value of the cell corresponding to the current robot pose.

geometry_msgs::msg::Pose2D mask_pose;  // robot coordinates in mask frame

// Transforming robot pose from current layer frame to mask frame
if (!transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
    return;
}

// Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j)
unsigned int mask_robot_i, mask_robot_j;
if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) {
    // Robot went out of mask range. Set "false" state by-default
    RCLCPP_WARN(
    logger_,
    "DummyCostmapFilter: Robot is outside of filter mask. Resetting binary state to default.");
    return;
}

// Getting filter_mask data from cell where the robot is placed
int8_t mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j);

Then, it uses this information to check whether the mask value of the cell corresponding to the current robot pose is higher than the threshold and publishes the log message.

if (base_ + mask_data * multiplier_ > mask_threshold) {
    RCLCPP_INFO(logger_, "Robot is Inside the mask");
} else {
    RCLCPP_INFO(logger_, "Robot is Outside the mask");
}

Compile and Install#

To compile and install the plugin, the file CMakeLists.txt should declare the following (minimum) dependencies:

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(pluginlib REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(nav2_util REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(geometry_msgs REQUIRED)

set(dependencies
    rclcpp
    rclcpp_lifecycle
    pluginlib
    nav_msgs
    nav2_msgs
    nav2_util
    nav2_costmap_2d
    geometry_msgs
)

To compile and install the plugin, you need to add and install a new library

add_library(dummy_costmap_filter SHARED src/dummy_costmap_filter.cpp)
ament_target_dependencies(dummy_costmap_filter SYSTEM ${dependencies})

install(TARGETS dummy_costmap_filter
    ARCHIVE DESTINATION lib
    LIBRARY DESTINATION lib
    RUNTIME DESTINATION bin
)

Finally, you need to export the newly created library and its dependencies

ament_export_libraries(dummy_costmap_filter)
ament_export_dependencies(${dependencies})
pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_filter_plugins.xml)
ament_package()

Also, it should be created a new XML file called costmap_filter_plugins.xml within the costmap_filters_tutorial package with the declaration of the plugin.

<class_libraries>
    <library path="dummy_costmap_filter">
    <class type="costmap_filters_tutorial::DummyCostmapFilter"   base_class_type="nav2_costmap_2d::Layer">
        <description>A Dummy Costmap Filter</description>
    </class>
    </library>
</class_libraries>

Finally, the package.xml file should include the required dependencies:

<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>pluginlib</depend>
<depend>nav_msgs</depend>
<depend>nav2_msgs</depend>
<depend>nav2_util</depend>
<depend>nav2_costmap_2d</depend>
<depend>geometry_msgs</depend>

And should export the plugin:

<export>
    <build_type>ament_cmake</build_type>
    <costmap_2d plugin="${prefix}/costmap_filter_plugins.xml" />
</export>

Usage#

To start using your new plugin, add it to the Costmap configuration file (Local Costmap or Global Costmap depending on your needs).

For example, to add a new Costmap Filter to the Local Costmap, you need to create a new configuration file. So you first need to create a new directory in the .pal folder of your robot.

mkdir -p ~/.pal/config

And then, within this folder, you can create a new local_costmap configuration file.

touch ~/.pal/config/99_my_local_costmap_config.yaml

Attention

The name of the file should start with a number to ensure that it is loaded last and overrides the default PAL configurations.

Within the newly created 99_my_local_costmap_config.yaml file, you can insert your custom local_costmap parameters. For the list of available parameters, their meaning and how they affect the local costmap, you can refer to the local_costmap configuration guide. When creating a new Navigation configuration file, you need to specify the node name it refers to, in this case, local_costmap.

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

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

      # Plugins Configuration
      dummy_filter:
          plugin: "costmap_filters_tutorial::DummyCostmapFilter"
          enabled: True
          filter_info_topic: "/dummy_map_mask/mask_info"
          mask_threshold: 50.0

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

Once created, to start using your custom local_costmap configuration, you need to restart the navigation module with the command:

pal module restart navigation

Note

This change is is persistent and will be loaded every time you start the navigation module. If you want to revert to the default PAL configuration, you can simply delete the custom configuration file you created in the ~/.pal/config folder.

Finally, to create your own Map Mask and host it, please refer to the steps described in the Keepout Filter DEMO.

Note

Mind to change the filter_info_topic parameter to the topic to the /dummy_map_mask/mask_info and the mask_topic to the /dummy_map_mask/mask.

How to create your Map Mask Plugin#

In this tutorial, you are going to learn how to create a Map Mask Plugin that will create a striped mask (a nav_msgs/msg/OccupancyGrid message)

For this demonstration, we are going to use a new ROS 2 package called costmap_filters_tutorial, in which we are going to create a new C++ source file called dummy_map_mask.cpp.

#include <string>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav2_msgs/msg/costmap_filter_info.hpp"
#include "pal_map_masks/core/map_mask.hpp"
#include "nav2_util/node_utils.hpp"


namespace costmap_filters_tutorial
{
class DummyMapMask : public pal_map_masks::MapMask
{
public:
  /**
   * @brief DummyMapMask plugin of type MapMask
  */
  DummyMapMask()
  : mask_topic_("/dummy_mask") {}

  /**
   * @brief DummyMapMask destructor
  */
  ~DummyMapMask() = default;

  /**
   * @brief Overridden method to configure DummyMapMask plugin.
   * @param parent pointer to user's node
   * @param plugin_name name of the plugin assigned at runtime
  */
  void configure(
    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
    const std::string & plugin_name) override
  {
    plugin_name_ = plugin_name;
    auto node = parent.lock();
    if (!node) {
      throw std::runtime_error("Unable to Lock Parent Node");
    }
    node_ = parent;

    nav2_util::declare_parameter_if_not_declared(
      node,
      plugin_name + ".mask_topic",
      rclcpp::ParameterValue(std::string()));

    node->get_parameter(
      plugin_name + ".mask_topic",
      mask_topic_);

    costmap_filter_info_publisher_ = node->create_publisher<nav2_msgs::msg::CostmapFilterInfo>(
      mask_topic_ + "_info",
      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());

    // Constant values for dummy mask topic
    costmap_filter_info_msg_.filter_mask_topic = mask_topic_;
    costmap_filter_info_msg_.type = 0;
    costmap_filter_info_msg_.base = 0.0;
    costmap_filter_info_msg_.multiplier = 1.0;
    mask_publisher_ = node->create_publisher<nav_msgs::msg::OccupancyGrid>(
      costmap_filter_info_msg_.filter_mask_topic,
      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
  }

  /**
   * @brief Overridden method to cleanup DummyMapMask plugin.
   */
  void cleanup() override
  {
    RCLCPP_INFO(logger_, "Cleaning DummyMapMask plugin");
    costmap_filter_info_publisher_.reset();
    mask_publisher_.reset();
  }

  /**
   * @brief Overridden method to activate DummyMapMask plugin.
   */
  void activate() override
  {
    RCLCPP_INFO(logger_, "Activating DummyMapMask plugin");
  }

  /**
   * @brief Overridden method to deactivate DummyMapMask plugin.
   */
  void deactivate() override
  {
    RCLCPP_INFO(logger_, "Deactivating DummyMapMask plugin");
  }

  /**
   * @brief Overridden method to implement logic for the Mask generation of DummyMapMask plugin.
   */
  bool generateMask() override
  {
    auto locked_node = node_.lock();
    if (!locked_node) {
      throw std::runtime_error("Unable to Lock Parent Node");
    }
    costmap_filter_info_msg_.header.frame_id = header_.frame_id;
    costmap_filter_info_msg_.header.stamp = locked_node->now();
    costmap_filter_info_publisher_->publish(costmap_filter_info_msg_);

    auto occupancy_grid = std::make_shared<nav_msgs::msg::OccupancyGrid>();
    occupancy_grid->header = header_;
    occupancy_grid->info = info_;

    occupancy_grid->data = std::vector<int8_t>(info_.width * info_.height, 0);
    for (size_t i = 0; i < occupancy_grid->data.size(); i++) {
      if (i % 2 == 0) {
        occupancy_grid->data.at(i) = 100;
      }
    }

    mask_publisher_->publish(*occupancy_grid);
    return true;
  }

private:
  // Interfaces
  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
  rclcpp::Logger logger_ {rclcpp::get_logger("DummyMapMask")};

  // ROS2 service interfaces
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr costmap_filter_info_publisher_;
  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_publisher_;

  // Info msg
  nav2_msgs::msg::CostmapFilterInfo costmap_filter_info_msg_;

  // Parameters
  std::string mask_topic_;
  std::string plugin_name_;
};
}  // namespace costmap_filters_tutorial

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
  costmap_filters_tutorial::DummyMapMask,
  pal_map_masks::MapMask)

Examine the code#

The example plugin inherits from the pal_map_masks::MapMask class. This base class provides five virtual functions that need to be implemented by the plugin class.

Virtual method

Description

Requires override?

configure()

Method that is used to configure the Map Mask plugin. This is where you can declare and read the nodeโ€™s parameters, initialize ROS 2 interfaces, and perform other setup tasks. This method takes a std::weak_ptr to the parent rclcpp_lifecycle::LifecycleNode and a std::string with the selected plugin name.

Yes

cleanup()

Method that is called when the on_cleanup() function is executed on the parent rclcpp_lifecycle::LifecycleNode. It clears all the data and stops all subscriptions.

Yes

activate()

Method that is called when the on_activate() function is executed on the parent rclcpp_lifecycle::LifecycleNode. It activates all the interfaces.

Yes

deactivate()

Method that is called when the on_deactivate() function is executed on the parent rclcpp_lifecycle::LifecycleNode. It deactivates all the interfaces.

Yes

generateMask()

Method that develops the algorithm for creating the map mask. It can implement any arbitrary logic that generates an occupancy grid, which will be used as a mask for a specific Costmap Filter.

Yes

The configure() function is used to declare and read a plugin parameter called mask_topic

nav2_util::declare_parameter_if_not_declared(
    node,
    plugin_name + ".mask_topic",
    rclcpp::ParameterValue(std::string()));

node->get_parameter(
    plugin_name + ".mask_topic",
    mask_topic_);

This parameter indicates the topic where the plugin will publish the nav_msgs/msg/OccupancyGrid message with the created mask. Finally, the generateMask() function is where the real logic of the plugin is implemented that for this example, simply creates a striped Occupancy Grid mask.

auto occupancy_grid = std::make_shared<nav_msgs::msg::OccupancyGrid>();
occupancy_grid->header = header_;
occupancy_grid->info = info_;

occupancy_grid->data = std::vector<int8_t>(info_.width * info_.height, 0);
for (size_t i = 0; i < occupancy_grid->data.size(); i++) {
    if (i % 2 == 0) {
    occupancy_grid->data.at(i) = 100;
    }
}

mask_publisher_->publish(*occupancy_grid);

Compile and Install#

To compile and install the plugin, the file CMakeLists.txt should declare the following (minimum) dependencies:

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(pluginlib REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(pal_map_masks REQUIRED)
find_package(nav2_util REQUIRED)

set(dependencies
    rclcpp
    rclcpp_lifecycle
    pluginlib
    nav_msgs
    nav2_msgs
    nav2_util
    pal_map_masks
)

To compile and install the plugin, you need to add and install a new library

add_library(dummy_map_mask SHARED src/dummy_map_mask.cpp)
ament_target_dependencies(dummy_map_mask SYSTEM ${dependencies})

install(TARGETS dummy_map_mask
    ARCHIVE DESTINATION lib
    LIBRARY DESTINATION lib
    RUNTIME DESTINATION bin
)

Finally, you need to export the newly created library and its dependencies

ament_export_libraries(dummy_map_mask)
ament_export_dependencies(${dependencies})
pluginlib_export_plugin_description_file(pal_map_masks map_mask_plugins.xml)
ament_package()

Also, it should be created a new XML file called map_mask_plugins.xml within the costmap_filters_tutorial package with the declaration of the plugin.

<class_libraries>
    <library path="dummy_map_mask">
    <class type="costmap_filters_tutorial::DummyMapMask" base_class_type="pal_map_masks::MapMask">
        <description>A Dummy Map Mask</description>
    </class>
    </library>
</class_libraries>

Finally, the package.xml file should include the required dependencies:

<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>pluginlib</depend>
<depend>nav_msgs</depend>
<depend>nav2_msgs</depend>
<depend>nav2_util</depend>
<depend>pal_map_masks</depend>

And should export the plugin:

<export>
    <build_type>ament_cmake</build_type>
    <costmap_filters_tutorial plugin="${prefix}/map_mask_plugins.xml" />
</export>

Usage#

To start using your new plugin, you need to create a new map_mask_server configuration file. Instead of starting from scratch, you first need to create a new directory in the .pal folder of your robot.

mkdir -p ~/.pal/config

And then, within this folder, you can create a new map_mask_server configuration file.

touch ~/.pal/config/99_my_map_mask_server_config.yaml

Attention

The name of the file should start with a number to ensure that it is loaded last and overrides the default PAL configurations.

Within the newly created 99_my_map_mask_server_config.yaml file, you can insert your custom map_mask_server parameters. For the list of available parameters, their meaning and how they affect the AMCL algorithm, you can refer to ๐Ÿ”Œ Cosmap Filters API. When creating a new Navigation configuration file, you need to specify the node name it refers to, in this case, map_mask_server. and add a new entry to the list of mask_plugins and configure it

/map_mask_server:
  ros__parameters:
      use_sim_time: ${use_sim_time}
      mask_plugins: ["keepout_filter", "speed_filter", "dummy_filter"]

      # Other filters params

      dummy_filter:
          plugin: "costmap_filters_tutorial::DummyMapMask"
          mask_topic: "/dummy_map_mask/mask"

Once created, to start using your custom map_mask_server configuration, you need to restart the advanced_navigation module with the command:

pal module restart advanced_navigation

Note

This change is is persistent and will be loaded every time you start the advanced_navigation module. If you want to revert to the default PAL configuration, you can simply delete the custom configuration file you created in the ~/.pal/config folder.

This will automatically create and advertise a new ROS 2 topic called /dummy_map_mask/mask with a striped mask.

../_images/map_mask_plugin.svg