../_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

πŸ”Œ Cosmap Filters API#

ROS 2 API#

To interact with the Costmap Filters, you can refer to the following ROS 2 APIs.

The map_mask_server, global_costmap, and local_costmap nodes implement the Costmap Filters functionalities. To configure the global_costmap and local_costmap, please refer to the Costmap configuration guide.

To configure the map_mask_server you can refer to the following table containing the node’s parameters and their explanation:

Parameter

Description

use_sim_time

Whether to use simulation time or not.

mask_plugins

List of plugins to load.

To configure the pal_map_masks::KeepoutMapMask you can refer to the following table containing the plugin’s parameters:

Parameter

Description

plugin

Name of the plugin to load (pal_map_masks::KeepoutMapMask).

mask_topic

ROS 2 Topic in which it will publish the generated Map Mask.

highways.mask_value

Value that will be used to replace the cells within the Highways Lanes (between 0.0 and 100.0). It indicates how desired that area is for the navigation (0.0 highly desirable to navigate in that area, 100.0 navigation forbidden).

highways.lift_cost

Value that will be used to replace the cells outside the Highways Lanes (between 0.0 and 100.0). It indicates how desired that area is for the navigation (0.0 highly desirable that the robot moves outside the Highways, 100.0 robot can only move within the highways).

virtual_obstacles.mask_value

Value that will be used to replace the cells within the Virtual Obstacle Zones (between 0.0 and 100.0). It indicates how desired that area is for the navigation (0.0 highly desirable to navigate in that area, 100.0 navigation forbidden).

To configure the pal_map_masks::SpeedMapMask you can refer to the following table containing the plugin’s parameters:

Parameter

Description

plugin

Name of the plugin to load (pal_map_masks::SpeedMapMask).

mask_topic

ROS 2 Topic in which it will publish the generated Map Mask.

mask_value

Value that will be used to replace the cells within the Speed Area (between 0.0 and 100.0). It indicates the desired speed percentage (0.0 robot not moving, 100.0 robot moving at nominal velocity).

C++ API#

The pal_map_masks::MapMask class provides an interface for creating and managing masks using occupancy grid data. It also offers several utility functions to draw polygons and lanes on the mask. For more information on how to create a new Map Mask Plugin, you can refer to 🧩 Costmap Filters Plugins.

Function

Description

virtual void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) = 0;

Configure the plugin with the provided node and plugin name.

  • parent: Pointer to the user’s node.

  • plugin_name: Name of the plugin assigned at runtime.

virtual void cleanup() = 0;

Cleanup the plugin. This method should be overridden to implement custom cleanup logic.

virtual void activate() = 0;

Activate the plugin. This method should be overridden to implement custom activation logic.

virtual void deactivate() = 0;

Deactivate the plugin. This method should be overridden to implement custom deactivation logic.

virtual bool generateMask() = 0;

Generate the mask. This method should be overridden to implement custom mask generation logic.

  • return: True if the mask generation was successful, false otherwise.

void reset();

Restore the mask using the last received nav_msgs::msg::OccupancyGrid.

void setOccupancyGrid(const nav_msgs::msg::OccupancyGrid & occupancy_grid);

Initialize the pal_map_masks::MapMask from an OccupancyGrid.

  • occupancy_grid: nav_msgs::msg::OccupancyGrid to use for the initialization of the mask.

bool isInitialized();

Check whether the first nav_msgs::msg::OccupancyGrid map has been received and used for the initialization of the map.

  • return: True if the map has been initialized, false otherwise.

nav_msgs::msg::OccupancyGrid::ConstSharedPtr toOccupancyGrid() const;

Convert the pal_map_masks::MapMask into an nav_msgs::msg::OccupancyGrid.

  • return: A constant shared pointer to an nav_msgs::msg::OccupancyGrid.

void replaceGridCellValues(int free, int occupied, int unknown = GridValues::Unknown);

Replace grid cell values in the mask with new specified values.

  • free: Value to replace GridValues::Free.

  • occupied: Value to replace GridValues::Occupied.

  • unknown: Value to replace GridValues::Unknown.

void drawPolygon(const std::vector<cv::Point2d> & points, double mask_value);

Draw polygons on the mask using the provided points and mask value.

  • points: Polygon points where each polygon is a vector of cv::Point2d.

  • mask_value: Value to replace the mask cells within the given polygons.

void drawLane(const std::pair<double, std::vector<cv::Point2d>> & points, double mask_value);

Draw lanes on the mask using the provided points, mask value, and lane width.

  • points: Lane points where each lane is a vector of cv::Point2d.

  • mask_value: Value to replace the mask cells within the given lanes.

inline void transformToPoints(const std::vector<tf2::Transform> & transforms, std::vector<cv::Point2d> & points);

Convert from a vector of tf2::Transform to a vector of cv::Point2d.

  • transforms: std::vector<tf2::Transform> to convert from.

  • points: std::vector<cv::Point2d> resulting from the conversion.

inline void transformToFrame(const std::vector<tf2::Transform> & in, std::vector<tf2::Transform> & out, const std::string & from_frame, const std::string & to_frame, std::shared_ptr<tf2_ros::Buffer> tf_buffer);

Transform a vector of tf2::Transform to a different frame.

  • in: Vector of input transforms.

  • out: Vector of output transforms.

  • from_frame: Source frame.

  • to_frame: Target frame.

  • tf_buffer: Shared pointer to a tf2_ros::Buffer.