From bbf63986795729cfd24b300a01143e8b17ee5e5a Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 3 Jul 2024 16:42:52 +0900 Subject: [PATCH] feat(pointcloud_preprocessor): load_distortion_parameter_from_yaml (#93) * feat: load distortion parameter from yaml Signed-off-by: vividf * chore: fix spell error Signed-off-by: vividf * feat: modify the nebula_node_container based on tier4 internal common_sensor_launch Signed-off-by: vividf * chore: use ParameterFile to load param Signed-off-by: vividf --------- Signed-off-by: vividf --- common_sensor_launch/CMakeLists.txt | 1 + .../distortion_corrector_node.param.yaml | 5 ++++ .../launch/nebula_node_container.launch.py | 25 ++++++++++++++++--- 3 files changed, 28 insertions(+), 3 deletions(-) create mode 100644 common_sensor_launch/config/distortion_corrector_node.param.yaml diff --git a/common_sensor_launch/CMakeLists.txt b/common_sensor_launch/CMakeLists.txt index 0b3dcbd5..89bfc9f5 100644 --- a/common_sensor_launch/CMakeLists.txt +++ b/common_sensor_launch/CMakeLists.txt @@ -11,4 +11,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/common_sensor_launch/config/distortion_corrector_node.param.yaml b/common_sensor_launch/config/distortion_corrector_node.param.yaml new file mode 100644 index 00000000..3afa4816 --- /dev/null +++ b/common_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_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index fa4ae575..629d3332 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -25,6 +25,7 @@ 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 @@ -84,6 +85,12 @@ def create_parameter_dict(*args): sensor_calib_fp ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) + # Pointcloud preprocessor parameters + distortion_corrector_node_param = ParameterFile( + param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context), + allow_substs=True, + ) + nodes = [] nodes.append( @@ -184,6 +191,7 @@ def create_parameter_dict(*args): ("~/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")}], ) ) @@ -265,6 +273,8 @@ def add_launch_arg(name: str, default_value=None, description=None): DeclareLaunchArgument(name, default_value=default_value, description=description) ) + common_sensor_share_dir = get_package_share_directory("common_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") @@ -285,13 +295,22 @@ def add_launch_arg(name: str, default_value=None, description=None): 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 ROS 2 component container communication") add_launch_arg("lidar_container_name", "nebula_node_container") 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" + ) + add_launch_arg( + "distortion_correction_node_param_path", + os.path.join( + common_sensor_share_dir, + "config", + "distortion_corrector_node.param.yaml", + ), + description="path to parameter file of distortion correction node", + ) set_container_executable = SetLaunchConfiguration( "container_executable",