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_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/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 8d9892b0..8d799261 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 @@ -37,43 +36,27 @@ 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")}], ) - # 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 +68,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_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 @@ - + 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/config/blockage_diagnostics_param_file.yaml b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml index b8b62b50..e43b2039 100644 --- a/aip_x2_launch/config/blockage_diagnostics_param_file.yaml +++ b/aip_x2_launch/config/blockage_diagnostics_param_file.yaml @@ -4,8 +4,11 @@ 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 dust_buffering_frames: 10 dust_buffering_interval: 1 + blockage_kernel: 10 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_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index c7e21aa1..9ccf52af 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -4,9 +4,9 @@ - + @@ -26,6 +26,9 @@ + + + @@ -46,6 +49,9 @@ + + + @@ -68,6 +74,9 @@ + + + @@ -89,6 +98,9 @@ + + + @@ -112,6 +124,9 @@ + + + @@ -133,6 +148,9 @@ + + + @@ -156,6 +174,9 @@ + + + @@ -177,6 +198,9 @@ + + + @@ -189,7 +213,6 @@ - diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py index cfb39cc4..b950761e 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 @@ -202,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")}], ) @@ -219,7 +231,7 @@ def create_parameter_dict(*args): name="dual_return_filter", remappings=[ ("input", "rectified/pointcloud_ex"), - ("output", "pointcloud"), + ("output", "pointcloud_before_sync"), ], parameters=[ { @@ -232,6 +244,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 +258,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 +338,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"], @@ -336,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 6aa031b0..3c0a09d8 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 @@ -38,49 +37,33 @@ 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")}], ) - # 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 +75,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/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/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 e88e8d8e..29795ae1 100644 --- a/aip_xx1_launch/launch/imu.launch.xml +++ b/aip_xx1_launch/launch/imu.launch.xml @@ -44,10 +44,12 @@ + + diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml index 7f77ad2d..b524b788 100644 --- a/aip_xx1_launch/launch/lidar.launch.xml +++ b/aip_xx1_launch/launch/lidar.launch.xml @@ -5,8 +5,8 @@ - + @@ -23,6 +23,11 @@ + + + + + @@ -40,6 +45,11 @@ + + + + + @@ -57,6 +67,11 @@ + + + + + @@ -74,6 +89,11 @@ + + + + + @@ -98,7 +118,6 @@ - diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index c7a30175..433b7f58 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 @@ -52,30 +51,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(): @@ -87,7 +70,6 @@ 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") add_launch_arg( diff --git a/aip_xx1_launch/launch/sensing.launch.xml b/aip_xx1_launch/launch/sensing.launch.xml index 44477cae..8213cba8 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 @@ - 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..e43b2039 --- /dev/null +++ b/common_sensor_launch/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + blockage_ratio_threshold: 0.1 + 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 + dust_buffering_frames: 10 + dust_buffering_interval: 1 + blockage_kernel: 10 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 diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index 7b6ee0c2..1990f695 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: @@ -183,7 +181,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"] @@ -284,13 +282,43 @@ 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")}], + ) + 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=container, + condition=IfCondition(LaunchConfiguration("enable_blockage_diag")), + ) - return [container, driver_component_loader] + return [container, driver_component_loader, blockage_diag_loader] def generate_launch_description(): @@ -336,6 +364,17 @@ def add_launch_arg(name: str, default_value=None, description=None): 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("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") + 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", 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 @@ + + + + +