Merge pull request #679 from tier4/sync-upstream

chore: sync upstream
tier4 · Jul 25, 2023 · 45204d2 · 45204d2
2 parents dbb1c9a + dfd0ee7
commit 45204d2
Show file tree

Hide file tree

Showing 37 changed files with 1,052 additions and 453 deletions.
diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -5,23 +5,32 @@
   <arg name="output/objects" default="objects"/>
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
   <arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
+  <arg name="detection_rois0" default="/perception/object_recognition/detection/rois0" description="detection rois output topic name"/>
   <arg name="image_raw1" default="/image_raw1"/>
   <arg name="camera_info1" default="/camera_info1"/>
+  <arg name="detection_rois1" default="/perception/object_recognition/detection/rois1"/>
   <arg name="image_raw2" default="/image_raw2"/>
   <arg name="camera_info2" default="/camera_info2"/>
+  <arg name="detection_rois2" default="/perception/object_recognition/detection/rois2"/>
   <arg name="image_raw3" default="/image_raw3"/>
   <arg name="camera_info3" default="/camera_info3"/>
+  <arg name="detection_rois3" default="/perception/object_recognition/detection/rois3"/>
   <arg name="image_raw4" default="/image_raw4"/>
   <arg name="camera_info4" default="/camera_info4"/>
+  <arg name="detection_rois4" default="/perception/object_recognition/detection/rois4"/>
   <arg name="image_raw5" default="/image_raw5"/>
   <arg name="camera_info5" default="/camera_info5"/>
+  <arg name="detection_rois5" default="/perception/object_recognition/detection/rois5"/>
   <arg name="image_raw6" default="/image_raw6"/>
   <arg name="camera_info6" default="/camera_info6"/>
+  <arg name="detection_rois6" default="/perception/object_recognition/detection/rois6"/>
   <arg name="image_raw7" default="/image_raw7"/>
   <arg name="camera_info7" default="/camera_info7"/>
+  <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+  <arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
@@ -65,6 +74,7 @@
         <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
         <arg name="output_clusters" value="clusters"/>
         <arg name="use_pointcloud_map" value="false"/>
+        <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
       </include>
     </group>
 
@@ -82,21 +92,21 @@
       <group>
         <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_cluster_fusion.launch.xml">
           <arg name="input/camera_info0" value="$(var camera_info0)"/>
-          <arg name="input/rois0" value="/perception/object_recognition/detection/rois0"/>
+          <arg name="input/rois0" value="$(var detection_rois0)"/>
           <arg name="input/camera_info1" value="$(var camera_info1)"/>
-          <arg name="input/rois1" value="/perception/object_recognition/detection/rois1"/>
+          <arg name="input/rois1" value="$(var detection_rois1)"/>
           <arg name="input/camera_info2" value="$(var camera_info2)"/>
-          <arg name="input/rois2" value="/perception/object_recognition/detection/rois2"/>
+          <arg name="input/rois2" value="$(var detection_rois2)"/>
           <arg name="input/camera_info3" value="$(var camera_info3)"/>
-          <arg name="input/rois3" value="/perception/object_recognition/detection/rois3"/>
+          <arg name="input/rois3" value="$(var detection_rois3)"/>
           <arg name="input/camera_info4" value="$(var camera_info4)"/>
-          <arg name="input/rois4" value="/perception/object_recognition/detection/rois4"/>
+          <arg name="input/rois4" value="$(var detection_rois4)"/>
           <arg name="input/camera_info5" value="$(var camera_info5)"/>
-          <arg name="input/rois5" value="/perception/object_recognition/detection/rois5"/>
+          <arg name="input/rois5" value="$(var detection_rois5)"/>
           <arg name="input/camera_info6" value="$(var camera_info6)"/>
-          <arg name="input/rois6" value="/perception/object_recognition/detection/rois6"/>
+          <arg name="input/rois6" value="$(var detection_rois6)"/>
           <arg name="input/camera_info7" value="$(var camera_info7)"/>
-          <arg name="input/rois7" value="/perception/object_recognition/detection/rois7"/>
+          <arg name="input/rois7" value="$(var detection_rois7)"/>
           <arg name="input/rois_number" value="$(var image_number)"/>
           <arg name="input/clusters" value="$(var input/clustering)"/>
           <arg name="input/image0" value="$(var image_raw0)"/>
@@ -183,21 +193,21 @@
     <push-ros-namespace namespace="pointpainting"/>
     <include file="$(find-pkg-share image_projection_based_fusion)/launch/pointpainting_fusion.launch.xml">
       <arg name="input/camera_info0" value="$(var camera_info0)"/>
-      <arg name="input/rois0" value="/perception/object_recognition/detection/rois0"/>
+      <arg name="input/rois0" value="$(var detection_rois0)"/>
       <arg name="input/camera_info1" value="$(var camera_info1)"/>
-      <arg name="input/rois1" value="/perception/object_recognition/detection/rois1"/>
+      <arg name="input/rois1" value="$(var detection_rois1)"/>
       <arg name="input/camera_info2" value="$(var camera_info2)"/>
