fix(ground_segmentation): add field select option

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Author: badai-nguyen

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

@@ -118,7 +118,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")}
@@ -268,6 +271,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")}
@@ -563,6 +567,7 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("use_pointcloud_container", "False")
     add_launch_arg("container_name", "perception_pipeline_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/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/scan_ground_filter_nodelet.cpp

@@ -58,6 +58,11 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
     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();
 
@@ -610,7 +615,7 @@ void ScanGroundFilterComponent::faster_filter(
   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() + 4);
+  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;