Merge pull request #1126 from tier4/hotfix/perception/refactor_pointc…

…loud_container

feat(perception): refactor pointcloud container

launch/tier4_localization_launch/launch/localization.launch.xml

@@ -15,14 +15,15 @@
   <arg name="eagleye_param_path"/>
 
   <arg name="input_pointcloud" default="/sensing/lidar/top/pointcloud"/>
-  <arg name="use_pointcloud_container" default="true" description="launch pointcloud container"/>
-  <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="lidar_container_name" default="/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container"/>
 
   <!-- localization module -->
   <group>
     <push-ros-namespace namespace="localization"/>
     <!-- pose_twist_estimator module -->
-    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/pose_twist_estimator.launch.xml"/>
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/pose_twist_estimator.launch.xml">
+      <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
+    </include>
 
     <!-- pose_twist_fusion_filter module -->
     <group>

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

@@ -23,7 +23,9 @@
       <group if="$(var gnss_enabled)">
         <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
       </group>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
+        <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
+      </include>
     </group>
   </group>
 
@@ -142,7 +144,9 @@
       <group if="$(var gnss_enabled)">
         <include file="$(find-pkg-share automatic_pose_initializer)/launch/automatic_pose_initializer.launch.xml"/>
       </group>
-      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py">
+        <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
+      </include>
     </group>
   </group>
 </launch>

launch/tier4_localization_launch/launch/util/util.launch.py

@@ -15,8 +15,6 @@
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import OpaqueFunction
-from launch.conditions import LaunchConfigurationNotEquals
-from launch.conditions import UnlessCondition
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
@@ -71,16 +69,9 @@ def load_composable_node_param(param_path):
         random_downsample_component,
     ]
 
-    target_container = (
-        "/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container"
-        if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
-        else LaunchConfiguration("pointcloud_container_name")
-    )
-
     load_composable_nodes = LoadComposableNodes(
-        condition=LaunchConfigurationNotEquals(target_container, ""),
         composable_node_descriptions=composable_nodes,
-        target_container=target_container,
+        target_container=LaunchConfiguration("lidar_container_name"),
     )
 
     return [load_composable_nodes]
@@ -115,11 +106,10 @@ def add_launch_arg(name: str, default_value=None, description=None):
         "path to the parameter file of random_downsample_filter",
     )
     add_launch_arg("use_intra_process", "true", "use ROS 2 component container communication")
