|
|
|
|
|
|
🧩 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 |
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 |
Yes |
process() |
Method that develops the algorithm that defines how to use that map’s
information. It Fills the Costmap2D with calculated data and makes an action
based on processed data.
This method takes 6 input parameters. A |
Yes |
resetFilter() |
Method is called when 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 first create the proper folder structure and then 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 use
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
Add a new entry to the filters list and configure it:
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 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
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 |
Method description |
Requires override? |
configure() |
Method that is used to configure the Map Mask plugin. Here is where you can
declare and read the node’s parameters, initialize ROS 2 interfaces
and so on. This method takes a |
Yes |
cleanup() |
Method that is called when the |
Yes |
activate() |
Method that is called when the |
Yes |
deactivate() |
Method that is called when the |
Yes |
generateMask() |
Method that develops the algorithm that defines how the map mask is created. It can implement any arbitrary logic that creates an occupancy grid that will be used as a Mask for a certain 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 can use the default configuration file provided by the PAL Robotics
mkdir -p ~/.pal/pal_navigation_cfg_params/params/pal_map_masks && cp /opt/pal/$PAL_DISTRO/share/pal_map_masks/params/default.yaml ~/.pal/pal_navigation_cfg_params/params/pal_map_masks/my_map_mask.yaml
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 configuration, you need to set it in the navigation pipeline by
modifying the <robot>_advanced_nav.yaml file in the <robot>_advanced_2dnav package.
sudo vi /opt/pal/$PAL_DISTRO/share/<robot>_advanced_2dnav/params/<robot>_advanced_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 map_mask_server to the name of your custom Costmap configuration
file.
nodes:
# Other nodes
map_mask_server:
app: map_mask_server
params: [my_map_mask]
Attention
Make sure that the name of the custom Costmap configuration file you use in the params field
(my_map_mask) is the same as the name of the file you created in the .pal folder
(my_map_mask.yaml).
Finally, to apply the changes and start using the new Costmap configuration, you need to restart the advanced navigation pipeline with the command:
pal module restart advanced_navigation
This will automatically create and advertise a new ROS 2 topic called /dummy_map_mask/mask with a striped mask.
