feat(probabilistic_occupancy_grid_map): accelerate occupancy grid map generation

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

@@ -239,19 +239,42 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
 void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(
   const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc)
 {
-  PclPointCloud tmp_behind_pc;
-  PclPointCloud tmp_front_pc;
-  for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"), y(*input_pc, "y"),
-       z(*input_pc, "z");
-       x != x.end(); ++x, ++y, ++z) {
+  size_t front_count = 0;
+  size_t behind_count = 0;
+
+  for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"); x != x.end(); ++x) {
     if (*x < 0.0) {
-      tmp_behind_pc.push_back(pcl::PointXYZ(*x, *y, *z));
+      behind_count++;
     } else {
-      tmp_front_pc.push_back(pcl::PointXYZ(*x, *y, *z));
+      front_count++;
     }
   }
-  pcl::toROSMsg(tmp_front_pc, front_pc);
-  pcl::toROSMsg(tmp_behind_pc, behind_pc);
+
+  sensor_msgs::PointCloud2Modifier front_pc_modifier(front_pc);
+  sensor_msgs::PointCloud2Modifier behind_pc_modifier(behind_pc);
+  front_pc_modifier.setPointCloud2FieldsByString(1, "xyz");
+  behind_pc_modifier.setPointCloud2FieldsByString(1, "xyz");
+  front_pc_modifier.resize(front_count);
+  behind_pc_modifier.resize(behind_count);
+
+  sensor_msgs::PointCloud2Iterator<float> fr_iter(front_pc, "x");
+  sensor_msgs::PointCloud2Iterator<float> be_iter(behind_pc, "x");
+
+  for (sensor_msgs::PointCloud2ConstIterator<float> in_iter(*input_pc, "x");
+       in_iter != in_iter.end(); ++in_iter) {
+    if (*in_iter < 0.0) {
+      *be_iter = in_iter[0];
+      be_iter[1] = in_iter[1];
+      be_iter[2] = in_iter[2];
+      ++be_iter;
+    } else {
+      *fr_iter = in_iter[0];
+      fr_iter[1] = in_iter[1];
+      fr_iter[2] = in_iter[2];
+      ++fr_iter;
+    }
+  }
+
   front_pc.header = input_pc->header;
   behind_pc.header = input_pc->header;
 }

perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml

@@ -12,6 +12,10 @@
         map_length_x: 150.0 # [m]
         map_length_y: 150.0 # [m]
 
+      # pointcloud donwsampler
+      downsample_input_pointcloud: true
+      downsample_voxel_size: 0.125 # [m]
+
       # each grid map parameters
       ogm_creation_config:
         height_filter:

perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml

@@ -8,6 +8,10 @@
       min_height: -1.0
       max_height: 2.0
 
+    # pointcloud donwsampler
+    downsample_input_pointcloud: true
+    downsample_voxel_size: 0.125 # [m]
+
     enable_single_frame_mode: false
     # use sensor pointcloud to filter obstacle pointcloud
     filter_obstacle_pointcloud_by_raw_pointcloud: false

perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp

@@ -116,6 +116,10 @@ bool extractCommonPointCloud(
   const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc,
   sensor_msgs::msg::PointCloud2 & output_obstacle_pc);
 
+bool extractApproximateCommonPointCloud(
+  const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc,
+  const float voxel_size, sensor_msgs::msg::PointCloud2 & output_obstacle_pc);
+
 unsigned char getApproximateOccupancyState(const unsigned char & value);
 }  // namespace utils
 

perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py

@@ -92,6 +92,60 @@
     return ogm_creation_config
 
 
