Merge pull request #1237 from tier4/cherry-pick/pr6209+5861+6753

chore(ground_segmentation): cherry pick pr6209+5861+6753+5980

perception/ground_segmentation/CMakeLists.txt

@@ -89,6 +89,7 @@ if(BUILD_TESTING)
 
   target_link_libraries(test_ray_ground_filter
     ground_segmentation
+    ${YAML_CPP_LIBRARIES}
   )
 
   ament_add_ros_isolated_gtest(test_scan_ground_filter

perception/ground_segmentation/config/ground_segmentation.param.yaml

@@ -29,3 +29,8 @@
         gnd_grid_buffer_size: 4
         detection_range_z_max: 2.5
         elevation_grid_mode: true
+        low_priority_region_x: -20.0
+        center_pcl_shift: 0.0
+        radial_divider_angle_deg: 1.0
+        use_recheck_ground_cluster: true
+        use_lowest_point: true

perception/ground_segmentation/config/ransac_ground_filter.param.yaml

@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+    base_frame: "base_link"
+    unit_axis: "z"
+    max_iterations: 1000
+    min_trial: 5000
+    min_points: 1000
+    outlier_threshold: 0.01
+    plane_slope_threshold: 10.0
+    voxel_size_x: 0.04
+    voxel_size_y: 0.04
+    voxel_size_z: 0.04
+    height_threshold: 0.01
+    debug: false

perception/ground_segmentation/config/ray_ground_filter.param.yaml

@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+      min_x: -0.01
+      max_x: 0.01
+      min_y: -0.01
+      max_y: 0.01
+      use_vehicle_footprint: false
+      general_max_slope: 8.0
+      local_max_slope: 6.0
+      initial_max_slope: 3.0
+      radial_divider_angle: 1.0
+      min_height_threshold: 0.15
+      concentric_divider_distance: 0.0
+      reclass_distance_threshold: 0.1

perception/ground_segmentation/config/scan_ground_filter.param.yaml

@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    global_slope_max_angle_deg: 10.0
+    local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode
+    split_points_distance_tolerance: 0.2
+    use_virtual_ground_point: True
+    split_height_distance: 0.2
+    non_ground_height_threshold: 0.20
+    grid_size_m: 0.1
+    grid_mode_switch_radius: 20.0
+    gnd_grid_buffer_size: 4
+    detection_range_z_max: 2.5
+    elevation_grid_mode: true
+    low_priority_region_x: -20.0
+    center_pcl_shift: 0.0
+    radial_divider_angle_deg: 1.0
+    use_recheck_ground_cluster: true
+    use_lowest_point: true

perception/ground_segmentation/docs/scan-ground-filter.md

@@ -50,6 +50,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 | `low_priority_region_x`           | float  | -20.0         | The non-zero x threshold in back side from which small objects detection is low priority [m]                                                                                                                                                                                                                                                                     |
 | `elevation_grid_mode`             | bool   | true          | Elevation grid scan mode option                                                                                                                                                                                                                                                                                                                                  |
 | `use_recheck_ground_cluster`      | bool   | true          | Enable recheck ground cluster                                                                                                                                                                                                                                                                                                                                    |
+| `use_lowest_point`                | bool   | true          | to select lowest point for reference in recheck ground cluster, otherwise select middle point                                                                                                                                                                                                                                                                    |
 
 ## Assumptions / Known limits
 

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

@@ -16,6 +16,7 @@
 #define GROUND_SEGMENTATION__SCAN_GROUND_FILTER_NODELET_HPP_
 
 #include "pointcloud_preprocessor/filter.hpp"
+#include "pointcloud_preprocessor/transform_info.hpp"
 
 #include <vehicle_info_util/vehicle_info.hpp>
 
@@ -50,7 +51,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
   // classified point label
   // (0: not classified, 1: ground, 2: not ground, 3: follow previous point,
   //  4: unkown(currently not used), 5: virtual ground)
-  enum class PointLabel {
+  enum class PointLabel : uint16_t {
     INIT = 0,
     GROUND,
     NON_GROUND,
@@ -59,19 +60,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
     VIRTUAL_GROUND,
     OUT_OF_RANGE
   };
-  struct PointRef
+
+  struct PointData
   {
-    float grid_size;    // radius of grid
-    uint16_t grid_id;   // id of grid in vertical
-    float radius;       // cylindrical coords on XY Plane
-    float theta;        // angle deg on XY plane
-    size_t radial_div;  // index of the radial division to which this point belongs to
+    float radius;  // cylindrical coords on XY Plane
     PointLabel point_state{PointLabel::INIT};
-
+    uint16_t grid_id;   // id of grid in vertical
     size_t orig_index;  // index of this point in the source pointcloud
-    pcl::PointXYZ * orig_point;
   };
-  using PointCloudRefVector = std::vector<PointRef>;
+  using PointCloudVector = std::vector<PointData>;
 
   struct GridCenter
   {
@@ -144,35 +141,61 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
 
     pcl::PointIndices getIndices() { return pcl_indices; }
     std::vector<float> getHeightList() { return height_list; }
+
+    pcl::PointIndices & getIndicesRef() { return pcl_indices; }
+    std::vector<float> & getHeightListRef() { return height_list; }
   };
 
   void filter(
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
 
+  // TODO(taisa1): Temporary Implementation: Remove this interface when all the filter nodes
+  // conform to new API
+  virtual void faster_filter(
+    const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
+    const pointcloud_preprocessor::TransformInfo & transform_info);
+
   tf2_ros::Buffer tf_buffer_{get_clock()};
   tf2_ros::TransformListener tf_listener_{tf_buffer_};
 
+  int x_offset_;
+  int y_offset_;
+  int z_offset_;
+  int intensity_offset_;
+  bool offset_initialized_;
+
+  void set_field_offsets(const PointCloud2ConstPtr & input);
+
+  void get_point_from_global_offset(
+    const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset);
+
   const uint16_t gnd_grid_continual_thresh_ = 3;
   bool elevation_grid_mode_;
   float non_ground_height_threshold_;
   float grid_size_rad_;
+  float tan_grid_size_rad_;
   float grid_size_m_;
   float low_priority_region_x_;
   uint16_t gnd_grid_buffer_size_;
   float grid_mode_switch_grid_id_;
   float grid_mode_switch_angle_rad_;
   float virtual_lidar_z_;
   float detection_range_z_max_;
-  float center_pcl_shift_;                  // virtual center of pcl to center mass
-  float grid_mode_switch_radius_;           // non linear grid size switching distance
-  double global_slope_max_angle_rad_;       // radians
-  double local_slope_max_angle_rad_;        // radians
+  float center_pcl_shift_;             // virtual center of pcl to center mass
+  float grid_mode_switch_radius_;      // non linear grid size switching distance
+  double global_slope_max_angle_rad_;  // radians
+  double local_slope_max_angle_rad_;   // radians
+  double global_slope_max_ratio_;
+  double local_slope_max_ratio_;
   double radial_divider_angle_rad_;         // distance in rads between dividers
   double split_points_distance_tolerance_;  // distance in meters between concentric divisions
-  double                                    // minimum height threshold regardless the slope,
-    split_height_distance_;                 // useful for close points
+  double split_points_distance_tolerance_square_;
+  double                     // minimum height threshold regardless the slope,
+    split_height_distance_;  // useful for close points
   bool use_virtual_ground_point_;
   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
   size_t radial_dividers_num_;
   VehicleInfo vehicle_info_;
 
@@ -186,23 +209,25 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
    */
 
   /*!
-   * Convert pcl::PointCloud to sorted PointCloudRefVector
+   * Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector
    * @param[in] in_cloud Input Point Cloud to be organized in radial segments
    * @param[out] out_radial_ordered_points_manager Vector of Points Clouds,
    *     each element will contain the points ordered
    */
   void convertPointcloud(
-    const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
-    std::vector<PointCloudRefVector> & out_radial_ordered_points_manager);
+    const PointCloud2ConstPtr & in_cloud,
+    std::vector<PointCloudVector> & out_radial_ordered_points_manager);
   void convertPointcloudGridScan(
-    const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
-    std::vector<PointCloudRefVector> & out_radial_ordered_points_manager);
+    const PointCloud2ConstPtr & in_cloud,
+    std::vector<PointCloudVector> & out_radial_ordered_points_manager);
   /*!
    * Output ground center of front wheels as the virtual ground point
    * @param[out] point Virtual ground origin point
    */
   void calcVirtualGroundOrigin(pcl::PointXYZ & point);
 
+  float calcGridSize(const PointData & p);
+
   /*!
    * Classifies Points in the PointCloud as Ground and Not Ground
    * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud
@@ -214,14 +239,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
   void initializeFirstGndGrids(
     const float h, const float r, const uint16_t id, std::vector<GridCenter> & gnd_grids);
 
-  void checkContinuousGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
-  void checkDiscontinuousGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
-  void checkBreakGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
+  void checkContinuousGndGrid(
+    PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
+  void checkDiscontinuousGndGrid(
+    PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
+  void checkBreakGndGrid(
+    PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
   void classifyPointCloud(
-    std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
+    const PointCloud2ConstPtr & in_cloud_ptr,
+    std::vector<PointCloudVector> & in_radial_ordered_clouds,
     pcl::PointIndices & out_no_ground_indices);
   void classifyPointCloudGridScan(
-    std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
+    const PointCloud2ConstPtr & in_cloud_ptr,
+    std::vector<PointCloudVector> & in_radial_ordered_clouds,
     pcl::PointIndices & out_no_ground_indices);
   /*!
    * Re-classifies point of ground cluster based on their height
@@ -230,18 +260,18 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
    * @param non_ground_indices Output non-ground PointCloud indices
    */
   void recheckGroundCluster(
-    PointsCentroid & gnd_cluster, const float non_ground_threshold,
+    PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point,
     pcl::PointIndices & non_ground_indices);
   /*!
    * Returns the resulting complementary PointCloud, one with the points kept
    * and the other removed as indicated in the indices
    * @param in_cloud_ptr Input PointCloud to which the extraction will be performed
    * @param in_indices Indices of the points to be both removed and kept
-   * @param out_object_cloud_ptr Resulting PointCloud with the indices kept
+   * @param out_object_cloud Resulting PointCloud with the indices kept
    */
   void extractObjectPoints(
-    const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
-    pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr);
+    const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices,
+    PointCloud2 & out_object_cloud);
 
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;

