Add PointCloud2ToGridMapMsg node

grid_map_pcl/CMakeLists.txt

@@ -99,7 +99,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 
 
-# Node.
+# Node grid_map_pcl_loader_node
 add_executable(grid_map_pcl_loader_node
   src/grid_map_pcl_loader_node.cpp
 )
@@ -117,6 +117,24 @@ target_link_libraries(grid_map_pcl_loader_node
   ${PROJECT_NAME}
   ${catkin_LIBRARIES}
 )
+# Node PointCloud2_to_GridMap_msg_node
+add_executable(PointCloud2_to_GridMap_msg_node
+  src/PointCloud2_to_GridMap_msg_node.cpp
+)
+add_dependencies(PointCloud2_to_GridMap_msg_node
+  ${PROJECT_NAME}
+)
+target_include_directories(PointCloud2_to_GridMap_msg_node PRIVATE
+  include
+)
+target_include_directories(PointCloud2_to_GridMap_msg_node SYSTEM PUBLIC
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+target_link_libraries(PointCloud2_to_GridMap_msg_node
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
 
 #############
 ## Install ##

grid_map_pcl/README.md

@@ -35,11 +35,16 @@ The algorithm will open the .pcd file, convert the point cloud to a grid map and
 1.  Place .pcd files in the package folder or anywhere on the system (e.g. grid_map_pcl/data/example.pcd).
 2.  Modify the *folder_path* inside the launch file such that the folder file points to the folder containing .pcd files (e.g. /*"path to the grid_map_pcl folder"*/data).
 3.  Change the *pcd_filename* to the point cloud file that you want to process
-4.  You can run the algorithm with: *roslaunch grid_map_pcl grid_map_pcl_loader_node.launch* 
+4.  You can run the algorithm with: `roslaunch grid_map_pcl grid_map_pcl_loader_node.launch`
 5.  Once the algorithm is done you will see the output in the console, then you can run rviz in a separate terminal (make sure that you have sourced your workspace, **DO NOT CLOSE** the terminal where *grid_map_pcl_loader_node* is running ) and visualize the resulting grid map. Instructions on how to visualize a grid map are in the grid map [README](../README.md).
 
 The resulting grid map will be saved in the folder given by *folder_path* variable in the launch file and will be named with a string contained inside the *output_grid_map* variable. For large point clouds (100M-140M points) the algorithm takes about 30-60 min to finish (with 6 threads). For sizes that are in the range of 40M to 60M points, the runtime varies between 5 and 15 min, depending on the number of points. Point cloud with around 10M points or less can be processed in a minute or two.
 
+Alternatively, you can also convert ROS `sensor_msgs::PointCloud2` to `grid_map_msgs::GridMap` messages by running:
+```bash
+roslaunch grid_map_pcl PointCloud2_to_GridMap_msg_node.launch
+```
+
 ## Parameters
 
 ### Algorithm Parameters (conversion from raw point clouds)

grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp

@@ -48,6 +48,12 @@ class GridMapPclLoader {
    */
   void loadCloudFromPcdFile(const std::string& filename);
 
+  /*!
+   * Loads the point cloud from a PointCloud2 message into memory
+   * @param[in] fullpath to the point cloud.
+   */
+  void loadCloudFromMessage(const sensor_msgs::PointCloud2& msg);
+
   /*!
    * Allows the user to set the input cloud
    * @param[in] pointer to the input point cloud.

grid_map_pcl/include/grid_map_pcl/PointCloud2_to_GridMap_msg_node.hpp

@@ -0,0 +1,40 @@
+/*
+ * PointCloud2_to_GridMap_msg_node.hpp
+ *
+ *  Created on: July 03, 2021
+ *      Author: Maximilian Stölzle
+ *      Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#include <ros/ros.h>
+#include "grid_map_pcl/GridMapPclLoader.hpp"
+
+
+//namespace grid_map_pcl {
+class PointCloud2ToGridMapMsgNode
+{
+    public:
+        /*!
+        * Constructor.
+        * @param nodeHandle the ROS node handle.
+        */
+        PointCloud2ToGridMapMsgNode(ros::NodeHandle& nodeHandle);
+        ~PointCloud2ToGridMapMsgNode();
+
+        /*!
+        * Callback function for the point cloud 2.
+        * @param message the point cloud2 message to be converted to a grid map msg
+        */
+        void sub_callback(const sensor_msgs::PointCloud2 & msg);
+
+    private:
+        std::string gridMapTopic_;
+        std::string pointCloudTopic_;
+
+        //! ROS nodehandle.
+        ros::NodeHandle nodeHandle_;
+
+        ros::Subscriber sub_;
+        ros::Publisher pub_;
+};
+//}

grid_map_pcl/launch/PointCloud2_to_GridMap_msg_node.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="point_cloud_topic"      default="/omni_stitched_cloud"/>
+  <arg name="grid_map_topic"         default="/grid_map" />
+  <arg name="map_frame"              default="map" />
+  <arg name="map_layer_name"         default="elevation" />
+  <arg name="prefix"                 default=""/>
+  <arg name="set_verbosity_to_debug" default="false"/>
+
+  <node name="PointCloud2_to_GridMap_msg_node" pkg="grid_map_pcl"
+      type="PointCloud2_to_GridMap_msg_node" output="screen" launch-prefix="$(arg prefix)">
+    <rosparam file="$(find grid_map_pcl)/config/parameters.yaml" />
+    <param name="point_cloud_topic"       type="string" value="$(arg point_cloud_topic)" />
+    <param name="grid_map_topic"          type="string" value="$(arg grid_map_topic)" />
+    <param name="map_frame"               type="string" value="$(arg map_frame)" />
+    <param name="map_layer_name"          type="string" value="$(arg map_layer_name)" />
+    <param name="set_verbosity_to_debug"  type="bool" value="$(arg set_verbosity_to_debug)" />
+  </node>
+</launch>

grid_map_pcl/src/GridMapPclLoader.cpp

@@ -14,6 +14,7 @@
 
 #include <pcl/common/io.h>
 #include <ros/console.h>
+#include <pcl_ros/point_cloud.h>
 
 #include <grid_map_core/GridMapMath.hpp>
 
@@ -32,6 +33,12 @@ void GridMapPclLoader::loadCloudFromPcdFile(const std::string& filename) {
   setInputCloud(inputCloud);
 }
 
+void GridMapPclLoader::loadCloudFromMessage(const sensor_msgs::PointCloud2& msg) {
+  Pointcloud::Ptr inputCloud (new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(msg, *inputCloud);
+  setInputCloud(inputCloud);
+}
+
 void GridMapPclLoader::setInputCloud(Pointcloud::ConstPtr inputCloud) {
   setRawInputCloud(inputCloud);
   setWorkingCloud(inputCloud);

grid_map_pcl/src/PointCloud2_to_GridMap_msg_node.cpp

@@ -0,0 +1,73 @@
+/*
+ * PointCloud2_to_GridMap_msg_node.cpp
+ *
+ *  Created on: June 25, 2021
+ *      Author: Maximilian Stölzle
+ *      Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#include <ros/ros.h>
+
+#include <grid_map_core/GridMap.hpp>
+#include <grid_map_ros/GridMapRosConverter.hpp>
+
+#include "grid_map_pcl/PointCloud2_to_GridMap_msg_node.hpp"
+#include "grid_map_pcl/GridMapPclLoader.hpp"
+#include "grid_map_pcl/helpers.hpp"
+
+namespace gm = ::grid_map::grid_map_pcl;
+
+PointCloud2ToGridMapMsgNode::PointCloud2ToGridMapMsgNode(ros::NodeHandle& nodeHandle)
+{
+  ROS_INFO("PointCloud2ToGridMapMsgNode started");
+  nodeHandle_ = nodeHandle;
+
+  // Publisher
+  nodeHandle_.param<std::string>("grid_map_topic", gridMapTopic_, "grid_map");
+  pub_ = nodeHandle_.advertise<grid_map_msgs::GridMap>(gridMapTopic_, 1, true);
+
+  // Subscriber
+  nodeHandle_.param<std::string>("point_cloud_topic", pointCloudTopic_, "point_cloud");
+  sub_ = nodeHandle_.subscribe(pointCloudTopic_, 1, &PointCloud2ToGridMapMsgNode::sub_callback, this);
+
+}
+
+PointCloud2ToGridMapMsgNode::~PointCloud2ToGridMapMsgNode()
+{
+  ROS_INFO("PointCloud2ToGridMapMsgNode deconstructed");
+}
+
+void PointCloud2ToGridMapMsgNode::sub_callback(const sensor_msgs::PointCloud2 & point_cloud_msg) 
+{
+  ROS_INFO("Received PointCloud2 message");
+  // init GridMapPclLoader
+  grid_map::GridMapPclLoader gridMapPclLoader;
+  gridMapPclLoader.loadParameters(gm::getParameterPath());
+
+  // load PCL point cloud from message
+  gridMapPclLoader.loadCloudFromMessage(point_cloud_msg);
+  gm::processPointcloud(&gridMapPclLoader, nodeHandle_);
+
+  // create grid map
+  grid_map::GridMap gridMap = gridMapPclLoader.getGridMap();
+  gridMap.setFrameId(gm::getMapFrame(nodeHandle_));
+
+  // publish grid map msg
+  grid_map_msgs::GridMap grid_map_msg;
+  grid_map::GridMapRosConverter::toMessage(gridMap, grid_map_msg);
+  pub_.publish(grid_map_msg);
+}
+
+int main(int argc, char** argv) {
+  ROS_INFO("Launched PointCloud2_to_GridMap_msg_node");
+
+  ros::init(argc, argv, "PointCloud2_to_GridMap_msg_node");
+  ros::NodeHandle nodeHandle("~");
+  gm::setVerbosityLevelToDebugIfFlagSet(nodeHandle);
+
+  PointCloud2ToGridMapMsgNode node = PointCloud2ToGridMapMsgNode(nodeHandle);
+
+  // run
+  ros::spin();
+  return EXIT_SUCCESS;
+}