diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index d24b1ff..acb12c6 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 @@ -31,10 +32,10 @@ def get_lidar_make(sensor_name): if sensor_name[:6].lower() == "pandar": - return "Hesai", ".csv" + return "Hesai" elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: - return "Velodyne", ".yaml" - return "unrecognized_sensor_model" + return "Velodyne" + raise ValueError("Unrecognized sensor model") def get_vehicle_info(context): @@ -71,19 +72,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) # Pointcloud preprocessor parameters distortion_corrector_node_param = ParameterFile( @@ -104,23 +111,28 @@ 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=[ + 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", "min_range", "max_range", - "frame_id", - "scan_phase", "cloud_min_angle", "cloud_max_angle", + "scan_phase", + "rotation_speed", + "return_mode", "dual_return_distance_threshold", ), }, @@ -228,41 +240,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", - ), - } - ], - ) - - 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():