-      <arg name="input/rois2" value="/perception/object_recognition/detection/rois2"/>
+      <arg name="input/rois2" value="$(var detection_rois2)"/>
       <arg name="input/camera_info3" value="$(var camera_info3)"/>
-      <arg name="input/rois3" value="/perception/object_recognition/detection/rois3"/>
+      <arg name="input/rois3" value="$(var detection_rois3)"/>
       <arg name="input/camera_info4" value="$(var camera_info4)"/>
-      <arg name="input/rois4" value="/perception/object_recognition/detection/rois4"/>
+      <arg name="input/rois4" value="$(var detection_rois4)"/>
       <arg name="input/camera_info5" value="$(var camera_info5)"/>
-      <arg name="input/rois5" value="/perception/object_recognition/detection/rois5"/>
+      <arg name="input/rois5" value="$(var detection_rois5)"/>
       <arg name="input/camera_info6" value="$(var camera_info6)"/>
-      <arg name="input/rois6" value="/perception/object_recognition/detection/rois6"/>
+      <arg name="input/rois6" value="$(var detection_rois6)"/>
       <arg name="input/camera_info7" value="$(var camera_info7)"/>
-      <arg name="input/rois7" value="/perception/object_recognition/detection/rois7"/>
+      <arg name="input/rois7" value="$(var detection_rois7)"/>
       <arg name="input/rois_number" value="$(var image_number)"/>
       <arg name="input/image0" value="$(var image_raw0)"/>
       <arg name="input/image1" value="$(var image_raw1)"/>

diff --git a/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -4,6 +4,7 @@
   <arg name="input/pointcloud"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `clustering`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+  <arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
@@ -59,6 +60,7 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="score_threshold" value="$(var score_threshold)"/>
+      <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
     </include>
   </group>
 

diff --git a/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -5,6 +5,7 @@
   <arg name="output/objects" default="objects"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `clustering`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+  <arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
@@ -33,6 +34,7 @@
         <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
         <arg name="output_clusters" value="clusters"/>
         <arg name="use_pointcloud_map" value="false"/>
+        <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
       </include>
     </group>
 

diff --git a/...er4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/...er4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
@@ -38,6 +38,7 @@ def __init__(self, context):
         self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"]
         self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"]
         self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"]
