diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py
index a5bdb08d..85d36ae2 100644
--- a/common_sensor_launch/launch/nebula_node_container.launch.py
+++ b/common_sensor_launch/launch/nebula_node_container.launch.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Tier IV, Inc. All rights reserved.
+# Copyright 2024 Tier IV, Inc. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -23,6 +23,7 @@
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
from launch_ros.parameter_descriptions import ParameterFile
import yaml
@@ -101,13 +102,14 @@ def create_parameter_dict(*args):
nodes = []
- nodes.append(
- ComposableNode(
- package="glog_component",
- plugin="GlogComponent",
- name="glog_component",
+ if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context):
+ nodes.append(
+ ComposableNode(
+ package="glog_component",
+ plugin="GlogComponent",
+ name="glog_component",
+ )
)
- )
nodes.append(
ComposableNode(
@@ -149,87 +151,155 @@ def create_parameter_dict(*args):
)
)
- cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
- cropbox_parameters["negative"] = True
-
- vehicle_info = get_vehicle_info(context)
- cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"]
- cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"]
- cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"]
- cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"]
- cropbox_parameters["min_z"] = vehicle_info["min_height_offset"]
- cropbox_parameters["max_z"] = vehicle_info["max_height_offset"]
+ if IfCondition(LaunchConfiguration("use_cuda_preprocessor")).evaluate(context):
+ nodes.append(
+ ComposableNode(
+ package="autoware_cuda_pointcloud_preprocessor",
+ plugin="autoware::cuda_organized_pointcloud_adapter::CudaOrganizedPointcloudAdapterNode",
+ name="cuda_organized_pointcloud_adapter_node",
+ remappings=[
+ ("~/input/pointcloud", "pointcloud_raw_ex"),
+ ("~/output/pointcloud", "cuda_points"),
+ ("~/output/pointcloud/cuda", "cuda_points/cuda"),
+ ],
+ # The whole node can not set use_intra_process due to type negotiation internal topics
+ # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+ )
+ )
- nodes.append(
- ComposableNode(
- package="autoware_pointcloud_preprocessor",
- plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent",
- name="crop_box_filter_self",
- remappings=[
- ("input", "pointcloud_raw_ex"),
- ("output", "self_cropped/pointcloud_ex"),
- ],
- parameters=[cropbox_parameters],
- extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+ preprocessor_parameters = {}
+
+ vehicle_info = get_vehicle_info(context)
+ preprocessor_parameters["self_crop.min_x"] = vehicle_info["min_longitudinal_offset"]
+ preprocessor_parameters["self_crop.max_x"] = vehicle_info["max_longitudinal_offset"]
+ preprocessor_parameters["self_crop.min_y"] = vehicle_info["min_lateral_offset"]
+ preprocessor_parameters["self_crop.max_y"] = vehicle_info["max_lateral_offset"]
+ preprocessor_parameters["self_crop.min_z"] = vehicle_info["min_height_offset"]
+ preprocessor_parameters["self_crop.max_z"] = vehicle_info["max_height_offset"]
+
+ mirror_info = get_vehicle_mirror_info(context)
+ preprocessor_parameters["mirror_crop.min_x"] = mirror_info["min_longitudinal_offset"]
+ preprocessor_parameters["mirror_crop.max_x"] = mirror_info["max_longitudinal_offset"]
+ preprocessor_parameters["mirror_crop.min_y"] = mirror_info["min_lateral_offset"]
+ preprocessor_parameters["mirror_crop.max_y"] = mirror_info["max_lateral_offset"]
+ preprocessor_parameters["mirror_crop.min_z"] = mirror_info["min_height_offset"]
+ preprocessor_parameters["mirror_crop.max_z"] = mirror_info["max_height_offset"]
+
+ nodes.append(
+ ComposableNode(
+ package="autoware_cuda_pointcloud_preprocessor",
+ plugin="autoware::cuda_pointcloud_preprocessor::CudaPointcloudPreprocessorNode",
+ name="cuda_pointcloud_preprocessor_node",
+ parameters=[
+ preprocessor_parameters,
+ distortion_corrector_node_param,
+ ring_outlier_filter_node_param,
+ ],
+ remappings=[
+ ("~/input/pointcloud", "cuda_points"),
+ ("~/input/pointcloud/cuda", "cuda_points/cuda"),
+ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
+ ("~/input/imu", "/sensing/imu/imu_data"),
+ ("~/output/pointcloud", "pointcloud_before_sync"),
+ ("~/output/pointcloud/cuda", "pointcloud_before_sync/cuda"),
+ ],
+ # The whole node can not set use_intra_process due to type negotiation internal topics
+ # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+ )
)
- )
- mirror_info = get_vehicle_mirror_info(context)
- cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"]
- cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"]
- cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"]
- cropbox_parameters["max_y"] = mirror_info["max_lateral_offset"]
- cropbox_parameters["min_z"] = mirror_info["min_height_offset"]
- cropbox_parameters["max_z"] = mirror_info["max_height_offset"]
+ else:
+ cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
+ cropbox_parameters["negative"] = True
+
+ vehicle_info = get_vehicle_info(context)
+ cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"]
+ cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"]
+ cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"]
+ cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"]
+ cropbox_parameters["min_z"] = vehicle_info["min_height_offset"]
+ cropbox_parameters["max_z"] = vehicle_info["max_height_offset"]
+
+ nodes.append(
+ ComposableNode(
+ package="autoware_pointcloud_preprocessor",
+ plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent",
+ name="crop_box_filter_self",
+ remappings=[
+ ("input", "pointcloud_raw_ex"),
+ ("output", "self_cropped/pointcloud_ex"),
+ ],
+ parameters=[cropbox_parameters],
+ extra_arguments=[
+ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+ ],
+ )
+ )
- nodes.append(
- ComposableNode(
- package="autoware_pointcloud_preprocessor",
- plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent",
- name="crop_box_filter_mirror",
- remappings=[
- ("input", "self_cropped/pointcloud_ex"),
- ("output", "mirror_cropped/pointcloud_ex"),
- ],
- parameters=[cropbox_parameters],
- extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+ mirror_info = get_vehicle_mirror_info(context)
+ cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"]
+ cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"]
+ cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"]
+ cropbox_parameters["max_y"] = mirror_info["max_lateral_offset"]
+ cropbox_parameters["min_z"] = mirror_info["min_height_offset"]
+ cropbox_parameters["max_z"] = mirror_info["max_height_offset"]
+
+ nodes.append(
+ ComposableNode(
+ package="autoware_pointcloud_preprocessor",
+ plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent",
+ name="crop_box_filter_mirror",
+ remappings=[
+ ("input", "self_cropped/pointcloud_ex"),
+ ("output", "mirror_cropped/pointcloud_ex"),
+ ],
+ parameters=[cropbox_parameters],
+ extra_arguments=[
+ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+ ],
+ )
)
- )
- nodes.append(
- ComposableNode(
- package="autoware_pointcloud_preprocessor",
- plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent",
- name="distortion_corrector_node",
- remappings=[
- ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
- ("~/input/imu", "/sensing/imu/imu_data"),
- ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"),
- ("~/output/pointcloud", "rectified/pointcloud_ex"),
- ],
- parameters=[distortion_corrector_node_param],
- extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+ nodes.append(
+ ComposableNode(
+ package="autoware_pointcloud_preprocessor",
+ plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent",
+ name="distortion_corrector_node",
+ remappings=[
+ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
+ ("~/input/imu", "/sensing/imu/imu_data"),
+ ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"),
+ ("~/output/pointcloud", "rectified/pointcloud_ex"),
+ ],
+ parameters=[distortion_corrector_node_param],
+ extra_arguments=[
+ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+ ],
+ )
)
- )
- # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
- if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true":
- ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
- else:
- ring_outlier_output_frame = {"output_frame": ""} # keep the output frame as the input frame
- nodes.append(
- ComposableNode(
- package="autoware_pointcloud_preprocessor",
- plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent",
- name="ring_outlier_filter",
- remappings=[
- ("input", "rectified/pointcloud_ex"),
- ("output", "pointcloud_before_sync"),
- ],
- parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame],
- extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+ # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
+ if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true":
+ ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
+ else:
+ ring_outlier_output_frame = {
+ "output_frame": ""
+ } # keep the output frame as the input frame
+ nodes.append(
+ ComposableNode(
+ package="autoware_pointcloud_preprocessor",
+ plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent",
+ name="ring_outlier_filter",
+ remappings=[
+ ("input", "rectified/pointcloud_ex"),
+ ("output", "pointcloud_before_sync"),
+ ],
+ parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame],
+ extra_arguments=[
+ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+ ],
+ )
)
- )
# set container to run all required components in the same process
container = ComposableNodeContainer(
@@ -238,10 +308,17 @@ def create_parameter_dict(*args):
package="rclcpp_components",
executable=LaunchConfiguration("container_executable"),
composable_node_descriptions=nodes,
+ condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
output="both",
)
- return [container]
+ load_composable_nodes = LoadComposableNodes(
+ composable_node_descriptions=nodes,
+ target_container=LaunchConfiguration("pointcloud_container_name"),
+ condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
+ )
+
+ return [container, load_composable_nodes]
def generate_launch_description():
@@ -278,6 +355,9 @@ 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("lidar_container_name", "nebula_node_container")
+ add_launch_arg("pointcloud_container_name", "pointcloud_container")
+ add_launch_arg("use_pointcloud_container", "False")
+ add_launch_arg("use_cuda_preprocessor", "False")
add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
add_launch_arg(
"vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml"
diff --git a/common_sensor_launch/launch/robosense_Bpearl.launch.xml b/common_sensor_launch/launch/robosense_Bpearl.launch.xml
index 8fa49ef1..7b21cc01 100644
--- a/common_sensor_launch/launch/robosense_Bpearl.launch.xml
+++ b/common_sensor_launch/launch/robosense_Bpearl.launch.xml
@@ -15,6 +15,9 @@
+
+
+
@@ -33,5 +36,8 @@
+
+
+
diff --git a/common_sensor_launch/launch/robosense_Helios.launch.xml b/common_sensor_launch/launch/robosense_Helios.launch.xml
index 09addc20..9fa510d4 100644
--- a/common_sensor_launch/launch/robosense_Helios.launch.xml
+++ b/common_sensor_launch/launch/robosense_Helios.launch.xml
@@ -15,6 +15,9 @@
+
+
+
@@ -33,5 +36,8 @@
+
+
+
diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml
index f0fcc075..5991128e 100644
--- a/common_sensor_launch/launch/velodyne_VLP16.launch.xml
+++ b/common_sensor_launch/launch/velodyne_VLP16.launch.xml
@@ -14,6 +14,9 @@
+
+
+
@@ -32,6 +35,9 @@
+
+
+
diff --git a/common_sensor_launch/launch/velodyne_VLS128.launch.xml b/common_sensor_launch/launch/velodyne_VLS128.launch.xml
index d2797ac8..3b95be28 100644
--- a/common_sensor_launch/launch/velodyne_VLS128.launch.xml
+++ b/common_sensor_launch/launch/velodyne_VLS128.launch.xml
@@ -14,6 +14,9 @@
+
+
+
@@ -32,6 +35,9 @@
+
+
+
diff --git a/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml b/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml
new file mode 100644
index 00000000..d05c40de
--- /dev/null
+++ b/sample_sensor_kit_launch/config/concatenate_and_time_sync_node.param.yaml
@@ -0,0 +1,23 @@
+/**:
+ ros__parameters:
+ use_cuda: true
+ debug_mode: false
+ has_static_tf_only: false
+ rosbag_replay: false
+ rosbag_length: 20.0
+ maximum_queue_size: 5
+ timeout_sec: 0.2
+ is_motion_compensated: true
+ publish_synchronized_pointcloud: true
+ keep_input_frame_in_synchronized_pointcloud: true
+ publish_previous_but_late_pointcloud: false
+ synchronized_pointcloud_postfix: pointcloud
+ input_twist_topic_type: twist
+ input_topics: [
+ "/sensing/lidar/right/pointcloud_before_sync",
+ "/sensing/lidar/top/pointcloud_before_sync",
+ "/sensing/lidar/left/pointcloud_before_sync",
+ ]
+ output_frame: base_link
+ lidar_timestamp_offsets: [0.0, 0.015, 0.016]
+ lidar_timestamp_noise_window: [0.01, 0.01, 0.01]
diff --git a/sample_sensor_kit_launch/launch/lidar.launch.xml b/sample_sensor_kit_launch/launch/lidar.launch.xml
index 8374aa24..b8133fce 100644
--- a/sample_sensor_kit_launch/launch/lidar.launch.xml
+++ b/sample_sensor_kit_launch/launch/lidar.launch.xml
@@ -5,6 +5,8 @@
+
+
@@ -21,6 +23,9 @@
+
+
+
@@ -38,6 +43,9 @@
+
+
+
@@ -55,6 +63,9 @@
+
+
+
@@ -72,6 +83,9 @@
+
+
+
diff --git a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py
index c29d74e3..b506d1fb 100644
--- a/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py
+++ b/sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py
@@ -12,7 +12,9 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+import os
+from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
@@ -22,9 +24,18 @@
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterFile
def launch_setup(context, *args, **kwargs):
+ # concatenate node parameters
+ concatenate_and_time_sync_node_param = ParameterFile(
+ param_file=LaunchConfiguration("concatenate_and_time_sync_node_param_path").perform(
+ context
+ ),
+ allow_substs=True,
+ )
+
# set concat filter as a component
concat_component = ComposableNode(
package="autoware_pointcloud_preprocessor",
@@ -34,19 +45,9 @@ def launch_setup(context, *args, **kwargs):
("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
("output", "concatenated/pointcloud"),
],
- parameters=[
- {
- "input_topics": [
- "/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",
- "publish_synchronized_pointcloud": True,
- }
- ],
- extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+ parameters=[concatenate_and_time_sync_node_param],
+ # The whole node can not set use_intra_process due to type negotiation internal topics
+ # extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
# load concat or passthrough filter
@@ -65,10 +66,20 @@ def generate_launch_description():
def add_launch_arg(name: str, default_value=None):
launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
+ sample_sensor_kit_launch_share_dir = get_package_share_directory("sample_sensor_kit_launch")
+
add_launch_arg("base_frame", "base_link")
add_launch_arg("use_multithread", "False")
add_launch_arg("use_intra_process", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
+ add_launch_arg(
+ "concatenate_and_time_sync_node_param_path",
+ os.path.join(
+ sample_sensor_kit_launch_share_dir,
+ "config",
+ "concatenate_and_time_sync_node.param.yaml",
+ ),
+ )
set_container_executable = SetLaunchConfiguration(
"container_executable",