+# generate downsampler
+def get_downsample_filter_node(setting: dict) -> ComposableNode:
+    plugin_str = setting["plugin"]
+    voxel_size = setting["voxel_size"]
+    node_name = setting["node_name"]
+    return ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin=plugin_str,
+        name=node_name,
+        remappings=[
+            ("input", setting["input_topic"]),
+            ("output", setting["output_topic"]),
+        ],
+        parameters=[
+            {
+                "voxel_size_x": voxel_size,
+                "voxel_size_y": voxel_size,
+                "voxel_size_z": voxel_size,
+            }
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+
+def get_downsample_preprocess_nodes(voxel_size: float, raw_pointcloud_topics: list) -> list:
+    nodes = []
+    for i in range(len(raw_pointcloud_topics)):
+        raw_settings = {
+            "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+            "node_name": "raw_pc_downsample_filter_" + str(i),
+            "input_topic": raw_pointcloud_topics[i],
+            "output_topic": raw_pointcloud_topics[i] + "_downsampled",
+            "voxel_size": voxel_size,
+        }
+        nodes.append(get_downsample_filter_node(raw_settings))
+    obstacle_settings = {
+        "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+        "node_name": "obstacle_pc_downsample_filter",
+        "input_topic": LaunchConfiguration("input/obstacle_pointcloud"),
+        "output_topic": "/perception/obstacle_segmentation/downsample/pointcloud",
+        "voxel_size": voxel_size,
+    }
+    nodes.append(get_downsample_filter_node(obstacle_settings))
+    raw_settings = {
+        "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+        "node_name": "obstacle_pc_downsample_filter",
+        "input_topic": LaunchConfiguration("input/obstacle_pointcloud"),
+        "output_topic": "/perception/obstacle_segmentation/downsample/pointcloud",
+        "voxel_size": voxel_size,
+    }
+    nodes.append(get_downsample_filter_node(raw_settings))
+    return nodes
+
+
 def launch_setup(context, *args, **kwargs):
     """Launch fusion based occupancy grid map creation nodes.
 
@@ -118,27 +172,52 @@
         LaunchConfiguration("pointcloud_container_name").perform(context),
     )
 
+    # Down sample settings
+    downsample_input_pointcloud: bool = total_config["downsample_input_pointcloud"]
+    downsample_voxel_size: float = total_config["downsample_voxel_size"]
+
+    # get obstacle pointcloud
+    obstacle_pointcloud_topic: str = (
+        "/perception/obstacle_segmentation/downsample/pointcloud"
+        if downsample_input_pointcloud
+        else LaunchConfiguration("input/obstacle_pointcloud").perform(context)
+    )
+
     for i in range(number_of_nodes):
         # load parameter file
         ogm_creation_config = get_ogm_creation_config(total_config, i)
         ogm_creation_config["updater_type"] = LaunchConfiguration("updater_type").perform(context)
         ogm_creation_config.update(updater_config)
 
+        raw_pointcloud_topic: str = fusion_config["raw_pointcloud_topics"][i]
+        frame_name: str = raw_pointcloud_topic.split("/")[
+            -2
+        ]  # assume `/sensing/lidar/top/pointcloud` -> `top
+        if downsample_input_pointcloud:
+            raw_pointcloud_topic += "_downsampled"
+
         # generate composable node
         node = ComposableNode(
             package="probabilistic_occupancy_grid_map",
             plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode",
-            name="occupancy_grid_map_node_in_" + str(i),
+            name="occupancy_grid_map_node" + str(i),
+            namespace=frame_name,
             remappings=[
-                ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
-                ("~/input/raw_pointcloud", fusion_config["raw_pointcloud_topics"][i]),
+                ("~/input/obstacle_pointcloud", obstacle_pointcloud_topic),
+                ("~/input/raw_pointcloud", raw_pointcloud_topic),
                 ("~/output/occupancy_grid_map", fusion_config["fusion_input_ogm_topics"][i]),
             ],
             parameters=[ogm_creation_config],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
         gridmap_generation_composable_nodes.append(node)
 
+    if downsample_input_pointcloud:
+        downsapmle_nodes = get_downsample_preprocess_nodes(
+            downsample_voxel_size, fusion_config["raw_pointcloud_topics"]
+        )
+        gridmap_generation_composable_nodes.extend(downsapmle_nodes)
+
     # 2. launch occupancy grid map fusion node
     gridmap_fusion_node = [
         ComposableNode(

perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py

@@ -26,6 +26,47 @@
 import yaml
 
 
+def get_downsample_filter_node(setting: dict) -> ComposableNode:
+    plugin_str = setting["plugin"]
+    voxel_size = setting["voxel_size"]
+    node_name = setting["node_name"]
+    return ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin=plugin_str,
+        name=node_name,
+        remappings=[
+            ("input", setting["input_topic"]),
+            ("output", setting["output_topic"]),
+        ],
+        parameters=[
+            {
+                "voxel_size_x": voxel_size,
+                "voxel_size_y": voxel_size,
+                "voxel_size_z": voxel_size,
+            }
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+
+def get_downsample_preprocess_nodes(voxel_size: float) -> list:
+    raw_settings = {
+        "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+        "node_name": "raw_pc_downsample_filter",
+        "input_topic": LaunchConfiguration("input/raw_pointcloud"),
+        "output_topic": "/sensing/lidar/concatenated/downsample/pointcloud",
+        "voxel_size": voxel_size,
+    }
+    obstacle_settings = {
+        "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+        "node_name": "obstacle_pc_downsample_filter",
+        "input_topic": LaunchConfiguration("input/obstacle_pointcloud"),
+        "output_topic": "/perception/obstacle_segmentation/downsample/pointcloud",
+        "voxel_size": voxel_size,
+    }
+    return [get_downsample_filter_node(raw_settings), get_downsample_filter_node(obstacle_settings)]
+
+
 def launch_setup(context, *args, **kwargs):
     # load parameter files
     param_file = LaunchConfiguration("param_file").perform(context)
@@ -38,19 +79,37 @@ def launch_setup(context, *args, **kwargs):
     with open(updater_param_file, "r") as f:
         occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"]
 
-    composable_nodes = [
+    # composable nodes
+    composable_nodes = []
+
+    # add downsample process
+    downsample_input_pointcloud: bool = pointcloud_based_occupancy_grid_map_node_params[
+        "downsample_input_pointcloud"
+    ]
+    if downsample_input_pointcloud:
+        voxel_grid_size: float = pointcloud_based_occupancy_grid_map_node_params[
+            "downsample_voxel_size"
+        ]
+        downsample_preprocess_nodes = get_downsample_preprocess_nodes(voxel_grid_size)
+        composable_nodes.extend(downsample_preprocess_nodes)
+
+    composable_nodes.append(
         ComposableNode(
             package="probabilistic_occupancy_grid_map",
             plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode",
             name="occupancy_grid_map_node",
             remappings=[
                 (
                     "~/input/obstacle_pointcloud",
-                    LaunchConfiguration("input/obstacle_pointcloud"),
+                    LaunchConfiguration("input/obstacle_pointcloud")
+                    if not downsample_input_pointcloud
+                    else "/perception/obstacle_segmentation/downsample/pointcloud",
                 ),
                 (
                     "~/input/raw_pointcloud",
-                    LaunchConfiguration("input/raw_pointcloud"),
+                    LaunchConfiguration("input/raw_pointcloud")
+                    if not downsample_input_pointcloud
+                    else "/sensing/lidar/concatenated/downsample/pointcloud",
                 ),
                 ("~/output/occupancy_grid_map", LaunchConfiguration("output")),
             ],
@@ -60,8 +119,8 @@ def launch_setup(context, *args, **kwargs):
                 {"updater_type": LaunchConfiguration("updater_type")},
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-        ),
-    ]
+        )
+    )
 
     occupancy_grid_map_container = ComposableNodeContainer(
         name=LaunchConfiguration("individual_container_name"),

perception/probabilistic_occupancy_grid_map/package.xml

@@ -34,6 +34,7 @@
   <depend>tier4_autoware_utils</depend>
   <depend>visualization_msgs</depend>
 
+  <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pointcloud_to_laserscan</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
@@ -44,6 +45,7 @@
   <test_depend>launch_testing</test_depend>
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>launch_testing_ros</test_depend>
+  <test_depend>pointcloud_preprocessor</test_depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp

@@ -162,9 +162,16 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
   // Filter obstacle pointcloud by raw pointcloud
   PointCloud2 filtered_obstacle_pc_common{};
   if (filter_obstacle_pointcloud_by_raw_pointcloud_) {
-    if (!utils::extractCommonPointCloud(
-          filtered_obstacle_pc, filtered_raw_pc, filtered_obstacle_pc_common)) {
-      filtered_obstacle_pc_common = filtered_obstacle_pc;
+    const bool use_exact_common_pointcloud = true;
+    const bool success_to_filter =
+      use_exact_common_pointcloud
+        ? utils::extractCommonPointCloud(
+            filtered_obstacle_pc, filtered_raw_pc, filtered_obstacle_pc_common)
+        : utils::extractApproximateCommonPointCloud(
+            filtered_obstacle_pc, filtered_raw_pc, 0.15f, filtered_obstacle_pc_common);
+    if (!success_to_filter) {
+      // RCLCPP_WARN_STREAM(get_logger(), "pointcloud filtering is failed in
+      // filter_obstacle_pointcloud_by_raw_pointcloud.");
     }
   } else {
     filtered_obstacle_pc_common = filtered_obstacle_pc;
@@ -190,8 +197,13 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
   occupancy_grid_map_ptr_->updateOrigin(
     gridmap_origin.position.x - occupancy_grid_map_ptr_->getSizeInMetersX() / 2,
     gridmap_origin.position.y - occupancy_grid_map_ptr_->getSizeInMetersY() / 2);
-  occupancy_grid_map_ptr_->updateWithPointCloud(
-    filtered_raw_pc, filtered_obstacle_pc_common, robot_pose, scan_origin);
+  // check the raw/obstacle pointcloud size
+  if (filtered_raw_pc.data.size() == 0 || filtered_obstacle_pc_common.data.size() == 0) {
+    RCLCPP_WARN_STREAM(get_logger(), "pointcloud size is 0");
+  } else {
+    occupancy_grid_map_ptr_->updateWithPointCloud(
+      filtered_raw_pc, filtered_obstacle_pc_common, robot_pose, scan_origin);
+  }
 
   if (enable_single_frame_mode_) {
     // publish