From 6c257a9e51daf5f24675962813551bef7ba83baf Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 19 Dec 2024 16:03:36 +0900 Subject: [PATCH 1/9] Fixed nebula_node_container.launch.py to use nebula v0.2.1 Signed-off-by: Shintaro Sakoda --- .../launch/nebula_node_container.launch.py | 274 +++++++----------- 1 file changed, 99 insertions(+), 175 deletions(-) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index 946fe7db..644cf71a 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -23,10 +23,8 @@ 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 from launch_ros.parameter_descriptions import ParameterFile -from launch_ros.substitutions import FindPackageShare import yaml @@ -35,6 +33,8 @@ def get_lidar_make(sensor_name): return "Hesai", ".csv" elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: return "Velodyne", ".yaml" + elif sensor_name.lower() in ["helios", "bpearl"]: + return "Robosense", None return "unrecognized_sensor_model" @@ -56,11 +56,14 @@ def get_vehicle_info(context): 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 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 create_parameter_dict(*args): result = {} for x in args: @@ -73,15 +76,34 @@ def create_parameter_dict(*args): 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, + if sensor_extension is not None: # Velodyne and Hesai + 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 + ) + else: # Robosense + sensor_calib_fp = "" + + # Pointcloud preprocessor parameters + distortion_corrector_node_param = ParameterFile( + 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, ) - assert os.path.exists( - sensor_calib_fp - ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) nodes = [] @@ -96,16 +118,18 @@ def create_parameter_dict(*args): nodes.append( ComposableNode( package="nebula_ros", - plugin=sensor_make + "DriverRosWrapper", - name=sensor_make.lower() + "_driver_ros_wrapper_node", + plugin=sensor_make + "RosWrapper", + name=sensor_make.lower() + "_ros_wrapper_node", parameters=[ { "calibration_file": sensor_calib_fp, "sensor_model": sensor_model, + "launch_hw": LaunchConfiguration("launch_driver"), **create_parameter_dict( "host_ip", "sensor_ip", "data_port", + "gnss_port", "return_mode", "min_range", "max_range", @@ -114,60 +138,25 @@ def create_parameter_dict(*args): "cloud_min_angle", "cloud_max_angle", "dual_return_distance_threshold", + "rotation_speed", + "packet_mtu_size", "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"), + # ("robosense_points", "pointcloud_raw_ex"), #for robosense + # ("pandar_points", "pointcloud_raw_ex"), # for hesai + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) - # There is an issue where hw_monitor crashes due to data race, - # so the monitor will now only be launched when explicitly specified with a launch command. - launch_hw_monitor: bool = IfCondition(LaunchConfiguration("launch_hw_monitor")).evaluate( - context - ) - launch_driver: bool = IfCondition(LaunchConfiguration("launch_driver")).evaluate(context) - if launch_hw_monitor and launch_driver: - 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 @@ -189,11 +178,13 @@ def create_parameter_dict(*args): ("output", "self_cropped/pointcloud_ex"), ], parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], ) ) - mirror_info = load_composable_node_param("vehicle_mirror_param_file") + mirror_info = get_vehicle_mirror_info(context) 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"] @@ -211,7 +202,9 @@ def create_parameter_dict(*args): ("output", "mirror_cropped/pointcloud_ex"), ], parameters=[cropbox_parameters], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], ) ) @@ -221,28 +214,28 @@ def create_parameter_dict(*args): plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent", name="distortion_corrector_node", remappings=[ - ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ( + "~/input/twist", + "/sensing/vehicle_velocity_converter/twist_with_covariance", + ), ("~/input/imu", "/sensing/imu/imu_data"), ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), ("~/output/pointcloud", "rectified/pointcloud_ex"), ], - parameters=[load_composable_node_param("distortion_corrector_node_param_file")], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + parameters=[distortion_corrector_node_param], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], ) ) - ring_outlier_filter_node_param = ParameterFile( - param_file=LaunchConfiguration("ring_outlier_filter_node_param_file").perform(context), - allow_substs=True, - ) - # 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_output_frame = {"output_frame": LaunchConfiguration("frame_id")} else: - # keep the output frame as the input frame - ring_outlier_output_frame = {"output_frame": ""} - + ring_outlier_output_frame = { + "output_frame": "" + } # keep the output frame as the input frame nodes.append( ComposableNode( package="autoware_pointcloud_preprocessor", @@ -253,7 +246,9 @@ def create_parameter_dict(*args): ("output", "pointcloud_before_sync"), ], parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], ) ) @@ -267,76 +262,7 @@ def create_parameter_dict(*args): output="both", ) - 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", - ), - } - ], - ) - - blockage_diag_component = ComposableNode( - package="autoware_pointcloud_preprocessor", - plugin="autoware::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, blockage_diag_loader] + return [container] def generate_launch_description(): @@ -345,19 +271,17 @@ def generate_launch_description(): def add_launch_arg(name: str, default_value=None, description=None): # a default_value of None is equivalent to not passing that kwarg at all launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) + 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") - add_launch_arg( - "launch_hw_monitor", - "False", - "do launch hardware monitor. Due to an issue where hw_monitor crashes due to data conflicts, the monitor in launched only when explicitly specified", - ) 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") @@ -370,41 +294,41 @@ def add_launch_arg(name: str, default_value=None, description=None): 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( + "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("use_multithread", "False", "use multithread") add_launch_arg( - "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" + "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("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("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("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.yaml"], + "vehicle_mirror_param_file", + description="path to the file of vehicle mirror position yaml", ) add_launch_arg( - "distortion_corrector_node_param_file", - [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"], + "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", ) add_launch_arg( - "ring_outlier_filter_node_param_file", - [FindPackageShare("common_sensor_launch"), "/config/ring_outlier_filter_node.param.yaml"], + "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( From a2b9c009ee3b04d911e98c1313446d18ec5e9d12 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 20 Dec 2024 04:10:30 +0000 Subject: [PATCH 2/9] ci(pre-commit): autofix --- .../launch/nebula_node_container.launch.py | 52 +++++-------------- 1 file changed, 13 insertions(+), 39 deletions(-) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index 644cf71a..deb9626c 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -85,23 +85,17 @@ def create_parameter_dict(*args): ) assert os.path.exists( sensor_calib_fp - ), "Sensor calib file under calibration/ was not found: {}".format( - sensor_calib_fp - ) + ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) else: # Robosense sensor_calib_fp = "" # Pointcloud preprocessor parameters distortion_corrector_node_param = ParameterFile( - param_file=LaunchConfiguration("distortion_correction_node_param_path").perform( - context - ), + 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 - ), + param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context), allow_substs=True, ) @@ -151,9 +145,7 @@ def create_parameter_dict(*args): # ("robosense_points", "pointcloud_raw_ex"), #for robosense # ("pandar_points", "pointcloud_raw_ex"), # for hesai ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -178,9 +170,7 @@ def create_parameter_dict(*args): ("output", "self_cropped/pointcloud_ex"), ], parameters=[cropbox_parameters], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -202,9 +192,7 @@ def create_parameter_dict(*args): ("output", "mirror_cropped/pointcloud_ex"), ], parameters=[cropbox_parameters], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -223,9 +211,7 @@ def create_parameter_dict(*args): ("~/output/pointcloud", "rectified/pointcloud_ex"), ], parameters=[distortion_corrector_node_param], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -233,9 +219,7 @@ def create_parameter_dict(*args): if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true": ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")} else: - ring_outlier_output_frame = { - "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="autoware_pointcloud_preprocessor", @@ -246,9 +230,7 @@ def create_parameter_dict(*args): ("output", "pointcloud_before_sync"), ], parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) ) @@ -271,9 +253,7 @@ def generate_launch_description(): def add_launch_arg(name: str, default_value=None, description=None): # a default_value of None is equivalent to not passing that kwarg at all launch_arguments.append( - DeclareLaunchArgument( - name, default_value=default_value, description=description - ) + DeclareLaunchArgument(name, default_value=default_value, description=description) ) common_sensor_share_dir = get_package_share_directory("common_sensor_launch") @@ -294,20 +274,14 @@ def add_launch_arg(name: str, default_value=None, description=None): 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("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("use_multithread", "False", "use multithread") - add_launch_arg( - "use_intra_process", "False", "use ROS 2 component container communication" - ) + 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("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", From 8fd160be658c80128d1799fad773f7a9a9baf799 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Fri, 20 Dec 2024 13:15:38 +0900 Subject: [PATCH 3/9] Fixed to reduce diff Signed-off-by: Shintaro Sakoda --- .../launch/nebula_node_container.launch.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index deb9626c..6a544ca5 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -202,10 +202,7 @@ def create_parameter_dict(*args): plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent", name="distortion_corrector_node", remappings=[ - ( - "~/input/twist", - "/sensing/vehicle_velocity_converter/twist_with_covariance", - ), + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), ("~/input/imu", "/sensing/imu/imu_data"), ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), ("~/output/pointcloud", "rectified/pointcloud_ex"), @@ -219,7 +216,8 @@ def create_parameter_dict(*args): if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true": ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")} else: - ring_outlier_output_frame = {"output_frame": ""} # keep the output frame as the input frame + # keep the output frame as the input frame + ring_outlier_output_frame = {"output_frame": ""} nodes.append( ComposableNode( package="autoware_pointcloud_preprocessor", From 00bf681dbf0ff68018b49aafd9af77fc00b75942 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Fri, 20 Dec 2024 13:25:04 +0900 Subject: [PATCH 4/9] Restore driver_component_loader and blockage_diag_loader Signed-off-by: Shintaro Sakoda --- .../launch/nebula_node_container.launch.py | 89 ++++++++++++++++++- 1 file changed, 88 insertions(+), 1 deletion(-) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index 6a544ca5..1b17f8ba 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -23,6 +23,7 @@ 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 from launch_ros.parameter_descriptions import ParameterFile import yaml @@ -64,6 +65,10 @@ def get_vehicle_mirror_info(context): 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: @@ -218,6 +223,7 @@ def create_parameter_dict(*args): else: # keep the output frame as the input frame ring_outlier_output_frame = {"output_frame": ""} + nodes.append( ComposableNode( package="autoware_pointcloud_preprocessor", @@ -242,7 +248,88 @@ def create_parameter_dict(*args): output="both", ) - return [container] + 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", + ), + } + ], + ) + + blockage_diag_component = ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::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, blockage_diag_loader] def generate_launch_description(): From 4eea735eef244e7162a617a6915f1e128f5ee046 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 20 Dec 2024 04:25:40 +0000 Subject: [PATCH 5/9] ci(pre-commit): autofix --- .../launch/nebula_node_container.launch.py | 20 ++++--------------- 1 file changed, 4 insertions(+), 16 deletions(-) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index 1b17f8ba..d46805aa 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -292,30 +292,18 @@ def create_parameter_dict(*args): parameters=[ { "angle_range": [ - float( - context.perform_substitution( - LaunchConfiguration("cloud_min_angle") - ) - ), - float( - context.perform_substitution( - LaunchConfiguration("cloud_max_angle") - ) - ), + 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" - ), + "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")} - ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) driver_component_loader = LoadComposableNodes( From 35acf7d3ac311ee8a562f59c5e4ae02a48cf76c5 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Fri, 20 Dec 2024 13:40:56 +0900 Subject: [PATCH 6/9] Added blockage_diagnostics_param_file Signed-off-by: Shintaro Sakoda --- .../launch/nebula_node_container.launch.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index d46805aa..cab08001 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -355,6 +355,15 @@ 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("lidar_container_name", "nebula_node_container") add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") + add_launch_arg( + "blockage_diagnostics_param_file", + os.path.join( + common_sensor_share_dir, + "config", + "blockage_diagnostics.param.yaml", + ), + description="path to parameter file of blockage diagnostics node", + ) add_launch_arg( "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml", From 965843b7bd2edf7915ce07b05158de5e5ab49f29 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 16 Jan 2025 13:27:37 +0900 Subject: [PATCH 7/9] Restore pandar_points remapping Signed-off-by: Shintaro Sakoda --- common_sensor_launch/launch/nebula_node_container.launch.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index cab08001..f1614380 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -146,9 +146,8 @@ def create_parameter_dict(*args): remappings=[ # cSpell:ignore knzo25 # TODO(knzo25): fix the remapping once nebula gets updated + ("pandar_points", "pointcloud_raw_ex"), ("velodyne_points", "pointcloud_raw_ex"), - # ("robosense_points", "pointcloud_raw_ex"), #for robosense - # ("pandar_points", "pointcloud_raw_ex"), # for hesai ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) From b9d8e856c2756471680162d059df14ae8d7eac71 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 16 Jan 2025 13:39:10 +0900 Subject: [PATCH 8/9] Restore used parameters Signed-off-by: Shintaro Sakoda --- .../launch/nebula_node_container.launch.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index f1614380..911253ae 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -353,7 +353,16 @@ 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("lidar_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("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", os.path.join( From 67953235e927a1d292126c8269d376fbc89d4669 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 16 Jan 2025 13:39:56 +0900 Subject: [PATCH 9/9] Restore retry_hw Signed-off-by: Shintaro Sakoda --- common_sensor_launch/launch/nebula_node_container.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index 911253ae..0c7d1ce3 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -334,6 +334,7 @@ def add_launch_arg(name: str, default_value=None, description=None): 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")