From 8158e531c851b6eb4f43409e947fb7417b68d7f2 Mon Sep 17 00:00:00 2001
From: kminoda <44218668+kminoda@users.noreply.github.com>
Date: Thu, 1 Feb 2024 17:43:47 +0900
Subject: [PATCH] feat(detection): add container option (#6228)

* feat(lidar_centerpoint,image_projection_based_fusion): add pointcloud_container option

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* revert lidar_perception_model

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* style(pre-commit): autofix

* fix: add options

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* fix: fix default param

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* update node name

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* fix: fix IfCondition

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* fix pointpainting namespace

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* fix: fix launch args

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* fix(euclidean_cluster): do not launch individual container when use_pointcloud_container is true

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* fix(euclidean_cluster): fix launch condition

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* fix(euclidean_cluster): fix launch condition

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* Update perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml

Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>

---------

Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>
---
 .../detector/camera_lidar_detector.launch.xml |  3 +
 .../detector/lidar_dnn_detector.launch.xml    |  3 +
 .../detector/lidar_rule_detector.launch.xml   |  3 +
 .../launch/euclidean_cluster.launch.py        | 13 ++++-
 .../launch/euclidean_cluster.launch.xml       |  5 ++
 ...xel_grid_based_euclidean_cluster.launch.py | 13 ++++-
 ...el_grid_based_euclidean_cluster.launch.xml |  5 ++
 .../launch/pointpainting_fusion.launch.xml    | 51 ++++++++++++++++-
 .../detection_class_remapper.param.yaml       |  6 +-
 .../launch/lidar_centerpoint.launch.xml       | 57 +++++++++++++------
 10 files changed, 135 insertions(+), 24 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
index 1c9201a9af8b3..2232feb6d7c67 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
@@ -85,6 +85,9 @@
       <arg name="model_path" value="$(var pointpainting_model_path)"/>
       <arg name="model_param_path" value="$(var lidar_model_param_path)/$(var lidar_detection_model).param.yaml"/>
       <arg name="class_remapper_param_path" value="$(var lidar_model_param_path)/detection_class_remapper.param.yaml"/>
+
+      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
     </include>
   </group>
 
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml
index 1a97659b357d8..5b5fabd4dd886 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml
@@ -22,6 +22,9 @@
         <arg name="model_path" value="$(var centerpoint_model_path)"/>
         <arg name="model_param_path" value="$(var lidar_model_param_path)/$(var centerpoint_model_name).param.yaml"/>
         <arg name="class_remapper_param_path" value="$(var lidar_model_param_path)/detection_class_remapper.param.yaml"/>
+
+        <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+        <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
       </include>
     </group>
   </group>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml
index ce34640bd3179..cec0c3bc05aac 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml
@@ -26,6 +26,9 @@
         <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
         <arg name="output_clusters" value="clusters"/>
         <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+
+        <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+        <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
       </include>
     </group>
 
diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py
index 66c25396a656e..b8d4f61a9cf28 100644
--- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py
+++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py
@@ -76,6 +76,13 @@ def load_composable_node_param(param_path):
         executable="component_container",
         composable_node_descriptions=[],
         output="screen",
+        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
+    )
+
+    target_container = (
+        LaunchConfiguration("pointcloud_container_name")
+        if IfCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
+        else container
     )
 
     use_low_height_pointcloud_loader = LoadComposableNodes(
@@ -83,13 +90,13 @@ def load_composable_node_param(param_path):
             low_height_cropbox_filter_component,
             use_low_height_euclidean_component,
         ],
-        target_container=container,
+        target_container=target_container,
         condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")),
     )
 
     disuse_low_height_pointcloud_loader = LoadComposableNodes(
         composable_node_descriptions=[disuse_low_height_euclidean_component],
-        target_container=container,
+        target_container=target_container,
         condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")),
     )
 
