feat: change from velodyne_vls to nebula driver (#64)

* feat: change from velodyne_vls to nebula driver Signed-off-by: David Wong <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: David Wong <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Aug 2, 2023 · 886e41d · 886e41d
1 parent 5a66ef0
commit 886e41d
Show file tree

Hide file tree

Showing 4 changed files with 131 additions and 94 deletions.
diff --git a/.../launch/velodyne_node_container.launch.py → ...ch/launch/nebula_node_container.launch.py b/.../launch/velodyne_node_container.launch.py → ...ch/launch/nebula_node_container.launch.py
@@ -1,4 +1,4 @@
-# Copyright 2020 Tier IV, Inc. All rights reserved.
+# Copyright 2023 Tier IV, Inc. All rights reserved.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -12,6 +12,10 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+import warnings
+
+from ament_index_python.packages import get_package_share_directory
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import OpaqueFunction
@@ -25,6 +29,14 @@
 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"
+
+
 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
@@ -57,33 +69,63 @@ def create_parameter_dict(*args):
             result[x] = LaunchConfiguration(x)
         return result
 
+    # 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")
+    nebula_ros_share_dir = get_package_share_directory("nebula_ros")
+
+    # Config
+    sensor_params_fp = LaunchConfiguration("config_file").perform(context)
+    if sensor_params_fp == "":
+        warnings.warn("No config file provided, using sensor model default", RuntimeWarning)
+        sensor_params_fp = os.path.join(
+            nebula_ros_share_dir, "config", sensor_make.lower(), sensor_model + ".yaml"
+        )
+    sensor_calib_fp = os.path.join(
+        nebula_decoders_share_dir,
+        "calibration",
+        sensor_make.lower(),
+        sensor_model + sensor_extension,
+    )
+    if not os.path.exists(sensor_params_fp):
+        sensor_params_fp = os.path.join(nebula_ros_share_dir, "config", "BaseParams.yaml")
+    assert os.path.exists(
+        sensor_params_fp
+    ), "Sensor params yaml file under config/ was not found: {}".format(sensor_params_fp)
+    assert os.path.exists(
+        sensor_calib_fp
+    ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp)
+    with open(sensor_params_fp, "r") as f:
+        sensor_params = yaml.safe_load(f)["/**"]["ros__parameters"]
+
     nodes = []
 
-    # turn packets into pointcloud as in
-    # https://github.com/ros-drivers/velodyne/blob/ros2/velodyne_pointcloud/launch/velodyne_convert_node-VLP16-composed-launch.py
     nodes.append(
         ComposableNode(
-            package="velodyne_pointcloud",
-            plugin="velodyne_pointcloud::Convert",
-            name="velodyne_convert_node",
+            package="nebula_ros",
+            plugin=sensor_make + "DriverRosWrapper",
+            name=sensor_make.lower() + "_driver_ros_wrapper_node",
             parameters=[
+                sensor_params,
                 {
+                    "calibration_file": sensor_calib_fp,
+                    "sensor_model": sensor_model,
                     **create_parameter_dict(
-                        "calibration",
+                        "return_mode",
                         "min_range",
                         "max_range",
-                        "num_points_thresholds",
-                        "invalid_intensity",
                         "frame_id",
                         "scan_phase",
-                        "view_direction",
-                        "view_width",
+                        "cloud_min_angle",
+                        "cloud_max_angle",
+                        "dual_return_distance_threshold",
                     ),
-                }
+                },
             ],
             remappings=[
-                ("velodyne_points", "pointcloud_raw"),
-                ("velodyne_points_ex", "pointcloud_raw_ex"),
+                ("aw_points", "pointcloud_raw"),
+                ("aw_points_ex", "pointcloud_raw_ex"),
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
@@ -164,6 +206,7 @@ def create_parameter_dict(*args):
         )
     )
 
+    # set container to run all required components in the same process
     container = ComposableNodeContainer(
         name=LaunchConfiguration("container_name"),
         namespace="pointcloud_preprocessor",
@@ -181,24 +224,25 @@ def create_parameter_dict(*args):
     )
 
     driver_component = ComposableNode(
-        package="velodyne_driver",
-        plugin="velodyne_driver::VelodyneDriver",
+        package="nebula_ros",
+        plugin=sensor_make + "HwInterfaceRosWrapper",
         # node is created in a global context, need to avoid name clash
-        name="velodyne_driver",
+        name=sensor_make.lower() + "_hw_interface_ros_wrapper_node",
         parameters=[
             {
+                "sensor_model": sensor_model,
+                "calibration_file": sensor_calib_fp,
                 **create_parameter_dict(
-                    "device_ip",
-                    "gps_time",
-                    "read_once",
-                    "read_fast",
-                    "repeat_delay",
-                    "frame_id",
-                    "model",
-                    "rpm",
-                    "port",
-                    "pcap",
+                    "sensor_ip",
+                    "host_ip",
                     "scan_phase",
+                    "return_mode",
+                    "frame_id",
+                    "rotation_speed",
+                    "data_port",
+                    "gnss_port",
+                    "cloud_min_angle",
+                    "cloud_max_angle",
                 ),
             }
         ],
@@ -228,38 +272,31 @@ def add_launch_arg(name: str, default_value=None, description=None):
             DeclareLaunchArgument(name, default_value=default_value, description=description)
         )
 
-    add_launch_arg("model", description="velodyne model name")
+    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("calibration", description="path to calibration file")
-    add_launch_arg("device_ip", "192.168.1.201", "device ip address")
+    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")
     add_launch_arg("base_frame", "base_link", "base frame id")
-    add_launch_arg("container_name", "velodyne_composable_node_container", "container name")
-    add_launch_arg("min_range", description="minimum view range")
-    add_launch_arg("max_range", description="maximum view range")
-    add_launch_arg("pcap", "")
-    add_launch_arg("port", "2368", description="device port number")
-    add_launch_arg("read_fast", "False")
-    add_launch_arg("read_once", "False")
-    add_launch_arg("repeat_delay", "0.0")
-    add_launch_arg("rpm", "600.0", "rotational frequency")
-    add_launch_arg("laserscan_ring", "-1")
-    add_launch_arg("laserscan_resolution", "0.007")
-    add_launch_arg("num_points_thresholds", "300")
-    add_launch_arg("invalid_intensity")
-    add_launch_arg("frame_id", "velodyne", "velodyne frame id")
-    add_launch_arg("gps_time", "False")
-    add_launch_arg("view_direction", description="the center of lidar angle")
-    add_launch_arg("view_width", description="lidar angle: 0~6.28 [rad]")
+    add_launch_arg("min_range", "0.3", "minimum view range for Velodyne sensors")
+    add_launch_arg("max_range", "300.0", "maximum view range for Velodyne sensors")
+    add_launch_arg("cloud_min_angle", "0", "minimum view angle setting on device")
+    add_launch_arg("cloud_max_angle", "360", "maximum view angle setting on device")
+    add_launch_arg("data_port", "2368", "device data port number")
+    add_launch_arg("gnss_port", "2380", "device gnss port number")
+    add_launch_arg("rotation_speed", "600.0", "rotational frequency")
+    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(
         "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml"
     )
     add_launch_arg("use_multithread", "False", "use multithread")
-    add_launch_arg("use_intra_process", "False", "use ROS2 component container communication")
+    add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication")
     add_launch_arg("use_pointcloud_container", "false")
-    add_launch_arg("container_name", "velodyne_node_container")
+    add_launch_arg("container_name", "nebula_node_container")
 
     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
@@ -1,35 +1,34 @@
 <launch>
   <!-- Params -->
   <arg name="launch_driver" default="true"/>
-
   <arg name="model" default="VLP16"/>
-  <arg name="calibration" default="$(find-pkg-share velodyne_pointcloud)/params/VLP16db.yaml"/>
   <arg name="max_range" default="130.0"/>
   <arg name="min_range" default="0.4"/>
-  <arg name="invalid_intensity" default="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]"/>
   <arg name="sensor_frame" default="velodyne"/>
+  <arg name="return_mode" default="SingleStrongest"/>
   <arg name="device_ip" default="192.168.1.201"/>
-  <arg name="port" default="2368"/>
+  <arg name="host_ip" default="255.255.255.255"/>
+  <arg name="data_port" default="2368"/>
   <arg name="scan_phase" default="0.0"/>
-  <arg name="view_direction" default="0.0"/>
-  <arg name="view_width" default="6.28"/>
+  <arg name="cloud_min_angle" default="0"/>
+  <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
   <arg name="use_pointcloud_container" default="false"/>
   <arg name="container_name" default="velodyne_node_container"/>
 
-  <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_node_container.launch.py">
+  <include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
-    <arg name="model" value="$(var model)"/>
-    <arg name="calibration" value="$(var calibration)"/>
+    <arg name="sensor_model" value="$(var model)"/>
+    <arg name="return_mode" value="$(var return_mode)"/>
     <arg name="max_range" value="$(var max_range)"/>
     <arg name="min_range" value="$(var min_range)"/>
-    <arg name="invalid_intensity" value="$(var invalid_intensity)"/>
     <arg name="frame_id" value="$(var sensor_frame)"/>
-    <arg name="device_ip" value="$(var device_ip)"/>
-    <arg name="port" value="$(var port)"/>
+    <arg name="sensor_ip" value="$(var device_ip)"/>
+    <arg name="host_ip" value="$(var host_ip)"/>
+    <arg name="data_port" value="$(var data_port)"/>
     <arg name="scan_phase" value="$(var scan_phase)"/>
-    <arg name="view_direction" value="$(var view_direction)"/>
-    <arg name="view_width" value="$(var view_width)"/>
+    <arg name="cloud_min_angle" value="$(var cloud_min_angle)"/>
+    <arg name="cloud_max_angle" value="$(var cloud_max_angle)"/>
     <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="false"/>

diff --git a/common_sensor_launch/launch/velodyne_VLS128.launch.xml b/common_sensor_launch/launch/velodyne_VLS128.launch.xml
@@ -1,38 +1,34 @@
 <launch>
   <!-- Params -->
   <arg name="launch_driver" default="true"/>
-
   <arg name="model" default="VLS128"/>
-  <arg name="calibration" default="$(find-pkg-share velodyne_pointcloud)/params/VLS-128_FS1.yaml"/>
   <arg name="max_range" default="250.0"/>
   <arg name="min_range" default="0.5"/>
-  <arg
-    name="invalid_intensity"
-    default="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]"
-  />
   <arg name="sensor_frame" default="velodyne"/>
+  <arg name="return_mode" default="SingleStrongest"/>
   <arg name="device_ip" default="192.168.1.201"/>
-  <arg name="port" default="2368"/>
+  <arg name="host_ip" default="255.255.255.255"/>
+  <arg name="data_port" default="2368"/>
   <arg name="scan_phase" default="0.0"/>
-  <arg name="view_direction" default="0.0"/>
-  <arg name="view_width" default="6.28"/>
+  <arg name="cloud_min_angle" default="0"/>
+  <arg name="cloud_max_angle" default="360"/>
   <arg name="vehicle_mirror_param_file"/>
   <arg name="use_pointcloud_container" default="false"/>
   <arg name="container_name" default="velodyne_node_container"/>
 
-  <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_node_container.launch.py">
+  <include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
     <arg name="launch_driver" value="$(var launch_driver)"/>
-    <arg name="model" value="$(var model)"/>
-    <arg name="calibration" value="$(var calibration)"/>
+    <arg name="sensor_model" value="$(var model)"/>
+    <arg name="return_mode" value="$(var return_mode)"/>
     <arg name="max_range" value="$(var max_range)"/>
     <arg name="min_range" value="$(var min_range)"/>
-    <arg name="invalid_intensity" value="$(var invalid_intensity)"/>
     <arg name="frame_id" value="$(var sensor_frame)"/>
-    <arg name="device_ip" value="$(var device_ip)"/>
-    <arg name="port" value="$(var port)"/>
+    <arg name="sensor_ip" value="$(var device_ip)"/>
+    <arg name="host_ip" value="$(var host_ip)"/>
+    <arg name="data_port" value="$(var data_port)"/>
     <arg name="scan_phase" value="$(var scan_phase)"/>
-    <arg name="view_direction" value="$(var view_direction)"/>
-    <arg name="view_width" value="$(var view_width)"/>
+    <arg name="cloud_min_angle" value="$(var cloud_min_angle)"/>
+    <arg name="cloud_max_angle" value="$(var cloud_max_angle)"/>
     <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
     <arg name="use_intra_process" value="true"/>
     <arg name="use_multithread" value="true"/>

diff --git a/sample_sensor_kit_launch/launch/lidar.launch.xml b/sample_sensor_kit_launch/launch/lidar.launch.xml
@@ -1,5 +1,6 @@
 <launch>
   <arg name="launch_driver" default="true"/>
+  <arg name="host_ip" default="192.168.1.10"/>
   <arg name="use_concat_filter" default="true"/>
   <arg name="vehicle_id" default="$(env VEHICLE_ID default)"/>
   <arg name="vehicle_mirror_param_file"/>
@@ -12,12 +13,12 @@
     <group>
       <push-ros-namespace namespace="top"/>
       <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_VLS128.launch.xml">
+        <arg name="max_range" value="250.0"/>
         <arg name="sensor_frame" value="velodyne_top"/>
         <arg name="device_ip" value="192.168.1.201"/>
-        <arg name="port" value="2368"/>
+        <arg name="host_ip" value="$(var host_ip)"/>
+        <arg name="data_port" value="2368"/>
         <arg name="scan_phase" value="300.0"/>
-        <!-- TODO(fred-apex-ai) sensor_timestamp not supported anymore in ros2 driver but default values changed here -->
-        <!-- <arg name="sensor_timestamp" value="false" /> -->
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
@@ -28,13 +29,14 @@
     <group>
       <push-ros-namespace namespace="left"/>
       <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_VLP16.launch.xml">
+        <arg name="max_range" value="5.0"/>
         <arg name="sensor_frame" value="velodyne_left"/>
         <arg name="device_ip" value="192.168.1.202"/>
-        <arg name="port" value="2369"/>
+        <arg name="host_ip" value="$(var host_ip)"/>
+        <arg name="data_port" value="2369"/>
         <arg name="scan_phase" value="180.0"/>
-        <arg name="max_range" value="15.0"/>
-        <!-- TODO(fred-apex-ai) sensor_timestamp not supported anymore in ros2 driver but default values changed here -->
-        <!-- <arg name="sensor_timestamp" value="false" /> -->
+        <arg name="cloud_min_angle" value="300"/>
+        <arg name="cloud_max_angle" value="60"/>
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
@@ -45,13 +47,14 @@
     <group>
       <push-ros-namespace namespace="right"/>
       <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_VLP16.launch.xml">
+        <arg name="max_range" value="5.0"/>
         <arg name="sensor_frame" value="velodyne_right"/>
         <arg name="device_ip" value="192.168.1.203"/>
-        <arg name="port" value="2370"/>
+        <arg name="host_ip" value="$(var host_ip)"/>
+        <arg name="data_port" value="2370"/>
         <arg name="scan_phase" value="180.0"/>
-        <arg name="max_range" value="15.0"/>
-        <!-- TODO(fred-apex-ai) sensor_timestamp not supported anymore in ros2 driver but default values changed here -->
-        <!-- <arg name="sensor_timestamp" value="false" /> -->
+        <arg name="cloud_min_angle" value="300"/>
+        <arg name="cloud_max_angle" value="60"/>
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
@@ -62,12 +65,14 @@
     <group>
       <push-ros-namespace namespace="rear"/>
       <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_VLP16.launch.xml">
+        <arg name="max_range" value="1.5"/>
         <arg name="sensor_frame" value="velodyne_rear"/>
         <arg name="device_ip" value="192.168.1.204"/>
-        <arg name="port" value="2371"/>
+        <arg name="host_ip" value="$(var host_ip)"/>
+        <arg name="data_port" value="2371"/>
         <arg name="scan_phase" value="180.0"/>
-        <!-- TODO(fred-apex-ai) sensor_timestamp not supported anymore in ros2 driver but default values changed here -->
-        <!-- <arg name="sensor_timestamp" value="false" /> -->
+        <arg name="cloud_min_angle" value="300"/>
+        <arg name="cloud_max_angle" value="60"/>
         <arg name="launch_driver" value="$(var launch_driver)"/>
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>