perception/ground_segmentation/launch/ransac_ground_filter.launch.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="ground_segmentation_param_file" default="$(find-pkg-share ground_segmentation)/config/ransac_ground_filter.param.yaml"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
+  <arg name="output/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+  <node pkg="ground_segmentation" exec="ransac_ground_filter_node" name="ransac_ground_filter_node" output="screen">
+    <remap from="input" to="$(var input/pointcloud)"/>
+    <remap from="output" to="$(var output/pointcloud)"/>
+    <param from="$(var ground_segmentation_param_file)"/>
+  </node>
+</launch>

perception/ground_segmentation/launch/ray_ground_filter.launch.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="ground_segmentation_param_file" default="$(find-pkg-share ground_segmentation)/config/ray_ground_filter.param.yaml"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
+  <arg name="output/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+  <node pkg="ground_segmentation" exec="ray_ground_filter_node" name="ray_ground_filter_node" output="screen">
+    <remap from="input" to="$(var input/pointcloud)"/>
+    <remap from="output" to="$(var output/pointcloud)"/>
+    <param from="$(var ground_segmentation_param_file)"/>
+  </node>
+</launch>

perception/ground_segmentation/launch/scan_ground_filter.launch.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="ground_segmentation_param_file" default="$(find-pkg-share ground_segmentation)/config/scan_ground_filter.param.yaml"/>
+
+  <!-- vehicle info -->
+  <arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
+  <arg name="output/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+  <node pkg="ground_segmentation" exec="scan_ground_filter_node" name="scan_ground_filter_node" output="screen">
+    <remap from="input" to="$(var input/pointcloud)"/>
+    <remap from="output" to="$(var output/pointcloud)"/>
+    <param from="$(var ground_segmentation_param_file)"/>
+    <param from="$(var vehicle_info_param_file)"/>
+  </node>
+</launch>

perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp

@@ -85,18 +85,18 @@ using pointcloud_preprocessor::get_param;
 RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options)
 : Filter("RANSACGroundFilter", options)
 {
-  base_frame_ = declare_parameter("base_frame", "base_link");
-  unit_axis_ = declare_parameter("unit_axis", "z");
-  max_iterations_ = declare_parameter("max_iterations", 1000);
-  min_inliers_ = declare_parameter("min_trial", 5000);
-  min_points_ = declare_parameter("min_points", 1000);
-  outlier_threshold_ = declare_parameter("outlier_threshold", 0.01);
-  plane_slope_threshold_ = declare_parameter("plane_slope_threshold", 10.0);
-  voxel_size_x_ = declare_parameter("voxel_size_x", 0.04);
-  voxel_size_y_ = declare_parameter("voxel_size_y", 0.04);
-  voxel_size_z_ = declare_parameter("voxel_size_z", 0.04);
-  height_threshold_ = declare_parameter("height_threshold", 0.01);
-  debug_ = declare_parameter("debug", false);
+  base_frame_ = declare_parameter<std::string>("base_frame", "base_link");
+  unit_axis_ = declare_parameter<std::string>("unit_axis");
+  max_iterations_ = declare_parameter<int>("max_iterations");
+  min_inliers_ = declare_parameter<int>("min_trial");
+  min_points_ = declare_parameter<int>("min_points");
+  outlier_threshold_ = declare_parameter<double>("outlier_threshold");
+  plane_slope_threshold_ = declare_parameter<double>("plane_slope_threshold");
+  voxel_size_x_ = declare_parameter<double>("voxel_size_x");
+  voxel_size_y_ = declare_parameter<double>("voxel_size_y");
+  voxel_size_z_ = declare_parameter<double>("voxel_size_z");
+  height_threshold_ = declare_parameter<double>("height_threshold");
+  debug_ = declare_parameter<bool>("debug");
 
   if (unit_axis_ == "x") {
     unit_vec_ = Eigen::Vector3d::UnitX();

perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp

@@ -48,22 +48,22 @@ RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & o
     grid_precision_ = 0.2;
     ray_ground_filter::generateColors(colors_, color_num_);
 
-    min_x_ = declare_parameter("min_x", -0.01);
-    max_x_ = declare_parameter("max_x", 0.01);
-    min_y_ = declare_parameter("min_y", -0.01);
-    max_y_ = declare_parameter("max_y", 0.01);
+    min_x_ = declare_parameter<double>("min_x");
+    max_x_ = declare_parameter<double>("max_x");
+    min_y_ = declare_parameter<double>("min_y");
+    max_y_ = declare_parameter<double>("max_y");
 
     setVehicleFootprint(min_x_, max_x_, min_y_, max_y_);
 
-    use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false);
+    use_vehicle_footprint_ = declare_parameter<bool>("use_vehicle_footprint", false);
 
-    general_max_slope_ = declare_parameter("general_max_slope", 8.0);
-    local_max_slope_ = declare_parameter("local_max_slope", 6.0);
-    initial_max_slope_ = declare_parameter("initial_max_slope", 3.0);
-    radial_divider_angle_ = declare_parameter("radial_divider_angle", 1.0);
-    min_height_threshold_ = declare_parameter("min_height_threshold", 0.15);
-    concentric_divider_distance_ = declare_parameter("concentric_divider_distance", 0.0);
-    reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold", 0.1);
+    general_max_slope_ = declare_parameter<double>("general_max_slope");
+    local_max_slope_ = declare_parameter<double>("local_max_slope");
+    initial_max_slope_ = declare_parameter<double>("initial_max_slope");
+    radial_divider_angle_ = declare_parameter<double>("radial_divider_angle");
+    min_height_threshold_ = declare_parameter<double>("min_height_threshold");
+    concentric_divider_distance_ = declare_parameter<double>("concentric_divider_distance");
+    reclass_distance_threshold_ = declare_parameter<double>("reclass_distance_threshold");
   }
 
   using std::placeholders::_1;