-    add_launch_arg("use_pointcloud_container", "True", "use pointcloud container")
     add_launch_arg(
-        "pointcloud_container_name",
-        "/pointcloud_container",
-        "container name",
+        "lidar_container_name",
+        "/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container",
+        "container name of main lidar used for localization",
     )
 
     add_launch_arg(

launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml

@@ -3,12 +3,12 @@
   <!-- Interface parameters -->
   <arg name="mode" default="lidar" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
 
-  <!-- LiDAR parameters -->
+  <!-- Lidar parameters -->
   <arg name="input/pointcloud"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <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"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
 
   <!-- Camera parameters -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -34,9 +34,10 @@
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
   <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
-  <!-- Camera-LiDAR-Radar fusion based detection -->
+  <!-- Camera-Lidar-Radar fusion based detection -->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_radar_fusion&quot;')">
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml">
+    <!-- Camera-Lidar detectors -->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml">
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
       <arg name="image_raw0" value="$(var image_raw0)"/>
       <arg name="camera_info0" value="$(var camera_info0)"/>
@@ -57,23 +58,61 @@
       <arg name="image_number" value="$(var image_number)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-      <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 container_name)"/>
+      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
       <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
-      <arg name="input/radar" value="$(var input/radar)"/>
-      <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
-      <arg name="roi_cluster_fusion_param_path" value="$(var roi_cluster_fusion_param_path)"/>
-      <arg name="roi_pointcloud_fusion_param_path" value="$(var roi_pointcloud_fusion_param_path)"/>
+    </include>
+    <!-- Lidar dnn-based detectors-->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml">
+      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+    </include>
+    <!-- Radar detector-->
+    <group>
+      <push-ros-namespace namespace="radar"/>
+      <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/radar_detector.launch.xml">
+        <arg name="input/radar" value="$(var input/radar)"/>
+        <arg name="output/objects" value="far_objects"/>
+        <arg name="filter/angle_threshold" value="1.0472"/>
+        <arg name="filter/velocity_threshold" value="3.0"/>
+        <arg name="split/velocity_threshold" value="4.5"/>
+        <arg name="split_range" value="70.0"/>
+        <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
+        <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
+      </include>
+    </group>
+    <!-- Object merger -->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml">
+      <arg name="image_raw0" value="$(var image_raw0)"/>
+      <arg name="camera_info0" value="$(var camera_info0)"/>
+      <arg name="image_raw1" value="$(var image_raw1)"/>
+      <arg name="camera_info1" value="$(var camera_info1)"/>
+      <arg name="image_raw2" value="$(var image_raw2)"/>
+      <arg name="camera_info2" value="$(var camera_info2)"/>
+      <arg name="image_raw3" value="$(var image_raw3)"/>
+      <arg name="camera_info3" value="$(var camera_info3)"/>
+      <arg name="image_raw4" value="$(var image_raw4)"/>
+      <arg name="camera_info4" value="$(var camera_info4)"/>
+      <arg name="image_raw5" value="$(var image_raw5)"/>
+      <arg name="camera_info5" value="$(var camera_info5)"/>
+      <arg name="image_raw6" value="$(var image_raw6)"/>
+      <arg name="camera_info6" value="$(var camera_info6)"/>
+      <arg name="image_raw7" value="$(var image_raw7)"/>
+      <arg name="camera_info7" value="$(var camera_info7)"/>
+      <arg name="image_number" value="$(var image_number)"/>
+      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+      <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="roi_detected_object_fusion_param_path" value="$(var roi_detected_object_fusion_param_path)"/>
     </include>
   </group>
 
-  <!-- Camera-LiDAR fusion based detection -->
+  <!-- Camera-Lidar fusion based detection -->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_fusion&quot;')">
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml">
+    <!-- Camera-Lidar detectors -->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml">
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
       <arg name="image_raw0" value="$(var image_raw0)"/>
       <arg name="camera_info0" value="$(var camera_info0)"/>
@@ -94,47 +133,107 @@
       <arg name="image_number" value="$(var image_number)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-      <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 container_name)"/>
+      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
       <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
     </include>
+    <!-- Lidar dnn-based detectors-->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml">
+      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+    </include>
+    <!-- Object merger -->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml">
+      <arg name="image_raw0" value="$(var image_raw0)"/>
+      <arg name="camera_info0" value="$(var camera_info0)"/>
+      <arg name="image_raw1" value="$(var image_raw1)"/>
+      <arg name="camera_info1" value="$(var camera_info1)"/>
+      <arg name="image_raw2" value="$(var image_raw2)"/>
+      <arg name="camera_info2" value="$(var camera_info2)"/>
+      <arg name="image_raw3" value="$(var image_raw3)"/>
+      <arg name="camera_info3" value="$(var camera_info3)"/>
+      <arg name="image_raw4" value="$(var image_raw4)"/>
+      <arg name="camera_info4" value="$(var camera_info4)"/>
+      <arg name="image_raw5" value="$(var image_raw5)"/>
+      <arg name="camera_info5" value="$(var camera_info5)"/>
+      <arg name="image_raw6" value="$(var image_raw6)"/>
+      <arg name="camera_info6" value="$(var camera_info6)"/>
+      <arg name="image_raw7" value="$(var image_raw7)"/>
+      <arg name="camera_info7" value="$(var camera_info7)"/>
+      <arg name="image_number" value="$(var image_number)"/>
+      <arg name="use_object_filter" value="$(var use_object_filter)"/>
+      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+    </include>
   </group>
 
-  <!-- LiDAR-Radar fusion based detection -->
+  <!-- Lidar-Radar fusion based detection -->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;lidar_radar_fusion&quot;')">
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml">
+    <!-- Radar detector-->
+    <group>
+      <push-ros-namespace namespace="radar"/>
+      <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/radar_detector.launch.xml">
+        <arg name="input/radar" value="$(var input/radar)"/>
+        <arg name="output/objects" value="far_objects"/>
+        <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
+        <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
+      </include>
+    </group>
+    <!-- Lidar dnn-based detectors-->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml">
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+    </include>
+    <!-- Lidar rule-based detectors-->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml">
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-      <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 container_name)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
-      <arg name="input/radar" value="$(var input/radar)"/>
-      <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+    </include>
+    <!-- Lidar object merger -->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/merger/lidar_merger.launch.xml">
+      <arg name="output/objects" value="lidar/objects"/>
+      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+      <arg name="use_object_filter" value="$(var use_object_filter)"/>
+    </include>
+    <!-- Lidar-Radar object merger-->
+    <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml">
+      <arg name="input/objects" value="lidar/objects"/>
+      <arg name="input/radars" value="radar/noise_filtered_objects"/>
+      <arg name="output/objects" value="objects"/>
     </include>
   </group>
 
-  <!-- LiDAR based detection -->
+  <!-- Lidar based detection -->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;lidar&quot;')">
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/lidar_based_detection.launch.xml">
+    <!-- Lidar dnn-based detectors-->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml">
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+    </include>
+    <!-- Lidar rule-based detectors-->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml">
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-      <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 container_name)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
-      <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+    </include>
+    <!-- Lidar object merger -->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/merger/lidar_merger.launch.xml">
+      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+      <arg name="use_object_filter" value="$(var use_object_filter)"/>
     </include>
   </group>
 
   <!-- Radar based detection -->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;radar&quot;')">
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/radar_based_detection.launch.xml">
+    <!-- Radar detector-->
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/radar_detector.launch.xml">
       <arg name="input/radar" value="$(var input/radar)"/>
       <arg name="output/objects" value="objects"/>
       <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>