@@ -106,6 +113,8 @@ def add_launch_arg(name: str, default_value=None):
             add_launch_arg("input_map", "/map/pointcloud_map"),
             add_launch_arg("output_clusters", "clusters"),
             add_launch_arg("use_low_height_cropbox", "false"),
+            add_launch_arg("use_pointcloud_container", "false"),
+            add_launch_arg("pointcloud_container_name", "pointcloud_container"),
             add_launch_arg(
                 "euclidean_param_path",
                 [
diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml
index fd1ea82befae0..f4deeccf7b76c 100644
--- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml
+++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml
@@ -5,6 +5,8 @@
   <arg name="output_clusters" default="clusters"/>
   <arg name="use_low_height_cropbox" default="false"/>
   <arg name="euclidean_param_path" default="$(find-pkg-share euclidean_cluster)/config/euclidean_cluster.param.yaml"/>
+  <arg name="use_pointcloud_container" default="false"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
 
   <include file="$(find-pkg-share euclidean_cluster)/launch/euclidean_cluster.launch.py">
     <arg name="input_points_raw_list" value="$(var input_pointcloud)"/>
@@ -12,5 +14,8 @@
     <arg name="output_clusters" value="$(var output_clusters)"/>
     <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
     <arg name="euclidean_param_path" value="$(var euclidean_param_path)"/>
+
+    <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+    <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
   </include>
 </launch>
diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
index 00bcd4422bd3c..607e1bf30ccaa 100644
--- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
+++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
@@ -77,6 +77,13 @@ def load_composable_node_param(param_path):
         executable="component_container",
         composable_node_descriptions=[],
         output="screen",
+        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
+    )
+
+    target_container = (
+        LaunchConfiguration("pointcloud_container_name")
+        if IfCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
+        else container
     )
 
     use_low_height_pointcloud_loader = LoadComposableNodes(
@@ -84,13 +91,13 @@ def load_composable_node_param(param_path):
             low_height_cropbox_filter_component,
             use_low_height_euclidean_component,
         ],
-        target_container=container,
+        target_container=target_container,
         condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")),
     )
 
     disuse_low_height_pointcloud_loader = LoadComposableNodes(
         composable_node_descriptions=[disuse_low_height_euclidean_component],
-        target_container=container,
+        target_container=target_container,
         condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")),
     )
     return [
@@ -110,6 +117,8 @@ def add_launch_arg(name: str, default_value=None):
             add_launch_arg("input_map", "/map/pointcloud_map"),
             add_launch_arg("output_clusters", "clusters"),
             add_launch_arg("use_low_height_cropbox", "false"),
+            add_launch_arg("use_pointcloud_container", "false"),
+            add_launch_arg("pointcloud_container_name", "pointcloud_container"),
             add_launch_arg(
                 "voxel_grid_based_euclidean_param_path",
                 [
diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml
index b6a426dabfd12..3a7c685d8f449 100644
--- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml
+++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml
@@ -5,6 +5,8 @@
   <arg name="output_clusters" default="clusters"/>
   <arg name="use_low_height_cropbox" default="false"/>
   <arg name="voxel_grid_based_euclidean_param_path" default="$(find-pkg-share euclidean_cluster)/config/voxel_grid_based_euclidean_cluster.param.yaml"/>
+  <arg name="use_pointcloud_container" default="false"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
 
   <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.py">
     <arg name="input_pointcloud" value="$(var input_pointcloud)"/>
@@ -12,5 +14,8 @@
     <arg name="output_clusters" value="$(var output_clusters)"/>
     <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
     <arg name="voxel_grid_based_euclidean_param_path" value="$(var voxel_grid_based_euclidean_param_path)"/>
+
+    <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+    <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
   </include>
 </launch>
diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml
index 33781461fa1cc..db13d73e37fa7 100644
--- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml
+++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml
@@ -26,6 +26,9 @@
   <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
   <arg name="build_only" default="false" description="shutdown node after TensorRT engine file is built"/>
 
+  <arg name="use_pointcloud_container" default="false" description="use pointcloud_container"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container" description="pointcloud_container name"/>
+
   <!-- for eval variable-->
   <arg name="input_rois_number" default="$(var input/rois_number)"/>
 
@@ -38,7 +41,53 @@
   <arg name="input/image5" default="/image_raw5"/>
   <arg name="input/image6" default="/image_raw6"/>
   <arg name="input/image7" default="/image_raw7"/>
-  <group>
+
+  <group if="$(var use_pointcloud_container)">
+    <load_composable_node target="$(var pointcloud_container_name)">
+      <composable_node pkg="image_projection_based_fusion" plugin="image_projection_based_fusion::PointPaintingFusionNode" name="pointpainting">
+        <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
+        <remap from="~/output/objects" to="$(var output/objects)"/>
+        <param name="trt_precision" value="fp16"/>
+        <param name="encoder_onnx_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"/>
+        <param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>
+        <param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
+        <param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
+        <param from="$(var model_param_path)"/>
+        <param from="$(var sync_param_path)"/>
+        <param from="$(var class_remapper_param_path)"/>
+        <param name="rois_number" value="$(var input/rois_number)"/>
+        <param name="build_only" value="$(var build_only)"/>
+
+        <!-- rois, camera and info -->
+        <param name="input/rois0" value="$(var input/rois0)"/>
+        <param name="input/camera_info0" value="$(var input/camera_info0)"/>
+        <param name="input/image0" value="$(var input/image0)"/>
+        <param name="input/rois1" value="$(var input/rois1)"/>
+        <param name="input/camera_info1" value="$(var input/camera_info1)"/>
+        <param name="input/image1" value="$(var input/image1)"/>
+        <param name="input/rois2" value="$(var input/rois2)"/>
+        <param name="input/camera_info2" value="$(var input/camera_info2)"/>
+        <param name="input/image2" value="$(var input/image2)"/>
+        <param name="input/rois3" value="$(var input/rois3)"/>
+        <param name="input/camera_info3" value="$(var input/camera_info3)"/>
+        <param name="input/image3" value="$(var input/image3)"/>
+        <param name="input/rois4" value="$(var input/rois4)"/>
+        <param name="input/camera_info4" value="$(var input/camera_info4)"/>
+        <param name="input/image4" value="$(var input/image4)"/>
+        <param name="input/rois5" value="$(var input/rois5)"/>
+        <param name="input/camera_info5" value="$(var input/camera_info5)"/>
+        <param name="input/image5" value="$(var input/image5)"/>
+        <param name="input/rois6" value="$(var input/rois6)"/>
+        <param name="input/camera_info6" value="$(var input/camera_info6)"/>
+        <param name="input/image6" value="$(var input/image6)"/>
+        <param name="input/rois7" value="$(var input/rois7)"/>
+        <param name="input/camera_info7" value="$(var input/camera_info7)"/>
+        <param name="input/image7" value="$(var input/image7)"/>
+        <extra_arg name="use_intra_process_comms" value="true"/>
+      </composable_node>
+    </load_composable_node>
+  </group>
+  <group unless="$(var use_pointcloud_container)">
     <node pkg="image_projection_based_fusion" exec="pointpainting_fusion_node" name="pointpainting" output="screen">
       <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
       <remap from="~/output/objects" to="$(var output/objects)"/>
diff --git a/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml
index ed378ffa44a70..baea087c96bca 100644
--- a/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml
+++ b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml
@@ -29,9 +29,9 @@
     max_area_matrix:
       #UNKNOWN, CAR,   TRUCK,  BUS,    TRAILER,   MOTORBIKE, BICYCLE, PEDESTRIAN
       [ 0.000,  0.000,  0.000,  0.000,   0.000,   0.000,     0.000,   0.000,      #UNKNOWN
-        0.000,  0.000, 36.000,  0.000,     inf,   0.000,     0.000,   0.000,      #CAR
-        0.000,  0.000,  0.000,  0.000,     inf,   0.000,     0.000,   0.000,      #TRUCK
-        0.000,  0.000,  0.000,  0.000,     inf,   0.000,     0.000,   0.000,      #BUS
+        0.000,  0.000, 36.000,  0.000, 999.999,   0.000,     0.000,   0.000,      #CAR
+        0.000,  0.000,  0.000,  0.000, 999.999,   0.000,     0.000,   0.000,      #TRUCK
+        0.000,  0.000,  0.000,  0.000, 999.999,   0.000,     0.000,   0.000,      #BUS
         0.000,  0.000,  0.000,  0.000,   0.000,   0.000,     0.000,   0.000,      #TRAILER
         0.000,  0.000,  0.000,  0.000,   0.000,   0.000,     0.000,   0.000,      #MOTORBIKE
         0.000,  0.000,  0.000,  0.000,   0.000,   0.000,     0.000,   0.000,      #BICYCLE
diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml
index a7f181ab78ac6..1210875770510 100644
--- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml
+++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml
@@ -11,20 +11,45 @@
   <arg name="has_twist" default="false"/>
   <arg name="build_only" default="false" description="shutdown node after TensorRT engine file is built"/>
 
-  <node pkg="lidar_centerpoint" exec="lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
-    <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
-    <remap from="~/output/objects" to="$(var output/objects)"/>
-    <param name="score_threshold" value="$(var score_threshold)"/>
-    <param name="densification_world_frame_id" value="map"/>
-    <param name="densification_num_past_frames" value="1"/>
-    <param name="trt_precision" value="fp16"/>
-    <param name="has_twist" value="$(var has_twist)"/>
-    <param name="encoder_onnx_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"/>
-    <param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>
-    <param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
-    <param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
-    <param name="build_only" value="$(var build_only)"/>
-    <param from="$(var model_param_path)"/>
-    <param from="$(var class_remapper_param_path)"/>
-  </node>
+  <arg name="use_pointcloud_container" default="false" description="use pointcloud_container"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container" description="pointcloud_container name"/>
+
+  <group if="$(var use_pointcloud_container)">
+    <load_composable_node target="$(var pointcloud_container_name)">
+      <composable_node pkg="lidar_centerpoint" plugin="centerpoint::LidarCenterPointNode" name="lidar_centerpoint">
+        <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
+        <remap from="~/output/objects" to="$(var output/objects)"/>
+        <param name="score_threshold" value="$(var score_threshold)"/>
+        <param name="densification_world_frame_id" value="map"/>
+        <param name="densification_num_past_frames" value="1"/>
+        <param name="trt_precision" value="fp16"/>
+        <param name="has_twist" value="$(var has_twist)"/>
+        <param name="encoder_onnx_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"/>
+        <param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>
+        <param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
+        <param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
+        <param name="build_only" value="$(var build_only)"/>
+        <param from="$(var model_param_path)"/>
+        <param from="$(var class_remapper_param_path)"/>
+      </composable_node>
+    </load_composable_node>
+  </group>
+  <group unless="$(var use_pointcloud_container)">
+    <node pkg="lidar_centerpoint" exec="lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
+      <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
+      <remap from="~/output/objects" to="$(var output/objects)"/>
+      <param name="score_threshold" value="$(var score_threshold)"/>
+      <param name="densification_world_frame_id" value="map"/>
+      <param name="densification_num_past_frames" value="1"/>
+      <param name="trt_precision" value="fp16"/>
+      <param name="has_twist" value="$(var has_twist)"/>
+      <param name="encoder_onnx_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"/>
+      <param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>
+      <param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
+      <param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
+      <param name="build_only" value="$(var build_only)"/>
+      <param from="$(var model_param_path)"/>
+      <param from="$(var class_remapper_param_path)"/>
+    </node>
+  </group>
 </launch>