From d5c5822eb659c145790ac2fe2f94363ebfa24fbb Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 8 Feb 2024 09:02:20 +0900 Subject: [PATCH 01/10] feat: remove use_pointcloud_container (#200) * feat: remove use_pointcloud_container Signed-off-by: kminoda * fix: remove unnecessary import Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> --- aip_x1_launch/launch/lidar.launch.xml | 2 -- .../launch/pointcloud_preprocessor.launch.py | 24 ++----------------- aip_x1_launch/launch/sensing.launch.xml | 2 -- aip_x2_launch/launch/lidar.launch.xml | 2 -- .../launch/pointcloud_preprocessor.launch.py | 24 ++----------------- aip_x2_launch/launch/sensing.launch.xml | 2 -- aip_xx1_launch/launch/lidar.launch.xml | 2 -- .../launch/pointcloud_preprocessor.launch.py | 23 ++---------------- aip_xx1_launch/launch/sensing.launch.xml | 2 -- 9 files changed, 6 insertions(+), 77 deletions(-) diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml index a6ceb956..56066df5 100644 --- a/aip_x1_launch/launch/lidar.launch.xml +++ b/aip_x1_launch/launch/lidar.launch.xml @@ -4,7 +4,6 @@ - @@ -57,7 +56,6 @@ - diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index 8d9892b0..ff012c8a 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -19,7 +19,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 @@ -50,30 +49,13 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - # set container to run all required components in the same process - container = ComposableNodeContainer( - name=LaunchConfiguration("individual_container_name"), - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[], - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - - target_container = ( - container - if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("pointcloud_container_name") - ) - # load concat or passthrough filter concat_loader = LoadComposableNodes( composable_node_descriptions=[concat_component], - target_container=target_container, + target_container=LaunchConfiguration("pointcloud_container_name"), ) - return [container, concat_loader] + return [concat_loader] def generate_launch_description(): @@ -85,9 +67,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("base_frame", "base_link") add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") - add_launch_arg("use_pointcloud_container", "False") add_launch_arg("pointcloud_container_name", "pointcloud_container") - add_launch_arg("individual_container_name", "concatenate_container") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml index a0fbd4b2..1c294fdf 100644 --- a/aip_x1_launch/launch/sensing.launch.xml +++ b/aip_x1_launch/launch/sensing.launch.xml @@ -2,7 +2,6 @@ - @@ -12,7 +11,6 @@ - diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index c7e21aa1..a1c8f5f6 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -4,7 +4,6 @@ - @@ -189,7 +188,6 @@ - diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py index 6aa031b0..6d435dee 100644 --- a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x2_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 @@ -56,31 +55,14 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - # set container to run all required components in the same process - container = ComposableNodeContainer( - name=LaunchConfiguration("individual_container_name"), - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[], - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - - target_container = ( - container - if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("pointcloud_container_name") - ) - # load concat or passthrough filter concat_loader = LoadComposableNodes( composable_node_descriptions=[concat_component], - target_container=target_container, + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_concat_filter")), ) - return [container, concat_loader] + return [concat_loader] def generate_launch_description(): @@ -92,9 +74,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("base_frame", "base_link") add_launch_arg("use_multithread", "True") add_launch_arg("use_intra_process", "True") - add_launch_arg("use_pointcloud_container", "False") add_launch_arg("pointcloud_container_name", "pointcloud_container") - add_launch_arg("individual_container_name", "concatenate_container") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_x2_launch/launch/sensing.launch.xml b/aip_x2_launch/launch/sensing.launch.xml index 51614263..274f6334 100644 --- a/aip_x2_launch/launch/sensing.launch.xml +++ b/aip_x2_launch/launch/sensing.launch.xml @@ -2,7 +2,6 @@ - @@ -12,7 +11,6 @@ - diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml index 0adfe218..417d4972 100644 --- a/aip_xx1_launch/launch/lidar.launch.xml +++ b/aip_xx1_launch/launch/lidar.launch.xml @@ -5,7 +5,6 @@ - @@ -101,7 +100,6 @@ - diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index e53dfb70..10542a54 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_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 @@ -57,30 +56,14 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - # set container to run all required components in the same process - container = ComposableNodeContainer( - name=LaunchConfiguration("individual_container_name"), - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - - target_container = ( - container - if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("pointcloud_container_name") - ) - # load concat or passthrough filter concat_loader = LoadComposableNodes( composable_node_descriptions=[concat_component], - target_container=target_container, + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_concat_filter")), ) - return [container, concat_loader] + return [concat_loader] def generate_launch_description(): @@ -92,9 +75,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("base_frame", "base_link") add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") - add_launch_arg("use_pointcloud_container", "False") add_launch_arg("pointcloud_container_name", "pointcloud_container") - add_launch_arg("individual_container_name", "concatenate_container") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_xx1_launch/launch/sensing.launch.xml b/aip_xx1_launch/launch/sensing.launch.xml index ba6b935c..2128f30a 100644 --- a/aip_xx1_launch/launch/sensing.launch.xml +++ b/aip_xx1_launch/launch/sensing.launch.xml @@ -3,7 +3,6 @@ - @@ -13,7 +12,6 @@ - From b900f4bd46197b1cf4887f48e0468a79cd13612b Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Wed, 14 Feb 2024 23:01:42 +0900 Subject: [PATCH 02/10] fix(common_sensor_launch): fix config of radar_tracks_msgs_converter (#211) Signed-off-by: scepter914 --- .../config/radar_tracks_msgs_converter.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml b/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml index 5a30a6bb..7b344aaa 100644 --- a/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml +++ b/common_sensor_launch/config/radar_tracks_msgs_converter.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: update_rate_hz: 20.0 + new_frame_id: "base_link" use_twist_compensation: true use_twist_yaw_compensation: false static_object_speed_threshold: 1.0 From 661b71cd9b0ed274b4aa508f339211a0170abd4a Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 5 Mar 2024 09:57:47 +0900 Subject: [PATCH 03/10] chore: rename ground_segmentation releated topic (#214) Signed-off-by: badai-nguyen --- aip_x1_launch/launch/topic_state_monitor.launch.py | 2 +- aip_x1_launch/launch/topic_state_monitor.launch.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_x1_launch/launch/topic_state_monitor.launch.py b/aip_x1_launch/launch/topic_state_monitor.launch.py index b45b0f4e..1c17606c 100644 --- a/aip_x1_launch/launch/topic_state_monitor.launch.py +++ b/aip_x1_launch/launch/topic_state_monitor.launch.py @@ -155,7 +155,7 @@ def generate_launch_description(): name="topic_state_monitor_rough_no_ground", parameters=[ { - "topic": "/perception/obstacle_segmentation/single_frame/pointcloud_raw", + "topic": "/perception/obstacle_segmentation/single_frame/pointcloud", "topic_type": "sensor_msgs/msg/PointCloud2", "best_effort": True, "diag_name": "sensing_topic_status", diff --git a/aip_x1_launch/launch/topic_state_monitor.launch.xml b/aip_x1_launch/launch/topic_state_monitor.launch.xml index 8912fb67..aa89fccd 100644 --- a/aip_x1_launch/launch/topic_state_monitor.launch.xml +++ b/aip_x1_launch/launch/topic_state_monitor.launch.xml @@ -76,7 +76,7 @@ - + From 73d85c54afa63f90269e16fde4a59762c5ac0982 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 19 Mar 2024 18:26:25 +0900 Subject: [PATCH 04/10] chore(blockage_diag): update param for aip_x2_launch (#223) * chore: update blockage param for aip_x2_launch Signed-off-by: badai-nguyen * chore: update param config Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- .../blockage_diagnostics_param_file.yaml | 1 + aip_x2_launch/launch/lidar.launch.xml | 25 +++++++++++++++++++ .../launch/pandar_node_container.launch.py | 10 +++++++- 3 files changed, 35 insertions(+), 1 deletion(-) diff --git a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml index b8b62b50..c6ce8e11 100644 --- a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml +++ b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml @@ -9,3 +9,4 @@ dust_kernel_size: 2 dust_buffering_frames: 10 dust_buffering_interval: 1 + blockage_kernel: 10 diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index a1c8f5f6..9ccf52af 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -6,6 +6,7 @@ + @@ -25,6 +26,9 @@ + + + @@ -45,6 +49,9 @@ + + + @@ -67,6 +74,9 @@ + + + @@ -88,6 +98,9 @@ + + + @@ -111,6 +124,9 @@ + + + @@ -132,6 +148,9 @@ + + + @@ -155,6 +174,9 @@ + + + @@ -176,6 +198,9 @@ + + + diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py index cfb39cc4..ee0291d5 100644 --- a/aip_x2_launch/launch/pandar_node_container.launch.py +++ b/aip_x2_launch/launch/pandar_node_container.launch.py @@ -43,6 +43,10 @@ def get_pandar_monitor_info(): return p +def str2vector(string): + return [float(x) for x in string.strip("[]").split(",")] + + def get_vehicle_info(context): # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py @@ -232,6 +236,7 @@ def create_parameter_dict(*args): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range"))) blockage_diag_component = ComposableNode( package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::BlockageDiagComponent", @@ -245,7 +250,9 @@ def create_parameter_dict(*args): "angle_range": LaunchConfiguration("angle_range"), "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), "vertical_bins": LaunchConfiguration("vertical_bins"), - "model": LaunchConfiguration("model"), + "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), + "max_distance_range": distance_range[1], + "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), } ] + [load_composable_node_param("blockage_diagnostics_param_file")], @@ -323,6 +330,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_frame", LaunchConfiguration("base_frame")) add_launch_arg("output_frame", LaunchConfiguration("base_frame")) add_launch_arg("dual_return_filter_param_file") + add_launch_arg("horizontal_resolution", "0.4") add_launch_arg( "blockage_diagnostics_param_file", [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"], From c47e3e8f4e12b4fb6d782a8341d81707929d36be Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 19 Mar 2024 18:26:40 +0900 Subject: [PATCH 05/10] fix: add blockage for xx1 (#191) * fix: enable blocakge diag for xx1 Signed-off-by: badai-nguyen * fix: xx1 launch Signed-off-by: badai-nguyen * fix: x2 launch Signed-off-by: badai-nguyen * ci(pre-commit): autofix * fix: remove duplicated angle_range param Signed-off-by: badai-nguyen * fix: remove x2 redundant param Signed-off-by: badai-nguyen * chore: update config file Signed-off-by: badai-nguyen * chore: disable xx1 blockage as default Signed-off-by: badai-nguyen * fix: velodyne launch Signed-off-by: badai-nguyen * Revert "fix: remove x2 redundant param" This reverts commit fa935883bae2f973dbfc51f64f7ed4bc3f896b0e. * Revert "fix: x2 launch" This reverts commit 4748c456d41c7099f913cecca7b31aecb9abf1e7. --------- Signed-off-by: badai-nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../sensor_kit.param.yaml | 9 +++ aip_xx1_launch/launch/lidar.launch.xml | 21 +++++++ common_sensor_launch/CMakeLists.txt | 1 + .../blockage_diagnostics_param_file.yaml | 12 ++++ .../launch/nebula_node_container.launch.py | 62 ++++++++++++++++--- .../launch/velodyne_VLP16.launch.xml | 10 +++ .../launch/velodyne_VLP32C.launch.xml | 10 +++ .../launch/velodyne_VLS128.launch.xml | 10 +++ 8 files changed, 126 insertions(+), 9 deletions(-) create mode 100644 common_sensor_launch/config/blockage_diagnostics_param_file.yaml diff --git a/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index ab32e0fe..56fc9dbd 100644 --- a/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -18,6 +18,15 @@ type: diagnostic_aggregator/AnalyzerGroup path: lidar analyzers: + performance_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: performance_monitoring + analyzers: + blockage: + type: diagnostic_aggregator/GenericAnalyzer + path: blockage + contains: [": blockage_validation"] + timeout: 1.0 velodyne: type: diagnostic_aggregator/AnalyzerGroup path: velodyne diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml index 417d4972..279bf9f2 100644 --- a/aip_xx1_launch/launch/lidar.launch.xml +++ b/aip_xx1_launch/launch/lidar.launch.xml @@ -6,6 +6,7 @@ + @@ -22,6 +23,11 @@ + + + + + @@ -39,6 +45,11 @@ + + + + + @@ -56,6 +67,11 @@ + + + + + @@ -73,6 +89,11 @@ + + + + + diff --git a/common_sensor_launch/CMakeLists.txt b/common_sensor_launch/CMakeLists.txt index 60a50fc2..5e719ce7 100644 --- a/common_sensor_launch/CMakeLists.txt +++ b/common_sensor_launch/CMakeLists.txt @@ -12,4 +12,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE config launch + config ) diff --git a/common_sensor_launch/config/blockage_diagnostics_param_file.yaml b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml new file mode 100644 index 00000000..c6ce8e11 --- /dev/null +++ b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + blockage_ratio_threshold: 0.1 + blockage_count_threshold: 50 + blockage_buffering_frames: 2 + blockage_buffering_interval: 1 + dust_ratio_threshold: 0.2 + dust_count_threshold: 10 + dust_kernel_size: 2 + dust_buffering_frames: 10 + dust_buffering_interval: 1 + blockage_kernel: 10 diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index c14c1390..ecd21d79 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.substitutions import FindPackageShare import yaml @@ -54,14 +55,11 @@ def get_vehicle_info(context): return p -def get_vehicle_mirror_info(context): - path = LaunchConfiguration("vehicle_mirror_param_file").perform(context) - with open(path, "r") as f: - p = yaml.safe_load(f)["/**"]["ros__parameters"] - return p - - def launch_setup(context, *args, **kwargs): + def load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), "r") as f: + return yaml.safe_load(f)["/**"]["ros__parameters"] + def create_parameter_dict(*args): result = {} for x in args: @@ -151,7 +149,7 @@ def create_parameter_dict(*args): ) ) - mirror_info = get_vehicle_mirror_info(context) + mirror_info = load_composable_node_param("vehicle_mirror_param_file") 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"] @@ -239,13 +237,49 @@ def create_parameter_dict(*args): ], ) + blockage_diag_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::BlockageDiagComponent", + name="blockage_diag", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "blockage_diag/pointcloud"), + ], + parameters=[ + { + "angle_range": [ + float(context.perform_substitution(LaunchConfiguration("cloud_min_angle"))), + float(context.perform_substitution(LaunchConfiguration("cloud_max_angle"))), + ], + "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), + "vertical_bins": LaunchConfiguration("vertical_bins"), + "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), + "max_distance_range": LaunchConfiguration("max_range"), + "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), + } + ] + + [load_composable_node_param("blockage_diagnostics_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + 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=container, condition=IfCondition(LaunchConfiguration("launch_driver")), ) + blockage_diag_loader = LoadComposableNodes( + composable_node_descriptions=[blockage_diag_component], + target_container=target_container, + condition=IfCondition(LaunchConfiguration("enable_blockage_diag")), + ) - return [container, driver_component_loader] + return [container, driver_component_loader, blockage_diag_loader] def generate_launch_description(): @@ -284,6 +318,16 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") add_launch_arg("container_name", "nebula_node_container") + add_launch_arg("enable_blockage_diag", "true") + add_launch_arg("horizontal_ring_id", "64") + add_launch_arg("vertical_bins", "128") + add_launch_arg("is_channel_order_top2down", "true") + add_launch_arg("horizontal_resolution", "0.4") + add_launch_arg( + "blockage_diagnostics_param_file", + [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics_param_file.yaml"], + ) + set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml index faa32305..a105b0a2 100644 --- a/common_sensor_launch/launch/velodyne_VLP16.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLP16.launch.xml @@ -16,6 +16,11 @@ + + + + + @@ -34,6 +39,11 @@ + + + + + diff --git a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml index c3db6428..3544d16e 100644 --- a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml @@ -16,6 +16,11 @@ + + + + + @@ -34,6 +39,11 @@ + + + + + diff --git a/common_sensor_launch/launch/velodyne_VLS128.launch.xml b/common_sensor_launch/launch/velodyne_VLS128.launch.xml index f1d32469..01c8affa 100644 --- a/common_sensor_launch/launch/velodyne_VLS128.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLS128.launch.xml @@ -16,6 +16,11 @@ + + + + + @@ -34,6 +39,11 @@ + + + + + From fb718d6fcd7b172f9a57e2359d1e3e6ec1c7eb1e Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 21 Mar 2024 15:25:02 +0900 Subject: [PATCH 06/10] fix(common_sensor_launch): fix use_container bug (#226) Signed-off-by: badai-nguyen --- .../launch/nebula_node_container.launch.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index ecd21d79..eb59ab5d 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -262,12 +262,6 @@ def create_parameter_dict(*args): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - 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=container, @@ -275,7 +269,7 @@ def create_parameter_dict(*args): ) blockage_diag_loader = LoadComposableNodes( composable_node_descriptions=[blockage_diag_component], - target_container=target_container, + target_container=container, condition=IfCondition(LaunchConfiguration("enable_blockage_diag")), ) From 772511297f476df0aeaf05e8696f1aea57889e88 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 29 Mar 2024 15:21:19 +0900 Subject: [PATCH 07/10] chore(blockage_diag): add option param for xx1 (#225) Signed-off-by: badai-nguyen --- .../config/blockage_diagnostics_param_file.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/common_sensor_launch/config/blockage_diagnostics_param_file.yaml b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml index c6ce8e11..e43b2039 100644 --- a/common_sensor_launch/config/blockage_diagnostics_param_file.yaml +++ b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml @@ -4,6 +4,8 @@ blockage_count_threshold: 50 blockage_buffering_frames: 2 blockage_buffering_interval: 1 + enable_dust_diag: false + publish_debug_image: false dust_ratio_threshold: 0.2 dust_count_threshold: 10 dust_kernel_size: 2 From 3517d2241c4b1fd039da530037c19fbf22c81cb2 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 29 Mar 2024 15:21:42 +0900 Subject: [PATCH 08/10] chore(blockage_diag): add option param for x2 (#224) Signed-off-by: badai-nguyen --- aip_x2_launch/config/blockage_diagnostics_param_file.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml index c6ce8e11..e43b2039 100644 --- a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml +++ b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml @@ -4,6 +4,8 @@ blockage_count_threshold: 50 blockage_buffering_frames: 2 blockage_buffering_interval: 1 + enable_dust_diag: false + publish_debug_image: false dust_ratio_threshold: 0.2 dust_count_threshold: 10 dust_kernel_size: 2 From 101480dcda49d2a323ce8b94ea409f9bba8c3baa Mon Sep 17 00:00:00 2001 From: TaikiYamada4 <129915538+TaikiYamada4@users.noreply.github.com> Date: Mon, 22 Apr 2024 15:33:13 +0900 Subject: [PATCH 09/10] feat(imu_launch): add gyro_bias_estimator.param.yaml (#232) * Added gyro_bias_estimator_param_path to all imu.launch.xml Signed-off-by: TaikiYamada4 * Added gyro_bias_estimator.param.yaml to common_sensor_launch Signed-off-by: TaikiYamada4 * Fixed value in gyro_bias_estimator.param.yaml Signed-off-by: TaikiYamada4 * Distribute gyro_bias_estimator.param.yaml to each aip_x1_launch, aip_x2_launch and aip_xx1_launch. Signed-off-by: TaikiYamada4 --------- Signed-off-by: TaikiYamada4 --- aip_x1_launch/config/gyro_bias_estimator.param.yaml | 6 ++++++ aip_x1_launch/launch/imu.launch.xml | 2 ++ aip_x2_launch/config/gyro_bias_estimator.param.yaml | 6 ++++++ aip_x2_launch/launch/imu.launch.xml | 2 ++ aip_xx1_launch/config/gyro_bias_estimator.param.yaml | 6 ++++++ aip_xx1_launch/launch/imu.launch.xml | 2 ++ 6 files changed, 24 insertions(+) create mode 100644 aip_x1_launch/config/gyro_bias_estimator.param.yaml create mode 100644 aip_x2_launch/config/gyro_bias_estimator.param.yaml create mode 100644 aip_xx1_launch/config/gyro_bias_estimator.param.yaml diff --git a/aip_x1_launch/config/gyro_bias_estimator.param.yaml b/aip_x1_launch/config/gyro_bias_estimator.param.yaml new file mode 100644 index 00000000..d552569f --- /dev/null +++ b/aip_x1_launch/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.008 # [rad/s] + timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] + straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/aip_x1_launch/launch/imu.launch.xml b/aip_x1_launch/launch/imu.launch.xml index c192c5a1..bf00b7ca 100644 --- a/aip_x1_launch/launch/imu.launch.xml +++ b/aip_x1_launch/launch/imu.launch.xml @@ -14,10 +14,12 @@ + + diff --git a/aip_x2_launch/config/gyro_bias_estimator.param.yaml b/aip_x2_launch/config/gyro_bias_estimator.param.yaml new file mode 100644 index 00000000..d552569f --- /dev/null +++ b/aip_x2_launch/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.008 # [rad/s] + timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] + straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/aip_x2_launch/launch/imu.launch.xml b/aip_x2_launch/launch/imu.launch.xml index 432316cb..e3f1a056 100644 --- a/aip_x2_launch/launch/imu.launch.xml +++ b/aip_x2_launch/launch/imu.launch.xml @@ -29,10 +29,12 @@ + + diff --git a/aip_xx1_launch/config/gyro_bias_estimator.param.yaml b/aip_xx1_launch/config/gyro_bias_estimator.param.yaml new file mode 100644 index 00000000..d552569f --- /dev/null +++ b/aip_xx1_launch/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.008 # [rad/s] + timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] + straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/aip_xx1_launch/launch/imu.launch.xml b/aip_xx1_launch/launch/imu.launch.xml index b764d598..85b620d7 100644 --- a/aip_xx1_launch/launch/imu.launch.xml +++ b/aip_xx1_launch/launch/imu.launch.xml @@ -23,10 +23,12 @@ + + From 4eb33785b04f832c78b096ba0ff461c224b836d6 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Mon, 13 May 2024 21:18:56 +0900 Subject: [PATCH 10/10] feat(update): update lidar pointcloud interface and concatenate pointcloud filter (#215) * feat: update common nebula container Signed-off-by: yoshiri * feat: fix xx1 pointcloud preprocessor launch Signed-off-by: yoshiri * feat: update x1 launcher to use new interface Signed-off-by: yoshiri * feat: update x2 launcher Signed-off-by: yoshiri * Update aip_x2_launch/launch/pandar_node_container.launch.py Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> * fix: fix x2 topics disconnection Signed-off-by: yoshiri --------- Signed-off-by: yoshiri Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> --- .../launch/new_livox_horizon.launch.py | 4 ++-- .../launch/pointcloud_preprocessor.launch.py | 9 +++++---- .../launch/velodyne_node_container.launch.py | 11 ++++++++++- .../launch/pandar_node_container.launch.py | 13 +++++++++++-- .../launch/pointcloud_preprocessor.launch.py | 17 +++++++++-------- .../launch/pointcloud_preprocessor.launch.py | 9 +++++---- .../launch/nebula_node_container.launch.py | 11 ++++++++++- 7 files changed, 52 insertions(+), 22 deletions(-) diff --git a/aip_x1_launch/launch/new_livox_horizon.launch.py b/aip_x1_launch/launch/new_livox_horizon.launch.py index de3f8b9a..922d23ba 100644 --- a/aip_x1_launch/launch/new_livox_horizon.launch.py +++ b/aip_x1_launch/launch/new_livox_horizon.launch.py @@ -52,12 +52,12 @@ def get_crop_box_min_range_component(context, livox_frame_id): name="crop_box_filter_min_range", remappings=[ ("input", "livox/tag_filtered/lidar" if use_tag_filter else "livox/lidar"), - ("output", "min_range_cropped/pointcloud"), + ("output", "min_range_cropped/pointcloud_before_sync"), ], parameters=[ { "input_frame": livox_frame_id, - "output_frame": LaunchConfiguration("base_frame"), + "output_frame": LaunchConfiguration("frame_id"), "min_x": 0.0, "max_x": LaunchConfiguration("min_range"), "min_y": -2.0, diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index ff012c8a..8d799261 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -36,14 +36,15 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/top/pointcloud", - "/sensing/lidar/front_left/min_range_cropped/pointcloud", - "/sensing/lidar/front_right/min_range_cropped/pointcloud", - "/sensing/lidar/front_center/min_range_cropped/pointcloud", + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/front_left/min_range_cropped/pointcloud_before_sync", + "/sensing/lidar/front_right/min_range_cropped/pointcloud_before_sync", + "/sensing/lidar/front_center/min_range_cropped/pointcloud_before_sync", ], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 1.0, "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py index 56b35116..2ce274b1 100644 --- a/aip_x1_launch/launch/velodyne_node_container.launch.py +++ b/aip_x1_launch/launch/velodyne_node_container.launch.py @@ -121,6 +121,13 @@ 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", @@ -128,8 +135,9 @@ def create_parameter_dict(*args): 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")}], ) ) @@ -218,6 +226,7 @@ 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 ROS2 component container communication") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py index ee0291d5..b950761e 100644 --- a/aip_x2_launch/launch/pandar_node_container.launch.py +++ b/aip_x2_launch/launch/pandar_node_container.launch.py @@ -206,14 +206,22 @@ def create_parameter_dict(*args): 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): + ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")} + else: + ring_outlier_filter_parameters = { + "output_frame": "" + } # keep the output frame as the input frame ring_outlier_filter_component = 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")}], ) @@ -223,7 +231,7 @@ def create_parameter_dict(*args): name="dual_return_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], parameters=[ { @@ -344,6 +352,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("min_azimuth_deg", "135.0") add_launch_arg("max_azimuth_deg", "225.0") add_launch_arg("enable_blockage_diag", "true") + add_launch_arg("output_as_sensor_frame", "True") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py index 6d435dee..3c0a09d8 100644 --- a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py @@ -37,19 +37,20 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/front_upper/pointcloud", - "/sensing/lidar/front_lower/pointcloud", - "/sensing/lidar/left_upper/pointcloud", - "/sensing/lidar/left_lower/pointcloud", - "/sensing/lidar/right_upper/pointcloud", - "/sensing/lidar/right_lower/pointcloud", - "/sensing/lidar/rear_upper/pointcloud", - "/sensing/lidar/rear_lower/pointcloud", + "/sensing/lidar/front_upper/pointcloud_before_sync", + "/sensing/lidar/front_lower/pointcloud_before_sync", + "/sensing/lidar/left_upper/pointcloud_before_sync", + "/sensing/lidar/left_lower/pointcloud_before_sync", + "/sensing/lidar/right_upper/pointcloud_before_sync", + "/sensing/lidar/right_lower/pointcloud_before_sync", + "/sensing/lidar/rear_upper/pointcloud_before_sync", + "/sensing/lidar/rear_lower/pointcloud_before_sync", ], "input_offset": [0.025, 0.025, 0.01, 0.0, 0.05, 0.05, 0.05, 0.05], "timeout_sec": 0.075, "output_frame": LaunchConfiguration("base_frame"), "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index 10542a54..726171f9 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py @@ -37,10 +37,10 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "input_topics": [ - "/sensing/lidar/top/pointcloud", - "/sensing/lidar/left/pointcloud", - "/sensing/lidar/right/pointcloud", - "/sensing/lidar/rear/pointcloud", + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/left/pointcloud_before_sync", + "/sensing/lidar/right/pointcloud_before_sync", + "/sensing/lidar/rear/pointcloud_before_sync", ], "output_frame": LaunchConfiguration("base_frame"), "input_offset": [ @@ -51,6 +51,7 @@ def launch_setup(context, *args, **kwargs): ], # each sensor will wait 60, 70, 70, 70ms "timeout_sec": 0.095, # set shorter than 100ms "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index eb59ab5d..c3339e0c 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -186,6 +186,13 @@ 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", @@ -193,8 +200,9 @@ def create_parameter_dict(*args): 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")}], ) ) @@ -311,6 +319,7 @@ 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("container_name", "nebula_node_container") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") add_launch_arg("enable_blockage_diag", "true") add_launch_arg("horizontal_ring_id", "64")