fix(ground_segmentation): add intensity field

launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py

@@ -117,7 +117,10 @@ def create_additional_pipeline(self, lidar_name):
                     ("output", f"{lidar_name}/pointcloud"),
                 ],
                 parameters=[
-                    self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"]
+                    self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"],
+                    {"input_frame": "base_link"},
+                    {"output_frame": "base_link"},
+                    {"output_intensity_field": LaunchConfiguration("output_intensity_field")},
                 ],
                 extra_arguments=[
                     {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
@@ -267,6 +270,7 @@ def create_common_pipeline(self, input_topic, output_topic):
                     self.vehicle_info,
                     {"input_frame": "base_link"},
                     {"output_frame": "base_link"},
+                    {"output_intensity_field": LaunchConfiguration("output_intensity_field")},
                 ],
                 extra_arguments=[
                     {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
@@ -545,6 +549,7 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("use_intra_process", "True")
     add_launch_arg("pointcloud_container_name", "pointcloud_container")
     add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud")
+    add_launch_arg("output_intensity_field", "False")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",

perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp

@@ -56,7 +56,7 @@ struct RGB
 
 class RANSACGroundFilterComponent : public pointcloud_preprocessor::Filter
 {
-  using PointType = pcl::PointXYZ;
+  using PointType = pcl::PointXYZI;
 
 protected:
   void filter(

perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp

@@ -84,7 +84,7 @@ namespace ground_segmentation
 {
 class RayGroundFilterComponent : public pointcloud_preprocessor::Filter
 {
-  typedef pcl::PointXYZ PointType_;
+  typedef pcl::PointXYZI PointType_;
 
   struct PointXYZRTColor
   {

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

@@ -196,6 +196,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
   bool use_recheck_ground_cluster_;  // to enable recheck ground cluster
   bool use_lowest_point_;  // to select lowest point for reference in recheck ground cluster,
                            // otherwise select middle point
+  int output_field_num_{3};
   size_t radial_dividers_num_;
   VehicleInfo vehicle_info_;
 

perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp

@@ -198,11 +198,11 @@ void RANSACGroundFilterComponent::extractPointsIndices(
 Eigen::Affine3d RANSACGroundFilterComponent::getPlaneAffine(
   const pcl::PointCloud<PointType> segment_ground_cloud, const Eigen::Vector3d & plane_normal)
 {
-  pcl::CentroidPoint<pcl::PointXYZ> centroid;
+  pcl::CentroidPoint<PointType> centroid;
   for (const auto p : segment_ground_cloud.points) {
     centroid.add(p);
   }
-  pcl::PointXYZ centroid_point;
+  PointType centroid_point;
   centroid.get(centroid_point);
   Eigen::Translation<double, 3> trans(centroid_point.x, centroid_point.y, centroid_point.z);
   const ground_segmentation::PlaneBasis basis = getPlaneBasis(plane_normal);

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -58,6 +58,11 @@
     use_virtual_ground_point_ = declare_parameter<bool>("use_virtual_ground_point");
     use_recheck_ground_cluster_ = declare_parameter<bool>("use_recheck_ground_cluster");
     use_lowest_point_ = declare_parameter<bool>("use_lowest_point");
+    if (declare_parameter<bool>("output_intensity_field")) {
+      output_field_num_ = 4;
+    } else {
+      output_field_num_ = 3;
+    }
     radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
     vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();
 
@@ -581,8 +586,6 @@
     std::memcpy(
       &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step],
       in_cloud_ptr->point_step * sizeof(uint8_t));
-    *reinterpret_cast<float *>(&out_object_cloud.data[output_data_size + intensity_offset_]) =
-      1;  // set intensity to 1
     output_data_size += in_cloud_ptr->point_step;
   }
 }
@@ -612,7 +615,7 @@
   output.row_step = no_ground_indices.indices.size() * input->point_step;
   output.data.resize(output.row_step);
   output.width = no_ground_indices.indices.size();
-  output.fields.assign(input->fields.begin(), input->fields.begin() + 3);
+  output.fields.assign(input->fields.begin(), input->fields.begin() + output_field_num_);
   output.is_dense = true;
   output.height = input->height;
   output.is_bigendian = input->is_bigendian;