+        self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"]
         self.timer_interval_ms = self.pointcloud_map_filter_param["timer_interval_ms"]
         self.use_dynamic_map_loading = self.pointcloud_map_filter_param["use_dynamic_map_loading"]
         self.map_update_distance_threshold = self.pointcloud_map_filter_param[
@@ -69,6 +70,7 @@ def create_normal_pipeline(self):
                 parameters=[
                     {
                         "distance_threshold": self.distance_threshold,
+                        "downsize_ratio_z_axis": self.downsize_ratio_z_axis,
                         "timer_interval_ms": self.timer_interval_ms,
                         "use_dynamic_map_loading": self.use_dynamic_map_loading,
                         "map_update_distance_threshold": self.map_update_distance_threshold,
@@ -125,6 +127,7 @@ def create_down_sample_pipeline(self):
                 parameters=[
                     {
                         "distance_threshold": self.distance_threshold,
+                        "downsize_ratio_z_axis": self.downsize_ratio_z_axis,
                         "timer_interval_ms": self.timer_interval_ms,
                         "use_dynamic_map_loading": self.use_dynamic_map_loading,
                         "map_update_distance_threshold": self.map_update_distance_threshold,

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -33,23 +33,32 @@
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="image_raw0" default="/sensing/camera/camera0/image_rect_color" description="image raw topic name"/>
   <arg name="camera_info0" default="/sensing/camera/camera0/camera_info" description="camera info topic name"/>
+  <arg name="detection_rois0" default="/perception/object_recognition/detection/rois0" description="detection rois output topic name"/>
   <arg name="image_raw1" default="/sensing/camera/camera1/image_rect_color"/>
   <arg name="camera_info1" default="/sensing/camera/camera1/camera_info"/>
+  <arg name="detection_rois1" default="/perception/object_recognition/detection/rois1"/>
   <arg name="image_raw2" default="/sensing/camera/camera2/image_rect_color"/>
   <arg name="camera_info2" default="/sensing/camera/camera2/camera_info"/>
+  <arg name="detection_rois2" default="/perception/object_recognition/detection/rois2"/>
   <arg name="image_raw3" default="/sensing/camera/camera3/image_rect_color"/>
   <arg name="camera_info3" default="/sensing/camera/camera3/camera_info"/>
+  <arg name="detection_rois3" default="/perception/object_recognition/detection/rois3"/>
   <arg name="image_raw4" default="/sensing/camera/camera4/image_rect_color"/>
   <arg name="camera_info4" default="/sensing/camera/camera4/camera_info"/>
+  <arg name="detection_rois4" default="/perception/object_recognition/detection/rois4"/>
   <arg name="image_raw5" default="/sensing/camera/camera5/image_rect_color"/>
   <arg name="camera_info5" default="/sensing/camera/camera5/camera_info"/>
+  <arg name="detection_rois5" default="/perception/object_recognition/detection/rois5"/>
   <arg name="image_raw6" default="/sensing/camera/camera6/image_rect_color"/>
   <arg name="camera_info6" default="/sensing/camera/camera6/camera_info"/>
+  <arg name="detection_rois6" default="/perception/object_recognition/detection/rois6"/>
   <arg name="image_raw7" default="/sensing/camera/camera7/image_rect_color"/>
   <arg name="camera_info7" default="/sensing/camera/camera7/camera_info"/>
+  <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
   <arg name="image_number" default="6" description="choose image raw number(1-8)"/>
   <arg name="use_vector_map" default="true" description="use vector map in prediction"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+  <arg name="use_downsample_pointcloud" default="false" description="use downsample pointcloud in perception"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg
     name="use_empty_dynamic_object_publisher"
@@ -137,6 +146,7 @@
           <arg name="voxel_grid_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_param_path)"/>
           <arg name="voxel_grid_based_euclidean_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_cluster_param_path)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+          <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
           <arg name="use_object_filter" value="$(var use_object_filter)"/>
           <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
           <arg name="container_name" value="$(var pointcloud_container_name)"/>

diff --git a/perception/compare_map_segmentation/README.md b/perception/compare_map_segmentation/README.md
@@ -76,14 +76,15 @@ This filter is a combination of the distance_based_compare_map_filter and voxel_
 
 #### Parameters
 
-| Name                            | Type  | Description                                                                                                                             | Default value |
-| :------------------------------ | :---- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| `use_dynamic_map_loading`       | bool  | map loading mode selection, `true` for dynamic map loading, `false` for static map loading, recommended for no-split map pointcloud     | true          |
-| `distance_threshold`            | float | Threshold distance to compare input points with map points [m]                                                                          | 0.5           |
-| `map_update_distance_threshold` | float | Threshold of vehicle movement distance when map update is necessary (in dynamic map loading) [m]                                        | 10.0          |
-| `map_loader_radius`             | float | Radius of map need to be loaded (in dynamic map loading) [m]                                                                            | 150.0         |
-| `timer_interval_ms`             | int   | Timer interval to check if the map update is necessary (in dynamic map loading) [ms]                                                    | 100           |
-| `publish_debug_pcd`             | bool  | Enable to publish voxelized updated map in `debug/downsampled_map/pointcloud` for debugging. It might cause additional computation cost | false         |
+| Name                            | Type   | Description                                                                                                                             | Default value |
+| :------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `use_dynamic_map_loading`       | bool   | map loading mode selection, `true` for dynamic map loading, `false` for static map loading, recommended for no-split map pointcloud     | true          |
+| `distance_threshold`            | float  | Threshold distance to compare input points with map points [m]                                                                          | 0.5           |
+| `map_update_distance_threshold` | float  | Threshold of vehicle movement distance when map update is necessary (in dynamic map loading) [m]                                        | 10.0          |
+| `map_loader_radius`             | float  | Radius of map need to be loaded (in dynamic map loading) [m]                                                                            | 150.0         |
+| `timer_interval_ms`             | int    | Timer interval to check if the map update is necessary (in dynamic map loading) [ms]                                                    | 100           |
+| `publish_debug_pcd`             | bool   | Enable to publish voxelized updated map in `debug/downsampled_map/pointcloud` for debugging. It might cause additional computation cost | false         |
+| `downsize_ratio_z_axis`         | double | Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis                                                | 0.5           |
 
 ## Assumptions / Known limits
 

diff --git a/...gmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/...gmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp
@@ -42,7 +42,7 @@ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
 public:
   DistanceBasedStaticMapLoader(
     rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex)
-  : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex)
+  : VoxelGridStaticMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex)
   {
     RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n");
   }
@@ -57,7 +57,7 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
   DistanceBasedDynamicMapLoader(
     rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex,
     rclcpp::CallbackGroup::SharedPtr main_callback_group)
-  : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group)
+  : VoxelGridDynamicMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex, main_callback_group)
   {
     RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n");
   }

diff --git a/...n/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp b/...n/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp
@@ -32,8 +32,9 @@ class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader
 {
 public:
   explicit VoxelBasedApproximateStaticMapLoader(
-    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex)
-  : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex)
+    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+    std::string * tf_map_input_frame, std::mutex * mutex)
+  : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
   {
     RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n");
   }
@@ -44,9 +45,11 @@ class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader
 {
 public:
   VoxelBasedApproximateDynamicMapLoader(
-    rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex,
+    rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+    std::string * tf_map_input_frame, std::mutex * mutex,
     rclcpp::CallbackGroup::SharedPtr main_callback_group)
-  : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group)
+  : VoxelGridDynamicMapLoader(
+      node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group)
   {
     RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n");
   }