feat(map_loader): add downsampled pointcloud publisher (autowarefound…

…ation#2418) * first commit Signed-off-by: kminoda <koji.minoda@tier4.jp> * debugged Signed-off-by: kminoda <koji.minoda@tier4.jp> * update readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * update param in tier4_map_launch Signed-off-by: kminoda <koji.minoda@tier4.jp> * debug * debugged Signed-off-by: kminoda <koji.minoda@tier4.jp> * Now build works Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix * set default param to false Signed-off-by: kminoda <koji.minoda@tier4.jp> * ci(pre-commit): autofix Signed-off-by: kminoda <koji.minoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Jan 27, 2023 · ddceb9a · ddceb9a
1 parent a015692
commit ddceb9a
Show file tree

Hide file tree

Showing 7 changed files with 67 additions and 12 deletions.
diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml
@@ -1,4 +1,8 @@
 /**:
   ros__parameters:
     enable_whole_load: true
-    enable_partial_load: true
+    enable_downsampled_whole_load: false
+    enable_partial_load: false
+
+    # only used when downsample_whole_load enabled
+    leaf_size: 3.0 # downsample leaf size [m]
diff --git a/map/map_loader/README.md b/map/map_loader/README.md
@@ -10,12 +10,17 @@ This package provides the features of loading various maps.
 Currently, it supports the following two types:
 
 - Publish raw pointcloud map
+- Publish downsampled pointcloud map
 - Send partial pointcloud map loading via ROS 2 service
 
 #### Publish raw pointcloud map (ROS 2 topic)
 
 The node publishes the raw pointcloud map loaded from the `.pcd` file(s).
 
+#### Publish downsampled pointcloud map (ROS 2 topic)
+
+The node publishes the downsampled pointcloud map loaded from the `.pcd` file(s). You can specify the downsample resolution by changing the `leaf_size` parameter.
+
 #### Send partial pointcloud map (ROS 2 service)
 
 Here, we assume that the pointcloud maps are divided into grids.
@@ -25,14 +30,17 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com
 
 ### Parameters
 
-| Name                | Type | Description                                    | Default value |
-| :------------------ | :--- | :--------------------------------------------- | :------------ |
-| enable_whole_load   | bool | A flag to enable raw pointcloud map publishing | true          |
-| enable_partial_load | bool | A flag to enable partial pointcloud map server | true          |
+| Name                          | Type  | Description                                                                       | Default value |
+| :---------------------------- | :---- | :-------------------------------------------------------------------------------- | :------------ |
+| enable_whole_load             | bool  | A flag to enable raw pointcloud map publishing                                    | true          |
+| enable_downsampled_whole_load | bool  | A flag to enable downsampled pointcloud map publishing                            | false         |
+| enable_partial_load           | bool  | A flag to enable partial pointcloud map server                                    | false         |
+| leaf_size                     | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0           |
 
 ### Interfaces
 
 - `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map
+- `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map
 - `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map
 
 ---

diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml
@@ -1,4 +1,8 @@
 /**:
   ros__parameters:
     enable_whole_load: true
-    enable_partial_load: true
+    enable_downsampled_whole_load: false
+    enable_partial_load: false
+
+    # only used when downsample_whole_load enabled
+    leaf_size: 3.0 # downsample leaf size [m]
diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp
@@ -15,22 +15,47 @@
 #include "pointcloud_map_loader_module.hpp"
 
 #include <fmt/format.h>
+#include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <string>
 #include <vector>
 
+sensor_msgs::msg::PointCloud2 downsample(
+  const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size)
+{
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(msg_input, *pcl_input);
+  pcl::VoxelGrid<pcl::PointXYZ> filter;
+  filter.setInputCloud(pcl_input);
+  filter.setLeafSize(leaf_size, leaf_size, leaf_size);
+  filter.filter(*pcl_output);
+
+  sensor_msgs::msg::PointCloud2 msg_output;
+  pcl::toROSMsg(*pcl_output, msg_output);
+  msg_output.header = msg_input.header;
+  return msg_output;
+}
+
 PointcloudMapLoaderModule::PointcloudMapLoaderModule(
-  rclcpp::Node * node, const std::vector<std::string> & pcd_paths, const std::string publisher_name)
+  rclcpp::Node * node, const std::vector<std::string> & pcd_paths, const std::string publisher_name,
+  const bool use_downsample)
 : logger_(node->get_logger())
 {
   rclcpp::QoS durable_qos{1};
   durable_qos.transient_local();
   pub_pointcloud_map_ =
     node->create_publisher<sensor_msgs::msg::PointCloud2>(publisher_name, durable_qos);
 
-  sensor_msgs::msg::PointCloud2 pcd = loadPCDFiles(pcd_paths);
+  sensor_msgs::msg::PointCloud2 pcd;
+  if (use_downsample) {
+    const float leaf_size = node->declare_parameter<float>("leaf_size");
+    pcd = loadPCDFiles(pcd_paths, leaf_size);
+  } else {
+    pcd = loadPCDFiles(pcd_paths, boost::none);
+  }
 
   if (pcd.width == 0) {
     RCLCPP_ERROR(logger_, "No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size());
@@ -42,7 +67,7 @@ PointcloudMapLoaderModule::PointcloudMapLoaderModule(
 }
 
 sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles(
-  const std::vector<std::string> & pcd_paths) const
+  const std::vector<std::string> & pcd_paths, const boost::optional<float> leaf_size) const
 {
   sensor_msgs::msg::PointCloud2 whole_pcd;
   sensor_msgs::msg::PointCloud2 partial_pcd;
@@ -59,6 +84,10 @@ sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles(
       RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path);
     }
 
+    if (leaf_size) {
+      partial_pcd = downsample(partial_pcd, leaf_size.get());
+    }
+
     if (whole_pcd.width == 0) {
       whole_pcd = partial_pcd;
     } else {

diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp
@@ -29,13 +29,14 @@ class PointcloudMapLoaderModule
 public:
   explicit PointcloudMapLoaderModule(
     rclcpp::Node * node, const std::vector<std::string> & pcd_paths,
-    const std::string publisher_name);
+    const std::string publisher_name, const bool use_downsample);
 
 private:
   rclcpp::Logger logger_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_pointcloud_map_;
 
-  sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector<std::string> & pcd_paths) const;
+  sensor_msgs::msg::PointCloud2 loadPCDFiles(
+    const std::vector<std::string> & pcd_paths, const boost::optional<float> leaf_size) const;
 };
 
 #endif  // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_
diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp
@@ -50,11 +50,19 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
   const auto pcd_paths =
     getPcdPaths(declare_parameter<std::vector<std::string>>("pcd_paths_or_directory"));
   bool enable_whole_load = declare_parameter<bool>("enable_whole_load");
+  bool enable_downsample_whole_load = declare_parameter<bool>("enable_downsampled_whole_load");
   bool enable_partial_load = declare_parameter<bool>("enable_partial_load");
 
   if (enable_whole_load) {
     std::string publisher_name = "output/pointcloud_map";
-    pcd_map_loader_ = std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name);
+    pcd_map_loader_ =
+      std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name, false);
+  }
+
+  if (enable_downsample_whole_load) {
+    std::string publisher_name = "output/debug/downsampled_pointcloud_map";
+    downsampled_pcd_map_loader_ =
+      std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name, true);
   }
 
   if (enable_partial_load) {

diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp
@@ -41,6 +41,7 @@ class PointCloudMapLoaderNode : public rclcpp::Node
   std::map<std::string, PCDFileMetadata> pcd_metadata_dict_;
 
   std::unique_ptr<PointcloudMapLoaderModule> pcd_map_loader_;
+  std::unique_ptr<PointcloudMapLoaderModule> downsampled_pcd_map_loader_;
   std::unique_ptr<PartialMapLoaderModule> partial_map_loader_;
 
   std::vector<std::string> getPcdPaths(