From 6275063720f6fc94b5e2b477337b4246889c4c2a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Kaan=20=C3=87olak?= <kaancolak95@gmail.com>
Date: Fri, 13 Jan 2023 13:09:18 +0300
Subject: [PATCH] feat(sample_sensor_kit_launch): pass container to velodyne
 nodes (#48)

* feat(sample_sensor_kit_launch): pass container to velodyne nodes

Signed-off-by: Kaan Colak <kcolak@leodrive.ai>

* feat(sample_sensor_kit_launch): align true/false

Signed-off-by: Kaan Colak <kcolak@leodrive.ai>

Signed-off-by: Kaan Colak <kcolak@leodrive.ai>
---
 .../launch/velodyne_VLP16.launch.xml          |  4 +++
 .../launch/velodyne_VLS128.launch.xml         |  4 +++
 .../launch/velodyne_node_container.launch.py  | 30 +++++++++++++------
 .../launch/lidar.launch.xml                   |  8 +++++
 4 files changed, 37 insertions(+), 9 deletions(-)

diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml
index 83aeb55e..8b897fbb 100644
--- a/common_sensor_launch/launch/velodyne_VLP16.launch.xml
+++ b/common_sensor_launch/launch/velodyne_VLP16.launch.xml
@@ -14,6 +14,8 @@
   <arg name="view_direction" default="0.0"/>
   <arg name="view_width" default="6.28"/>
   <arg name="vehicle_mirror_param_file"/>
+  <arg name="use_pointcloud_container" default="false"/>
+  <arg name="container_name" default="velodyne_node_container"/>
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
@@ -31,6 +33,8 @@
     <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="false"/>
+    <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+    <arg name="container_name" value="$(var container_name)"/>
   </include>
 
   <!-- Velodyne Monitor -->
diff --git a/common_sensor_launch/launch/velodyne_VLS128.launch.xml b/common_sensor_launch/launch/velodyne_VLS128.launch.xml
index 48ea5bf9..27242193 100644
--- a/common_sensor_launch/launch/velodyne_VLS128.launch.xml
+++ b/common_sensor_launch/launch/velodyne_VLS128.launch.xml
@@ -17,6 +17,8 @@
   <arg name="view_direction" default="0.0"/>
   <arg name="view_width" default="6.28"/>
   <arg name="vehicle_mirror_param_file"/>
+  <arg name="use_pointcloud_container" default="false"/>
+  <arg name="container_name" default="velodyne_node_container"/>
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
@@ -34,6 +36,8 @@
     <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="true"/>
+    <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+    <arg name="container_name" value="$(var container_name)"/>
   </include>
 
   <!-- Velodyne Monitor -->
diff --git a/common_sensor_launch/launch/velodyne_node_container.launch.py b/common_sensor_launch/launch/velodyne_node_container.launch.py
index 0519112e..a1c5debb 100644
--- a/common_sensor_launch/launch/velodyne_node_container.launch.py
+++ b/common_sensor_launch/launch/velodyne_node_container.launch.py
@@ -164,14 +164,20 @@ def create_parameter_dict(*args):
         )
     )
 
-    # set container to run all required components in the same process
     container = ComposableNodeContainer(
-        # need unique name, otherwise all processes in same container and the node names then clash
-        name="velodyne_node_container",
+        name=LaunchConfiguration("container_name"),
         namespace="pointcloud_preprocessor",
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=nodes,
+        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
+        output="screen",
+    )
+
+    component_loader = LoadComposableNodes(
+        composable_node_descriptions=nodes,
+        target_container=LaunchConfiguration("container_name"),
+        condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
     )
 
     driver_component = ComposableNode(
@@ -198,15 +204,19 @@ def create_parameter_dict(*args):
         ],
     )
 
-    # one way to add a ComposableNode conditional on a launch argument to a
-    # container. The `ComposableNode` itself doesn't accept a condition
-    loader = LoadComposableNodes(
+    target_container = (
+        container
+        if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
+        else LaunchConfiguration("container_name")
+    )
+
+    driver_component_loader = LoadComposableNodes(
         composable_node_descriptions=[driver_component],
-        target_container=container,
-        condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")),
+        target_container=target_container,
+        condition=IfCondition(LaunchConfiguration("launch_driver")),
     )
 
-    return [container, loader]
+    return [container, component_loader, driver_component_loader]
 
 
 def generate_launch_description():
@@ -248,6 +258,8 @@ def add_launch_arg(name: str, default_value=None, description=None):
     )
     add_launch_arg("use_multithread", "False", "use multithread")
     add_launch_arg("use_intra_process", "False", "use ROS2 component container communication")
+    add_launch_arg("use_pointcloud_container", "false")
+    add_launch_arg("container_name", "velodyne_node_container")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",
diff --git a/sample_sensor_kit_launch/launch/lidar.launch.xml b/sample_sensor_kit_launch/launch/lidar.launch.xml
index bfdc3845..50aaf30f 100644
--- a/sample_sensor_kit_launch/launch/lidar.launch.xml
+++ b/sample_sensor_kit_launch/launch/lidar.launch.xml
@@ -20,6 +20,8 @@
         <!-- <arg name="sensor_timestamp" value="false" /> -->
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
+        <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+        <arg name="container_name" value="$(var pointcloud_container_name)"/>
       </include>
     </group>
 
@@ -34,6 +36,8 @@
         <!-- <arg name="sensor_timestamp" value="false" /> -->
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
+        <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+        <arg name="container_name" value="$(var pointcloud_container_name)"/>
       </include>
     </group>
 
@@ -48,6 +52,8 @@
         <!-- <arg name="sensor_timestamp" value="false" /> -->
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
+        <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+        <arg name="container_name" value="$(var pointcloud_container_name)"/>
       </include>
     </group>
 
@@ -62,6 +68,8 @@
         <!-- <arg name="sensor_timestamp" value="false" /> -->
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
+        <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
+        <arg name="container_name" value="$(var pointcloud_container_name)"/>
       </include>
     </group>