diff --git a/aip_x2_launch/launch/nebula_node_container.launch.py b/aip_x2_launch/launch/nebula_node_container.launch.py index 97e16d78..e8e204b0 100644 --- a/aip_x2_launch/launch/nebula_node_container.launch.py +++ b/aip_x2_launch/launch/nebula_node_container.launch.py @@ -13,6 +13,7 @@ # limitations under the License. import os +from pathlib import Path from ament_index_python.packages import get_package_share_directory import launch @@ -27,16 +28,17 @@ 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 def get_lidar_make(sensor_name): if sensor_name[:6].lower() == "pandar": - return "Hesai", ".csv" - elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: - return "Velodyne", ".yaml" - return "unrecognized_sensor_model" + return "Hesai" + elif sensor_name[:3].lower() in ["vlp", "vls"]: + return "Velodyne" + raise ValueError("Unrecognized sensor model") def get_vehicle_info(context): @@ -80,19 +82,25 @@ def str2vector(string): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) - sensor_make, sensor_extension = get_lidar_make(sensor_model) - 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, + sensor_make = get_lidar_make(sensor_model) + + # Configuration file containing sensor model's default parameters + + sensor_config = LaunchConfiguration("config_file").perform(context) + if sensor_config == "": + sensor_config = ( + Path(get_package_share_directory("nebula_ros")) + / "config" + / "lidar" + / sensor_make.lower() + / (sensor_model + ".param.yaml") + ) + else: + sensor_config = Path(sensor_config) + + assert sensor_config.exists(), "Sensor configuration not found: {}".format( + sensor_config.absolute().as_posix() ) - assert os.path.exists( - sensor_calib_fp - ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) glog_component = ComposableNode( package="glog_component", @@ -100,91 +108,44 @@ def str2vector(string): name="glog_component", ) - nebula_ros_hw_monitor_component = ComposableNode( - package="nebula_ros", - plugin=sensor_make + "HwMonitorRosWrapper", - name=sensor_make.lower() + "_hardware_monitor_ros_wrapper_node", - parameters=[ - { - "sensor_model": LaunchConfiguration("sensor_model"), - "return_mode": LaunchConfiguration("return_mode"), - "frame_id": LaunchConfiguration("frame_id"), - "scan_phase": LaunchConfiguration("scan_phase"), - "sensor_ip": LaunchConfiguration("sensor_ip"), - "host_ip": LaunchConfiguration("host_ip"), - "data_port": LaunchConfiguration("data_port"), - "gnss_port": LaunchConfiguration("gnss_port"), - "packet_mtu_size": LaunchConfiguration("packet_mtu_size"), - "rotation_speed": LaunchConfiguration("rotation_speed"), - "cloud_min_angle": LaunchConfiguration("cloud_min_angle"), - "cloud_max_angle": LaunchConfiguration("cloud_max_angle"), - "diag_span": LaunchConfiguration("diag_span"), - "dual_return_distance_threshold": LaunchConfiguration( - "dual_return_distance_threshold" - ), - "delay_monitor_ms": LaunchConfiguration("delay_monitor_ms"), - }, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - nebula_ros_hw_interface_component = ComposableNode( + nebula_component = ComposableNode( package="nebula_ros", - plugin=sensor_make + "HwInterfaceRosWrapper", - name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", + plugin=sensor_make + "RosWrapper", + name=sensor_make.lower() + "_ros_wrapper_node", parameters=[ + ParameterFile(sensor_config, allow_substs=True), { "sensor_model": sensor_model, - "calibration_file": sensor_calib_fp, + "launch_hw": LaunchConfiguration("launch_driver"), **create_parameter_dict( - "sensor_ip", "host_ip", - "scan_phase", - "return_mode", - "frame_id", - "rotation_speed", + "sensor_ip", "data_port", - "cloud_min_angle", - "cloud_max_angle", - "dual_return_distance_threshold", "gnss_port", "packet_mtu_size", "setup_sensor", - "ptp_profile", - "ptp_transport_type", - "ptp_switch_type", - "ptp_domain", - ), - }, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - nebula_ros_driver_component = ComposableNode( - package="nebula_ros", - plugin=sensor_make + "DriverRosWrapper", - name=sensor_make.lower() + "_driver_ros_wrapper_node", - parameters=[ - { - "calibration_file": sensor_calib_fp, - "sensor_model": sensor_model, - **create_parameter_dict( - "host_ip", - "sensor_ip", - "data_port", - "return_mode", + "frame_id", + "diag_span", "min_range", "max_range", - "frame_id", + "cloud_min_angle", + "cloud_max_angle", "scan_phase", + "rotation_speed", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", "dual_return_distance_threshold", ), "launch_hw": True, }, ], remappings=[ - ("aw_points", "pointcloud_raw"), - ("aw_points_ex", "pointcloud_raw_ex"), + # cSpell:ignore knzo25 + # TODO(knzo25): fix the remapping once nebula gets updated + ("pandar_points", "pointcloud_raw_ex"), ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -332,7 +293,7 @@ def str2vector(string): executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ glog_component, - nebula_ros_driver_component, + nebula_component, self_crop_component, right_mirror_crop_component, left_mirror_crop_component, @@ -340,15 +301,6 @@ def str2vector(string): ], ) - driver_loader = LoadComposableNodes( - composable_node_descriptions=[ - nebula_ros_hw_interface_component, - nebula_ros_hw_monitor_component, - ], - target_container=container, - condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), - ) - ring_outlier_filter_loader = LoadComposableNodes( composable_node_descriptions=[ring_outlier_filter_component], target_container=container, @@ -369,7 +321,6 @@ def str2vector(string): return [ container, - driver_loader, ring_outlier_filter_loader, dual_return_filter_loader, blockage_diag_loader, @@ -387,8 +338,8 @@ def add_launch_arg(name: str, default_value=None, description=None): 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("setup_sensor", "True", "configure sensor") + add_launch_arg("launch_driver", "true", "do launch driver") + add_launch_arg("setup_sensor", "true", "configure sensor") 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") @@ -411,7 +362,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg( "distortion_corrector_node_param_file", - [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"], + [FindPackageShare("pointcloud_preprocessor"), "/config/distortion_corrector_node.param.yaml"], ) add_launch_arg("diag_span", "1000") diff --git a/aip_x2_launch/launch/sensing.launch.xml b/aip_x2_launch/launch/sensing.launch.xml index c50b68be..41ceddca 100644 --- a/aip_x2_launch/launch/sensing.launch.xml +++ b/aip_x2_launch/launch/sensing.launch.xml @@ -25,9 +25,9 @@ --> - + diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index 08cd9b7d..cf3f3d63 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -13,6 +13,7 @@ # limitations under the License. import os +from pathlib import Path from ament_index_python.packages import get_package_share_directory import launch @@ -25,15 +26,17 @@ 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 def get_lidar_make(sensor_name): if sensor_name[:6].lower() == "pandar": - return "Hesai", ".csv" - elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: - return "Velodyne", ".yaml" - return "unrecognized_sensor_model" + return "Hesai" + elif sensor_name[:3].lower() in ["vlp", "vls"]: + return "Velodyne" + raise ValueError("Unrecognized sensor model") def get_vehicle_info(context): @@ -70,19 +73,25 @@ def create_parameter_dict(*args): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) - sensor_make, sensor_extension = get_lidar_make(sensor_model) - 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, + sensor_make = get_lidar_make(sensor_model) + + # Configuration file containing sensor model's default parameters + + sensor_config = LaunchConfiguration("config_file").perform(context) + if sensor_config == "": + sensor_config = ( + Path(get_package_share_directory("nebula_ros")) + / "config" + / "lidar" + / sensor_make.lower() + / (sensor_model + ".param.yaml") + ) + else: + sensor_config = Path(sensor_config) + + assert sensor_config.exists(), "Sensor configuration not found: {}".format( + sensor_config.absolute().as_posix() ) - assert os.path.exists( - sensor_calib_fp - ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) nodes = [] @@ -97,68 +106,43 @@ def create_parameter_dict(*args): nodes.append( ComposableNode( package="nebula_ros", - plugin=sensor_make + "HwMonitorRosWrapper", - name=sensor_make.lower() + "_hardware_monitor_ros_wrapper_node", - parameters=[ - { - "sensor_model": LaunchConfiguration("sensor_model"), - "return_mode": LaunchConfiguration("return_mode"), - "frame_id": LaunchConfiguration("frame_id"), - "scan_phase": LaunchConfiguration("scan_phase"), - "sensor_ip": LaunchConfiguration("sensor_ip"), - "host_ip": LaunchConfiguration("host_ip"), - "data_port": LaunchConfiguration("data_port"), - "gnss_port": LaunchConfiguration("gnss_port"), - "packet_mtu_size": LaunchConfiguration("packet_mtu_size"), - "rotation_speed": LaunchConfiguration("rotation_speed"), - "cloud_min_angle": LaunchConfiguration("cloud_min_angle"), - "cloud_max_angle": LaunchConfiguration("cloud_max_angle"), - "diag_span": LaunchConfiguration("diag_span"), - "dual_return_distance_threshold": LaunchConfiguration( - "dual_return_distance_threshold" - ), - "delay_monitor_ms": LaunchConfiguration("delay_monitor_ms"), - }, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - 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=[ + ParameterFile(sensor_config, allow_substs=True), { - "calibration_file": sensor_calib_fp, "sensor_model": sensor_model, + "launch_hw": LaunchConfiguration("launch_driver"), **create_parameter_dict( "host_ip", "sensor_ip", "data_port", - "return_mode", + "gnss_port", + "packet_mtu_size", + "setup_sensor", + "frame_id", + "diag_span", "min_range", "max_range", - "frame_id", - "scan_phase", "cloud_min_angle", "cloud_max_angle", - "dual_return_distance_threshold", - "gnss_port", - "packet_mtu_size", - "setup_sensor", + "scan_phase", + "rotation_speed", + "return_mode", "ptp_profile", + "ptp_domain", "ptp_transport_type", "ptp_switch_type", - "ptp_domain", + "retry_hw", + "dual_return_distance_threshold", ), "launch_hw": True, }, ], remappings=[ - ("aw_points", "pointcloud_raw"), - ("aw_points_ex", "pointcloud_raw_ex"), + # cSpell:ignore knzo25 + # TODO(knzo25): fix the remapping once nebula gets updated + ("pandar_points", "pointcloud_raw_ex"), ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -260,45 +244,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", - "cloud_min_angle", - "cloud_max_angle", - "dual_return_distance_threshold", - "gnss_port", - "packet_mtu_size", - "setup_sensor", - "ptp_profile", - "ptp_transport_type", - "ptp_switch_type", - "ptp_domain", - ), - } - ], - ) - - driver_component_loader = LoadComposableNodes( - composable_node_descriptions=[driver_component], - target_container=container, - condition=IfCondition(LaunchConfiguration("launch_driver")), - ) - - return [container, driver_component_loader] + return [container] def generate_launch_description(): @@ -312,8 +258,13 @@ def add_launch_arg(name: str, default_value=None, description=None): 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("setup_sensor", "True", "configure sensor") + add_launch_arg( + "launch_driver", + "true", + "whether to connect to a sensor or to listen to packet replays instead", + ) + 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") @@ -334,10 +285,13 @@ def add_launch_arg(name: str, default_value=None, description=None): "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" ) add_launch_arg("diag_span", "1000") - add_launch_arg("delay_monitor_ms", "2000") 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") set_container_executable = SetLaunchConfiguration( "container_executable",