From effa3fc7691755445ae08068f2fa7284fd97ec4b Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Wed, 2 Oct 2024 16:46:52 +0900 Subject: [PATCH 1/4] Merge https://github.com/knzo25/awsim_sensor_kit_launch/tree/test/awsim_nebula_integration Signed-off-by: Shintaro Sakoda --- .../launch/lidar.launch.xml | 36 +++- .../distortion_corrector_node.param.yaml | 5 + ...nch.py => nebula_node_container.launch.py} | 177 +++++++++++++++++- .../launch/velodyne_VLP16.launch.xml | 39 ++++ .../launch/velodyne_lidar.launch.xml | 14 -- 5 files changed, 247 insertions(+), 24 deletions(-) create mode 100644 common_awsim_sensor_launch/config/distortion_corrector_node.param.yaml rename common_awsim_sensor_launch/launch/{velodyne_node_container.launch.py => nebula_node_container.launch.py} (58%) create mode 100644 common_awsim_sensor_launch/launch/velodyne_VLP16.launch.xml delete mode 100644 common_awsim_sensor_launch/launch/velodyne_lidar.launch.xml diff --git a/awsim_sensor_kit_launch/launch/lidar.launch.xml b/awsim_sensor_kit_launch/launch/lidar.launch.xml index 79add12..0ed6703 100644 --- a/awsim_sensor_kit_launch/launch/lidar.launch.xml +++ b/awsim_sensor_kit_launch/launch/lidar.launch.xml @@ -1,4 +1,6 @@ + + @@ -9,27 +11,49 @@ - + + + + + + + + - - + + + + + + + + + + - - + + + + + + + + + + - diff --git a/common_awsim_sensor_launch/config/distortion_corrector_node.param.yaml b/common_awsim_sensor_launch/config/distortion_corrector_node.param.yaml new file mode 100644 index 0000000..3afa481 --- /dev/null +++ b/common_awsim_sensor_launch/config/distortion_corrector_node.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + base_frame: base_link + use_imu: true + use_3d_distortion_correction: false diff --git a/common_awsim_sensor_launch/launch/velodyne_node_container.launch.py b/common_awsim_sensor_launch/launch/nebula_node_container.launch.py similarity index 58% rename from common_awsim_sensor_launch/launch/velodyne_node_container.launch.py rename to common_awsim_sensor_launch/launch/nebula_node_container.launch.py index fb08701..e130dd5 100644 --- a/common_awsim_sensor_launch/launch/velodyne_node_container.launch.py +++ b/common_awsim_sensor_launch/launch/nebula_node_container.launch.py @@ -13,6 +13,7 @@ # limitations under the License. import launch +from ament_index_python.packages import get_package_share_directory from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration @@ -23,10 +24,18 @@ from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.substitutions import FindPackageShare +import os import yaml from ament_index_python.packages import get_package_share_directory from pathlib import Path +def get_lidar_make(sensor_name): + if sensor_name[:6].lower() == "pandar": + return "Hesai", ".csv" + elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: + return "Velodyne", ".yaml" + return "unrecognized_sensor_model" def get_vehicle_info(context): # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support @@ -62,6 +71,98 @@ def create_parameter_dict(*args): nodes = [] + # Model and make + sensor_model = LaunchConfiguration("sensor_model").perform(context) + sensor_make, sensor_extension = get_lidar_make(sensor_model) + nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") + + # Calibration file + sensor_calib_fp = os.path.join( + nebula_decoders_share_dir, + "calibration", + sensor_make.lower(), + sensor_model + sensor_extension, + ) + assert os.path.exists( + sensor_calib_fp + ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) + + nodes = [] + + nodes.append( + ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + ) + + nodes.append( + ComposableNode( + package="nebula_ros", + plugin=sensor_make + "DriverRosWrapper", + name=sensor_make.lower() + "_driver_ros_wrapper_node", + parameters=[ + { + "calibration_file": sensor_calib_fp, + "sensor_model": sensor_model, + **create_parameter_dict( + "host_ip", + "sensor_ip", + "data_port", + "return_mode", + "min_range", + "max_range", + "frame_id", + "scan_phase", + "cloud_min_angle", + "cloud_max_angle", + "dual_return_distance_threshold", + "setup_sensor", + "retry_hw", + ), + }, + ], + remappings=[ + # cSpell:ignore knzo25 + # TODO(knzo25): fix the remapping once nebula gets updated + ("pandar_points", "pointcloud_raw_ex"), + ("velodyne_points", "pointcloud_raw_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + + nodes.append( + ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwMonitorRosWrapper", + name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node", + parameters=[ + { + "sensor_model": sensor_model, + **create_parameter_dict( + "return_mode", + "frame_id", + "scan_phase", + "sensor_ip", + "host_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "rotation_speed", + "cloud_min_angle", + "cloud_max_angle", + "diag_span", + "dual_return_distance_threshold", + "delay_monitor_ms", + ), + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") cropbox_parameters["negative"] = True @@ -147,6 +248,45 @@ def create_parameter_dict(*args): composable_node_descriptions=nodes, ) + driver_component = ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwInterfaceRosWrapper", + # node is created in a global context, need to avoid name clash + name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", + parameters=[ + { + "sensor_model": sensor_model, + "calibration_file": sensor_calib_fp, + **create_parameter_dict( + "sensor_ip", + "host_ip", + "scan_phase", + "return_mode", + "frame_id", + "rotation_speed", + "data_port", + "gnss_port", + "cloud_min_angle", + "cloud_max_angle", + "packet_mtu_size", + "dual_return_distance_threshold", + "setup_sensor", + "ptp_profile", + "ptp_transport_type", + "ptp_switch_type", + "ptp_domain", + "retry_hw", + ), + } + ], + ) + + driver_component_loader = LoadComposableNodes( + composable_node_descriptions=[driver_component], + target_container=container, + condition=IfCondition(LaunchConfiguration("launch_driver")), + ) + distortion_component = ComposableNode( package="autoware_pointcloud_preprocessor", plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent", @@ -185,7 +325,7 @@ def create_parameter_dict(*args): condition=launch.conditions.UnlessCondition(LaunchConfiguration("use_distortion_corrector")), ) - return [container, distortion_loader, distortion_relay_loader] + return [container, driver_component_loader, distortion_loader, distortion_relay_loader] def generate_launch_description(): @@ -201,21 +341,50 @@ def add_launch_arg(name: str, default_value=None, description=None): get_package_share_directory("common_awsim_sensor_launch") ) + add_launch_arg("sensor_model", description="sensor model name") + add_launch_arg("config_file", "", description="sensor configuration file") + add_launch_arg("launch_driver", "True", "do launch driver") + add_launch_arg("setup_sensor", "True", "configure sensor") + add_launch_arg("retry_hw", "false", "retry hw") + add_launch_arg("sensor_ip", "192.168.1.201", "device ip address") + add_launch_arg("host_ip", "255.255.255.255", "host ip address") + add_launch_arg("scan_phase", "0.0") add_launch_arg("base_frame", "base_link", "base frame id") - add_launch_arg("container_name", "velodyne_composable_node_container", "container name") + add_launch_arg("min_range", "0.3", "minimum view range for Velodyne sensors") + add_launch_arg("max_range", "300.0", "maximum view range for Velodyne sensors") + add_launch_arg("cloud_min_angle", "0", "minimum view angle setting on device") + add_launch_arg("cloud_max_angle", "360", "maximum view angle setting on device") + add_launch_arg("data_port", "2368", "device data port number") + add_launch_arg("gnss_port", "2380", "device gnss port number") + add_launch_arg("packet_mtu_size", "1500", "packet mtu size") + add_launch_arg("rotation_speed", "600", "rotational frequency") + add_launch_arg("dual_return_distance_threshold", "0.1", "dual return distance threshold") + add_launch_arg("frame_id", "lidar", "frame id") add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox") add_launch_arg("output_frame", LaunchConfiguration("base_frame"), "use for cropbox") add_launch_arg( "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" ) add_launch_arg("use_multithread", "False", "use multithread") - add_launch_arg("use_intra_process", "False", "use ROS2 component container communication") add_launch_arg("output_as_sensor_frame", "False", "output final pointcloud in sensor frame") add_launch_arg("frame_id", "base_link", "frame id") add_launch_arg( "ring_outlier_filter_node_param_path", str(common_sensor_share_dir / "config" / "ring_outlier_filter_node.param.yaml"), - description="path to parameter file of ring outlier filter node", + description="path to parameter file of ring outlier filter node") + add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") + add_launch_arg("container_name", "nebula_node_container") + add_launch_arg("ptp_profile", "1588v2") + add_launch_arg("ptp_transport_type", "L2") + add_launch_arg("ptp_switch_type", "TSN") + add_launch_arg("ptp_domain", "0") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") + add_launch_arg("diag_span", "1000", "") + add_launch_arg("delay_monitor_ms", "2000", "") + + add_launch_arg( + "distortion_corrector_node_param_file", + [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"], ) set_container_executable = SetLaunchConfiguration( diff --git a/common_awsim_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_awsim_sensor_launch/launch/velodyne_VLP16.launch.xml new file mode 100644 index 0000000..3fee1dd --- /dev/null +++ b/common_awsim_sensor_launch/launch/velodyne_VLP16.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common_awsim_sensor_launch/launch/velodyne_lidar.launch.xml b/common_awsim_sensor_launch/launch/velodyne_lidar.launch.xml deleted file mode 100644 index 036ff39..0000000 --- a/common_awsim_sensor_launch/launch/velodyne_lidar.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - From d51a0001a4a9ee9086ab0086baf88a0fb8f8771c Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Wed, 2 Oct 2024 17:06:52 +0900 Subject: [PATCH 2/4] Fixed some code Signed-off-by: Shintaro Sakoda --- .../launch/nebula_node_container.launch.py | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/common_awsim_sensor_launch/launch/nebula_node_container.launch.py b/common_awsim_sensor_launch/launch/nebula_node_container.launch.py index e130dd5..b4543e2 100644 --- a/common_awsim_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_awsim_sensor_launch/launch/nebula_node_container.launch.py @@ -25,9 +25,7 @@ from launch_ros.descriptions import ComposableNode from launch_ros.parameter_descriptions import ParameterFile from launch_ros.substitutions import FindPackageShare -import os import yaml -from ament_index_python.packages import get_package_share_directory from pathlib import Path def get_lidar_make(sensor_name): @@ -74,18 +72,19 @@ def create_parameter_dict(*args): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) sensor_make, sensor_extension = get_lidar_make(sensor_model) - nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") + nebula_decoders_share_dir = Path(get_package_share_directory("nebula_decoders")) # Calibration file - sensor_calib_fp = os.path.join( - nebula_decoders_share_dir, - "calibration", - sensor_make.lower(), - sensor_model + sensor_extension, + sensor_calib_fp = ( + nebula_decoders_share_dir + / "calibration" + / sensor_make.lower() + / (sensor_model + sensor_extension) ) - assert os.path.exists( - sensor_calib_fp - ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) + assert ( + sensor_calib_fp.exists() + ), f"Sensor calib file under calibration/ was not found: {sensor_calib_fp}" + sensor_calib_fp = str(sensor_calib_fp) nodes = [] From d31859e95bab988721ef19176c60b11e065640b7 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Wed, 2 Oct 2024 17:07:28 +0900 Subject: [PATCH 3/4] Fixed order Signed-off-by: Shintaro Sakoda --- .../launch/nebula_node_container.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common_awsim_sensor_launch/launch/nebula_node_container.launch.py b/common_awsim_sensor_launch/launch/nebula_node_container.launch.py index b4543e2..e02a6a2 100644 --- a/common_awsim_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_awsim_sensor_launch/launch/nebula_node_container.launch.py @@ -13,7 +13,6 @@ # limitations under the License. import launch -from ament_index_python.packages import get_package_share_directory from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration @@ -26,6 +25,7 @@ from launch_ros.parameter_descriptions import ParameterFile from launch_ros.substitutions import FindPackageShare import yaml +from ament_index_python.packages import get_package_share_directory from pathlib import Path def get_lidar_make(sensor_name): From 0250b040550761c10eb848d5f7138e4e2da07f37 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Wed, 2 Oct 2024 17:16:55 +0900 Subject: [PATCH 4/4] FIxed order Signed-off-by: Shintaro Sakoda --- .../launch/nebula_node_container.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common_awsim_sensor_launch/launch/nebula_node_container.launch.py b/common_awsim_sensor_launch/launch/nebula_node_container.launch.py index e02a6a2..af2c655 100644 --- a/common_awsim_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_awsim_sensor_launch/launch/nebula_node_container.launch.py @@ -365,13 +365,13 @@ def add_launch_arg(name: str, default_value=None, description=None): "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" ) add_launch_arg("use_multithread", "False", "use multithread") + add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") add_launch_arg("output_as_sensor_frame", "False", "output final pointcloud in sensor frame") add_launch_arg("frame_id", "base_link", "frame id") add_launch_arg( "ring_outlier_filter_node_param_path", str(common_sensor_share_dir / "config" / "ring_outlier_filter_node.param.yaml"), description="path to parameter file of ring outlier filter node") - add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") add_launch_arg("container_name", "nebula_node_container") add_launch_arg("ptp_profile", "1588v2") add_launch_arg("ptp_transport_type", "L2")