feat(detection): add container option (autowarefoundation#6228)

* feat(lidar_centerpoint,image_projection_based_fusion): add pointcloud_container option Signed-off-by: kminoda <[email protected]> * revert lidar_perception_model Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * fix: add options Signed-off-by: kminoda <[email protected]> * fix: fix default param Signed-off-by: kminoda <[email protected]> * update node name Signed-off-by: kminoda <[email protected]> * fix: fix IfCondition Signed-off-by: kminoda <[email protected]> * fix pointpainting namespace Signed-off-by: kminoda <[email protected]> * fix: fix launch args Signed-off-by: kminoda <[email protected]> * fix(euclidean_cluster): do not launch individual container when use_pointcloud_container is true Signed-off-by: kminoda <[email protected]> * fix(euclidean_cluster): fix launch condition Signed-off-by: kminoda <[email protected]> * fix(euclidean_cluster): fix launch condition Signed-off-by: kminoda <[email protected]> * Update perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml Co-authored-by: Shunsuke Miura <[email protected]> --------- Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shunsuke Miura <[email protected]> Co-authored-by: Kenzo Lobos Tsunekawa <[email protected]>
kminoda · Feb 1, 2024 · 1fa906b · 1fa906b
1 parent 5c0b890
commit 1fa906b
Show file tree

Hide file tree

Showing 10 changed files with 135 additions and 24 deletions.
diff --git a/...tion_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/...tion_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/...ception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/...ception_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/...eption_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/...eption_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
@@ -76,20 +76,27 @@ 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(
         composable_node_descriptions=[
             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
@@ -5,12 +5,17 @@
   <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)"/>
     <arg name="input_map" value="$(var input_map)"/>
     <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
@@ -77,20 +77,27 @@ 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(
         composable_node_descriptions=[
             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
@@ -5,12 +5,17 @@
   <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)"/>
     <arg name="input_map" value="$(var input_map)"/>
     <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
@@ -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
@@ -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
@@ -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>