From f649db1fa0df1dbb77f553305acdcc01662057d9 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 13 Nov 2024 10:14:24 +0900 Subject: [PATCH] feature(single_lidar_common_launch): load from parameter (#3) Signed-off-by: vividf --- .../distortion_corrector_node.param.yaml | 1 + .../ring_outlier_filter_node.param.yaml | 14 +++++++ .../launch/nebula_node_container.launch.py | 40 ++++++++++++------- 3 files changed, 41 insertions(+), 14 deletions(-) create mode 100644 single_lidar_common_launch/config/ring_outlier_filter_node.param.yaml diff --git a/single_lidar_common_launch/config/distortion_corrector_node.param.yaml b/single_lidar_common_launch/config/distortion_corrector_node.param.yaml index 3afa481..c558756 100644 --- a/single_lidar_common_launch/config/distortion_corrector_node.param.yaml +++ b/single_lidar_common_launch/config/distortion_corrector_node.param.yaml @@ -3,3 +3,4 @@ base_frame: base_link use_imu: true use_3d_distortion_correction: false + update_azimuth_and_distance: false diff --git a/single_lidar_common_launch/config/ring_outlier_filter_node.param.yaml b/single_lidar_common_launch/config/ring_outlier_filter_node.param.yaml new file mode 100644 index 0000000..76bf689 --- /dev/null +++ b/single_lidar_common_launch/config/ring_outlier_filter_node.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + distance_ratio: 1.03 + object_length_threshold: 0.1 + num_points_threshold: 4 + max_rings_num: 128 + max_points_num_per_ring: 4000 + publish_outlier_pointcloud: false + min_azimuth_deg: 0.0 + max_azimuth_deg: 360.0 + max_distance: 12.0 + vertical_bins: 128 + horizontal_bins: 36 + noise_threshold: 2 diff --git a/single_lidar_common_launch/launch/nebula_node_container.launch.py b/single_lidar_common_launch/launch/nebula_node_container.launch.py index b4c57a6..b2ea470 100644 --- a/single_lidar_common_launch/launch/nebula_node_container.launch.py +++ b/single_lidar_common_launch/launch/nebula_node_container.launch.py @@ -90,6 +90,10 @@ def create_parameter_dict(*args): param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context), allow_substs=True, ) + ring_outlier_filter_node_param = ParameterFile( + param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context), + allow_substs=True, + ) nodes = [] @@ -152,8 +156,8 @@ def create_parameter_dict(*args): nodes.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", name="crop_box_filter_self", remappings=[ ("input", "pointcloud_raw_ex"), @@ -174,8 +178,8 @@ def create_parameter_dict(*args): nodes.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::CropBoxFilterComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", name="crop_box_filter_mirror", remappings=[ ("input", "self_cropped/pointcloud_ex"), @@ -188,8 +192,8 @@ def create_parameter_dict(*args): nodes.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::DistortionCorrectorComponent", + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent", name="distortion_corrector_node", remappings=[ ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), @@ -204,21 +208,19 @@ 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).lower() == "true": - ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")} + ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")} else: - ring_outlier_filter_parameters = { - "output_frame": "" - } # keep the output frame as the input frame + ring_outlier_output_frame = {"output_frame": ""} # keep the output frame as the input frame nodes.append( ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + 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_parameters], + parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -226,7 +228,7 @@ def create_parameter_dict(*args): # set container to run all required components in the same process container = ComposableNodeContainer( name=LaunchConfiguration("container_name"), - namespace="pointcloud_preprocessor", + namespace="autoware_pointcloud_preprocessor", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=nodes, @@ -274,6 +276,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg( "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" ) + add_launch_arg( "distortion_correction_node_param_path", os.path.join( @@ -283,6 +286,15 @@ def add_launch_arg(name: str, default_value=None, description=None): ), description="path to parameter file of distortion correction node", ) + add_launch_arg( + "ring_outlier_filter_node_param_path", + os.path.join( + common_sensor_share_dir, + "config", + "ring_outlier_filter_node.param.yaml", + ), + description="path to parameter file of ring outlier filter node", + ) set_container_executable = SetLaunchConfiguration( "container_executable",