add grid_map_msgs/GridMap topic type

Author: umiklos

CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(grid_map_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_msgs REQUIRED)
@@ -36,6 +37,7 @@ set(INCLUDE_DIRS
   ${std_msgs_INCLUDE_DIRS}
   ${sensor_msgs_INCLUDE_DIRS}
   ${nav_msgs_INCLUDE_DIRS}
+  ${grid_map_msgs_INCLUDE_DIRS}
   ${rclcpp_components_INCLUDE_DIRS}
 )
 include_directories(${INCLUDE_DIRS})
@@ -46,6 +48,7 @@ set(ament_dependencies
   std_msgs
   sensor_msgs
   nav_msgs
+  grid_map_msgs
   visualization_msgs
   tf2
   tf2_msgs

README.md

@@ -1,5 +1,5 @@
 # `pointcloud_to_grid` ROS 2 package
-This package converts `sensor_msgs/PointCloud2` LIDAR data to `nav_msgs/OccupancyGrid` 2D map data based on intensity and / or height.
+This package converts `sensor_msgs/PointCloud2` LIDAR data to both `nav_msgs/OccupancyGrid` and `grid_map_msgs/GridMap` 2D map data based on intensity and / or height.
 ![](doc/grid_map01.gif)
 
 [![Static Badge](https://img.shields.io/badge/ROS_2-Humble-34aec5)](https://docs.ros.org/en/humble/)
@@ -17,7 +17,8 @@ Don't foget to `source ~/ros2_ws/install/setup.bash`.
 
 
 ## Features
-- Few dependencies (ROS 2 and PCL mainly) [ROS installation](http://wiki.ros.org/ROS/Installation)
+- Few dependencies (ROS 2, PCL, and grid_map_msgs mainly) [ROS installation](http://wiki.ros.org/ROS/Installation)
+- **Dual output format support**: Publishes both `nav_msgs/OccupancyGrid` and `grid_map_msgs/GridMap` messages simultaneously
 - Simple as possible
 - Fast
 
@@ -58,6 +59,42 @@ Start the visualization in a **new terminal** :
 ros2 launch pointcloud_to_grid rviz.launch.py
 ```
 
+## Dual Output Format Support
+
+The package now supports publishing both `nav_msgs/OccupancyGrid` and `grid_map_msgs/GridMap` message types simultaneously. This allows for better integration with different ROS 2 packages that may prefer one format over the other.
+
+### New Parameters
+
+In addition to the existing parameters, the following new parameters control the GridMap output topics:
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `mapi_gridmap_topic_name` | string | `intensity_gridmap` | Topic name for intensity GridMap |
+| `maph_gridmap_topic_name` | string | `height_gridmap` | Topic name for height GridMap |
+
+### Output Topics
+
+The node now publishes to four topics simultaneously:
+
+**OccupancyGrid format:**
+- `intensity_grid` (`nav_msgs/OccupancyGrid`)
+- `height_grid` (`nav_msgs/OccupancyGrid`)
+
+**GridMap format:**
+- `intensity_gridmap` (`grid_map_msgs/GridMap`)
+- `height_gridmap` (`grid_map_msgs/GridMap`)
+
+### Usage Example
+
+```bash
+# Launch with custom GridMap topic names
+ros2 launch pointcloud_to_grid demo.launch.py topic:=my_pointcloud mapi_gridmap_topic_name:=my_intensity_map maph_gridmap_topic_name:=my_height_map
+```
+
+## QoS Configuration
+
+The package is configured to use `BEST_EFFORT` reliability QoS policy for the input point cloud subscription. This ensures compatibility with typical LiDAR sensor publishers that often use this policy for performance reasons. This prevents QoS compatibility warnings that might appear with the default `RELIABLE` policy.
+
 
 ## Related solutions
 - [github.com/ANYbotics/grid_map](https://github.com/ANYbotics/grid_map) - This is a C++ library with ROS interface to manage two-dimensional grid maps with multiple data layers. 

include/pointcloud_to_grid/pointcloud_to_grid_core.hpp

@@ -1,6 +1,7 @@
 #pragma once
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
+#include "grid_map_msgs/msg/grid_map.hpp"
 #include <memory>
 class PointXY
 {
@@ -30,6 +31,8 @@ class GridMap
   std::string frame_out;
   std::string mapi_topic_name;
   std::string maph_topic_name;
+  std::string mapi_gridmap_topic_name;
+  std::string maph_gridmap_topic_name;
   float topleft_x;
   float topleft_y;
   float bottomright_x;
@@ -56,6 +59,38 @@ class GridMap
     // resolution/grid size [m/cell]
   }
 
+  void initGridMap(std::shared_ptr<grid_map_msgs::msg::GridMap> grid_map_msg, const std::string& layer_name)
+  {
+    grid_map_msg->header.frame_id = GridMap::frame_out;
+    grid_map_msg->info.resolution = cell_size;
+    grid_map_msg->info.length_x = length_x;
+    grid_map_msg->info.length_y = length_y;
+    grid_map_msg->info.pose.position.x = position_x;
+    grid_map_msg->info.pose.position.y = position_y;
+    grid_map_msg->info.pose.position.z = 0.0;
+    grid_map_msg->info.pose.orientation.w = 1.0;
+    grid_map_msg->info.pose.orientation.x = 0.0;
+    grid_map_msg->info.pose.orientation.y = 0.0;
+    grid_map_msg->info.pose.orientation.z = 0.0;
+    
+    // Clear existing layers and add the new one
+    grid_map_msg->layers.clear();
+    grid_map_msg->layers.push_back(layer_name);
+    
+    // Initialize data array
+    grid_map_msg->data.clear();
+    grid_map_msg->data.resize(1); // One layer
+    grid_map_msg->data[0].layout.dim.resize(2);
+    grid_map_msg->data[0].layout.dim[0].label = "column_index";
+    grid_map_msg->data[0].layout.dim[0].size = cell_num_y;
+    grid_map_msg->data[0].layout.dim[0].stride = cell_num_x * cell_num_y;
+    grid_map_msg->data[0].layout.dim[1].label = "row_index";
+    grid_map_msg->data[0].layout.dim[1].size = cell_num_x;
+    grid_map_msg->data[0].layout.dim[1].stride = cell_num_x;
+    grid_map_msg->data[0].layout.data_offset = 0;
+    grid_map_msg->data[0].data.resize(cell_num_x * cell_num_y);
+  }
+
   void paramRefresh()
   {
     topleft_x = position_x + length_x / 2;

launch/demo.launch.py

@@ -24,8 +24,12 @@ def generate_launch_description():
                 {'length_x': 40.0},
                 {'length_y': 60.0},
                 #{'frame_out': 'os1_sensor'},
+                # OccupancyGrid topics
                 {'mapi_topic_name': 'intensity_grid'},
                 {'maph_topic_name': 'height_grid'},
+                # GridMap topics
+                {'mapi_gridmap_topic_name': 'intensity_gridmap'},
+                {'maph_gridmap_topic_name': 'height_gridmap'},
             ]
         )
 

launch/dual_output_example.launch.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'cell_size',
+            default_value='0.5',
+            description='Cell size for the grid map [m/cell]'
+        ),
+        DeclareLaunchArgument(
+            'length_x',
+            default_value='20.0',
+            description='Length in x-direction [m]'
+        ),
+        DeclareLaunchArgument(
+            'length_y',
+            default_value='30.0',
+            description='Length in y-direction [m]'
+        ),
+        DeclareLaunchArgument(
+            'cloud_in_topic',
+            default_value='nonground',
+            description='Input point cloud topic'
+        ),
+        
+        Node(
+            package='pointcloud_to_grid',
+            executable='pointcloud_to_grid_node',
+            name='pointcloud_to_grid_dual',
+            parameters=[{
+                'cell_size': LaunchConfiguration('cell_size'),
+                'length_x': LaunchConfiguration('length_x'),
+                'length_y': LaunchConfiguration('length_y'),
+                'cloud_in_topic': LaunchConfiguration('cloud_in_topic'),
+                # OccupancyGrid topics
+                'mapi_topic_name': 'intensity_grid',
+                'maph_topic_name': 'height_grid',
+                # GridMap topics
+                'mapi_gridmap_topic_name': 'intensity_gridmap',
+                'maph_gridmap_topic_name': 'height_gridmap',
+                'verbose1': True,
+            }],
+            output='screen'
+        )
+    ])

launch/grid_trajectory.launch.py

@@ -24,8 +24,12 @@ def generate_launch_description():
                 {'length_x': 40.0},  # meters
                 {'length_y': 60.0},  # meters
                 #{'frame_out': 'os1_sensor'},
+                # OccupancyGrid topics
                 {'mapi_topic_name': 'intensity_grid'},
                 {'maph_topic_name': 'height_grid'},
+                # GridMap topics
+                {'mapi_gridmap_topic_name': 'intensity_gridmap'},
+                {'maph_gridmap_topic_name': 'height_gridmap'},
                 {'search_length': 4.0}, # meters
                 {'search_range_deg': 80.0}, # degrees
                 {'search_resolution_deg': 0.5}, # degrees

package.xml

@@ -17,6 +17,7 @@
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>visualization_msgs</depend>
+  <depend>grid_map_msgs</depend>
   <depend>tf2_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>

src/grid_trajectory.cpp

@@ -1,6 +1,7 @@
 // ROS
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
+#include "grid_map_msgs/msg/grid_map.hpp"
 #include "sensor_msgs/msg/point_cloud.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
@@ -26,6 +27,8 @@
 // nav_msgs::msg::OccupancyGrid::Ptr height_grid(new nav_msgs::msg::OccupancyGrid);
 auto height_grid = std::make_shared<nav_msgs::msg::OccupancyGrid>();
 auto intensity_grid = std::make_shared<nav_msgs::msg::OccupancyGrid>();
+auto height_gridmap = std::make_shared<grid_map_msgs::msg::GridMap>();
+auto intensity_gridmap = std::make_shared<grid_map_msgs::msg::GridMap>();
 class PointCloudToGrid : public rclcpp::Node
 {
   rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters)
@@ -46,10 +49,24 @@ class PointCloudToGrid : public rclcpp::Node
         grid_map.maph_topic_name = param.as_string();
         pub_hgrid = this->create_publisher<nav_msgs::msg::OccupancyGrid>(grid_map.maph_topic_name, 10);
       }
+      if (param.get_name() == "mapi_gridmap_topic_name")
+      {
+        grid_map.mapi_gridmap_topic_name = param.as_string();
+        pub_igridmap = this->create_publisher<grid_map_msgs::msg::GridMap>(grid_map.mapi_gridmap_topic_name, 10);
+      }
+      if (param.get_name() == "maph_gridmap_topic_name")
+      {
+        grid_map.maph_gridmap_topic_name = param.as_string();
+        pub_hgridmap = this->create_publisher<grid_map_msgs::msg::GridMap>(grid_map.maph_gridmap_topic_name, 10);
+      }
       if (param.get_name() == "cloud_in_topic")
       {
         cloud_in_topic = param.as_string();
-        sub_pc2_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(cloud_in_topic, 10, std::bind(&PointCloudToGrid::lidar_callback, this, std::placeholders::_1));
+        // Configure QoS for sensor data - use BEST_EFFORT reliability to match typical sensor publishers
+        auto sensor_qos = rclcpp::QoS(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
+        sub_pc2_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+            cloud_in_topic, sensor_qos, 
+            std::bind(&PointCloudToGrid::lidar_callback, this, std::placeholders::_1));
       }
       if (param.get_name() == "cell_size")
       {
@@ -114,6 +131,8 @@ class PointCloudToGrid : public rclcpp::Node
   {
     this->declare_parameter<std::string>("mapi_topic_name", "intensity_grid");
     this->declare_parameter<std::string>("maph_topic_name", "height_grid");
+    this->declare_parameter<std::string>("mapi_gridmap_topic_name", "intensity_gridmap");
+    this->declare_parameter<std::string>("maph_gridmap_topic_name", "height_gridmap");
     this->declare_parameter<std::string>("cloud_in_topic", cloud_in_topic);
     this->declare_parameter<float>("cell_size", 0.5);
     this->declare_parameter<float>("position_x", 0.0);
@@ -131,6 +150,8 @@ class PointCloudToGrid : public rclcpp::Node
 
     this->get_parameter("mapi_topic_name", grid_map.mapi_topic_name);
     this->get_parameter("maph_topic_name", grid_map.maph_topic_name);
+    this->get_parameter("mapi_gridmap_topic_name", grid_map.mapi_gridmap_topic_name);
+    this->get_parameter("maph_gridmap_topic_name", grid_map.maph_gridmap_topic_name);
     this->get_parameter("cloud_in_topic", cloud_in_topic);
     this->get_parameter("cell_size", grid_map.cell_size);
     this->get_parameter("position_x", grid_map.position_x);
@@ -150,12 +171,20 @@ class PointCloudToGrid : public rclcpp::Node
 
     pub_igrid = this->create_publisher<nav_msgs::msg::OccupancyGrid>(grid_map.mapi_topic_name, 10);
     pub_hgrid = this->create_publisher<nav_msgs::msg::OccupancyGrid>(grid_map.maph_topic_name, 10);
+    pub_igridmap = this->create_publisher<grid_map_msgs::msg::GridMap>(grid_map.mapi_gridmap_topic_name, 10);
+    pub_hgridmap = this->create_publisher<grid_map_msgs::msg::GridMap>(grid_map.maph_gridmap_topic_name, 10);
     pub_marker = this->create_publisher<visualization_msgs::msg::MarkerArray>("debug_marker", 10);
-    sub_pc2_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(cloud_in_topic, 10, std::bind(&PointCloudToGrid::lidar_callback, this, std::placeholders::_1));
+    
+    // Configure QoS for sensor data - use BEST_EFFORT reliability to match typical sensor publishers
+    auto sensor_qos = rclcpp::QoS(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
+    sub_pc2_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+        cloud_in_topic, sensor_qos, 
+        std::bind(&PointCloudToGrid::lidar_callback, this, std::placeholders::_1));
     callback_handle_ = this->add_on_set_parameters_callback(std::bind(&PointCloudToGrid::parametersCallback, this, std::placeholders::_1));
     RCLCPP_INFO_STREAM(this->get_logger(), "pointcloud_to_grid_node has been started.");
     RCLCPP_INFO_STREAM(this->get_logger(), "Subscribing to: " << cloud_in_topic.c_str());
     RCLCPP_INFO_STREAM(this->get_logger(), "Publishing to: " << grid_map.mapi_topic_name.c_str() << " and " << grid_map.maph_topic_name.c_str());
+    RCLCPP_INFO_STREAM(this->get_logger(), "Publishing GridMaps to: " << grid_map.mapi_gridmap_topic_name.c_str() << " and " << grid_map.maph_gridmap_topic_name.c_str());
   }
 
 private:
@@ -180,6 +209,8 @@ class PointCloudToGrid : public rclcpp::Node
     // Initialize grid
     grid_map.initGrid(intensity_grid);
     grid_map.initGrid(height_grid);
+    grid_map.initGridMap(intensity_gridmap, "intensity");
+    grid_map.initGridMap(height_gridmap, "height");
     // width*height/cell_size^2 eg width = 20m, height = 30m, cell_size data size = 6000
     // or cell_num_x * cell_num_ys
     // -128 127 int8[] data
@@ -299,8 +330,23 @@ class PointCloudToGrid : public rclcpp::Node
     height_grid->info.map_load_time = this->now();
     height_grid->data = hpoints;
 
+    // Convert to GridMap format
+    intensity_gridmap->header.stamp = this->now();
+    intensity_gridmap->header.frame_id = input_msg->header.frame_id;
+    for (size_t i = 0; i < ipoints.size(); ++i) {
+      intensity_gridmap->data[0].data[i] = static_cast<float>(ipoints[i]) / 127.0f; // Normalize to float
+    }
+
+    height_gridmap->header.stamp = this->now();
+    height_gridmap->header.frame_id = input_msg->header.frame_id;
+    for (size_t i = 0; i < hpoints.size(); ++i) {
+      height_gridmap->data[0].data[i] = static_cast<float>(hpoints[i]) / 127.0f; // Normalize to float
+    }
+
     pub_hgrid->publish(*height_grid);
     pub_igrid->publish(*intensity_grid);
+    pub_hgridmap->publish(*height_gridmap);
+    pub_igridmap->publish(*intensity_gridmap);
     pub_marker->publish(mark_array);
     // pub_hgrid->publish(height_grid);
     if (verbose1)
@@ -310,6 +356,7 @@ class PointCloudToGrid : public rclcpp::Node
   }
 
   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_igrid, pub_hgrid;
+  rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr pub_igridmap, pub_hgridmap;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_marker;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pc2_;
   OnSetParametersCallbackHandle::SharedPtr callback_handle_;