Merge branch 'main' into feat/separate_concatenate_node

common_sensor_launch/launch/nebula_node_container.launch.py

@@ -86,6 +86,14 @@ def create_parameter_dict(*args):
 
     nodes = []
 
+    nodes.append(
+        ComposableNode(
+            package="glog_component",
+            plugin="GlogComponent",
+            name="glog_component",
+        )
+    )
+
     nodes.append(
         ComposableNode(
             package="nebula_ros",
@@ -180,15 +188,23 @@ def create_parameter_dict(*args):
         )
     )
 
+    # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
+    if LaunchConfiguration("output_as_sensor_frame").perform(context):
+        ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")}
+    else:
+        ring_outlier_filter_parameters = {
+            "output_frame": ""
+        }  # keep the output frame as the input frame
     nodes.append(
         ComposableNode(
             package="pointcloud_preprocessor",
             plugin="pointcloud_preprocessor::RingOutlierFilterComponent",
             name="ring_outlier_filter",
             remappings=[
                 ("input", "rectified/pointcloud_ex"),
-                ("output", "pointcloud"),
+                ("output", "pointcloud_before_sync"),
             ],
+            parameters=[ring_outlier_filter_parameters],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
     )
@@ -200,14 +216,7 @@ def create_parameter_dict(*args):
         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")),
+        output="both",
     )
 
     driver_component = ComposableNode(
@@ -238,19 +247,13 @@ def create_parameter_dict(*args):
         ],
     )
 
-    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=target_container,
+        target_container=container,
         condition=IfCondition(LaunchConfiguration("launch_driver")),
     )
 
-    return [container, component_loader, driver_component_loader]
+    return [container, driver_component_loader]
 
 
 def generate_launch_description():
@@ -287,8 +290,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 ROS 2 component container communication")
-    add_launch_arg("use_pointcloud_container", "false")
-    add_launch_arg("container_name", "nebula_node_container")
+    add_launch_arg("lidar_container_name", "nebula_node_container")
+    add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",

common_sensor_launch/launch/velodyne_VLP16.launch.xml

@@ -13,7 +13,6 @@
   <arg name="cloud_min_angle" default="0"/>
   <arg name="cloud_max_angle" default="360"/>
   <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/nebula_node_container.launch.py">
@@ -32,7 +31,6 @@
     <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>
 

common_sensor_launch/launch/velodyne_VLS128.launch.xml

@@ -13,7 +13,6 @@
   <arg name="cloud_min_angle" default="0"/>
   <arg name="cloud_max_angle" default="360"/>
   <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/nebula_node_container.launch.py">
@@ -32,7 +31,6 @@
     <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>
 

sample_sensor_kit_launch/launch/gnss.launch.xml

@@ -33,8 +33,6 @@
       <arg name="output_topic_gnss_fixed" value="fixed"/>
 
       <arg name="use_gnss_ins_orientation" value="true"/>
-
-      <arg name="gnss_frame" value="gnss_link"/>
     </include>
   </group>
 </launch>

sample_sensor_kit_launch/launch/lidar.launch.xml

@@ -4,7 +4,6 @@
   <arg name="use_concat_filter" default="true"/>
   <arg name="vehicle_id" default="$(env VEHICLE_ID default)"/>
   <arg name="vehicle_mirror_param_file"/>
-  <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
 
   <group>
@@ -21,8 +20,7 @@
         <arg name="scan_phase" value="300.0"/>
         <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)"/>
+        <arg name="container_name" value="pointcloud_container"/>
       </include>
     </group>
 
@@ -39,8 +37,7 @@
         <arg name="cloud_max_angle" value="60"/>
         <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)"/>
+        <arg name="container_name" value="pointcloud_container"/>
       </include>
     </group>
 
@@ -57,8 +54,7 @@
         <arg name="cloud_max_angle" value="60"/>
         <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)"/>
+        <arg name="container_name" value="pointcloud_container"/>
       </include>
     </group>
 
@@ -75,17 +71,15 @@
         <arg name="cloud_max_angle" value="60"/>
         <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)"/>
+        <arg name="container_name" value="pointcloud_container"/>
       </include>
     </group>
 
     <include file="$(find-pkg-share sample_sensor_kit_launch)/launch/pointcloud_preprocessor.launch.py">
       <arg name="base_frame" value="base_link"/>
       <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 pointcloud_container_name)"/>
+      <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
     </include>
   </group>
 </launch>

sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py

@@ -20,7 +20,6 @@
 from launch.conditions import IfCondition
 from launch.conditions import UnlessCondition
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
 
@@ -49,9 +48,9 @@ def launch_setup(context, *args, **kwargs):
             parameters=[
                 {
                     "input_topics": [
-                        "/sensing/lidar/top/pointcloud",
-                        "/sensing/lidar/left/pointcloud",
-                        "/sensing/lidar/right/pointcloud",
+                        "/sensing/lidar/top/pointcloud_before_sync",
+                        "/sensing/lidar/left/pointcloud_before_sync",
+                        "/sensing/lidar/right/pointcloud_before_sync",
                     ],
                     "output_frame": LaunchConfiguration("base_frame"),
                     "input_twist_topic_type": "twist",
@@ -71,9 +70,9 @@ def launch_setup(context, *args, **kwargs):
             parameters=[
                 {
                     "input_topics": [
-                        "/sensing/lidar/top/pointcloud",
-                        "/sensing/lidar/left/pointcloud",
-                        "/sensing/lidar/right/pointcloud",
+                        "/sensing/lidar/top/pointcloud_before_sync",
+                        "/sensing/lidar/left/pointcloud_before_sync",
+                        "/sensing/lidar/right/pointcloud_before_sync",
                     ],
                     "output_frame": LaunchConfiguration("base_frame"),
                     "approximate_sync": True,
@@ -105,7 +104,7 @@ def launch_setup(context, *args, **kwargs):
 
     # set container to run all required components in the same process
     container = ComposableNodeContainer(
-        name=LaunchConfiguration("container_name"),
+        name=LaunchConfiguration("pointcloud_container_name"),
         namespace="",
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
@@ -117,7 +116,7 @@ def launch_setup(context, *args, **kwargs):
     target_container = (
         container
         if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
-        else LaunchConfiguration("container_name")
+        else LaunchConfiguration("pointcloud_container_name")
     )
 
     # load concat or passthrough filter
@@ -127,7 +126,7 @@ def launch_setup(context, *args, **kwargs):
         condition=IfCondition(LaunchConfiguration("use_concat_filter")),
     )
 
-    return [container, concat_loader]
+    return [concat_loader]
 
 
 def generate_launch_description():
@@ -140,8 +139,8 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("use_multithread", "False")
     add_launch_arg("use_intra_process", "False")
     add_launch_arg("use_pointcloud_container", "False")
-    add_launch_arg("container_name", "pointcloud_preprocessor_container")
     add_launch_arg("concatenate_data__separate_pointcloud_sync_and_concatenate_nodes", "True")
+    add_launch_arg("pointcloud_container_name", "pointcloud_container")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",

sample_sensor_kit_launch/launch/sensing.launch.xml

@@ -1,7 +1,6 @@
 <launch>
   <arg name="launch_driver" default="true" description="do launch driver"/>
   <arg name="vehicle_mirror_param_file" description="path to the file of vehicle mirror position yaml"/>
-  <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
   <arg name="vehicle_id" default="$(env VEHICLE_ID default)"/>
 
@@ -10,7 +9,6 @@
     <include file="$(find-pkg-share sample_sensor_kit_launch)/launch/lidar.launch.xml">
       <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="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
     </include>