Merge branch 'main' into HEAD

Signed-off-by: yoshiri <[email protected]>
autowarefoundation · Dec 15, 2023 · 9a95283 · 9a95283
2 parents f1b7294 + 5ccd754
commit 9a95283
Show file tree

Hide file tree

Showing 9 changed files with 169 additions and 131 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,9 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import OpaqueFunction
@@ -25,6 +28,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 +68,51 @@ 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")
+
+    # Calibration file
+    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)
+
     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=[
                 {
+                    "calibration_file": sensor_calib_fp,
+                    "sensor_model": sensor_model,
                     **create_parameter_dict(
-                        "calibration",
+                        "host_ip",
+                        "sensor_ip",
+                        "data_port",
+                        "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")}],
         )
@@ -158,12 +187,13 @@ def create_parameter_dict(*args):
             name="ring_outlier_filter",
             remappings=[
                 ("input", "rectified/pointcloud_ex"),
-                ("output", "outlier_filtered/pointcloud"),
+                ("output", "pointcloud"),
             ],
             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("container_name"),
         namespace="pointcloud_preprocessor",
@@ -181,24 +211,28 @@ 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",
+                    "packet_mtu_size",
+                    "dual_return_distance_threshold",
+                    "setup_sensor",
                 ),
             }
         ],
@@ -228,38 +262,33 @@ 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("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")
     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("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("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="device_ip" default="192.168.1.201"/>
-  <arg name="port" default="2368"/>
+  <arg name="return_mode" default="SingleStrongest"/>
+  <arg name="sensor_ip" default="192.168.1.201"/>
+  <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 sensor_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"/>
@@ -39,6 +38,6 @@
 
   <!-- Velodyne Monitor -->
   <include file="$(find-pkg-share velodyne_monitor)/launch/velodyne_monitor.launch.xml" if="$(var launch_driver)">
-    <arg name="ip_address" value="$(var device_ip)"/>
+    <arg name="ip_address" value="$(var sensor_ip)"/>
   </include>
 </launch>
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="device_ip" default="192.168.1.201"/>
-  <arg name="port" default="2368"/>
+  <arg name="return_mode" default="SingleStrongest"/>
+  <arg name="sensor_ip" default="192.168.1.201"/>
+  <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 sensor_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"/>
@@ -42,6 +38,6 @@
 
   <!-- Velodyne Monitor -->
   <include file="$(find-pkg-share velodyne_monitor)/launch/velodyne_monitor.launch.xml" if="$(var launch_driver)">
-    <arg name="ip_address" value="$(var device_ip)"/>
+    <arg name="ip_address" value="$(var sensor_ip)"/>
   </include>
 </launch>
diff --git a/common_sensor_launch/package.xml b/common_sensor_launch/package.xml
@@ -10,9 +10,8 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
-  <exec_depend>velodyne_driver</exec_depend>
+  <exec_depend>nebula_sensor_driver</exec_depend>
   <exec_depend>velodyne_monitor</exec_depend>
-  <exec_depend>velodyne_pointcloud</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

diff --git a/sample_sensor_kit_launch/launch/gnss.launch.xml b/sample_sensor_kit_launch/launch/gnss.launch.xml
@@ -1,7 +1,6 @@
 <launch>
   <arg name="launch_driver" default="true"/>
   <arg name="gnss_receiver" default="ublox" description="ublox(default) or septentrio"/>
-  <arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE"/>
 
   <group>
     <push-ros-namespace namespace="gnss"/>
@@ -33,7 +32,6 @@
       <arg name="output_topic_gnss_pose_cov" value="pose_with_covariance"/>
       <arg name="output_topic_gnss_fixed" value="fixed"/>
 
-      <arg name="coordinate_system" value="$(var coordinate_system)"/>
       <arg name="use_gnss_ins_orientation" value="true"/>
 
       <arg name="gnss_frame" value="gnss_link"/>

diff --git a/sample_sensor_kit_launch/launch/imu.launch.xml b/sample_sensor_kit_launch/launch/imu.launch.xml
@@ -13,10 +13,18 @@
       </node>
     </group>
 
+    <arg name="imu_raw_name" default="tamagawa/imu_raw"/>
+    <arg name="imu_corrector_param_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/sample_sensor_kit/imu_corrector.param.yaml"/>
     <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
-      <arg name="input_topic" value="tamagawa/imu_raw"/>
+      <arg name="input_topic" value="$(var imu_raw_name)"/>
       <arg name="output_topic" value="imu_data"/>
-      <arg name="param_file" value="$(find-pkg-share individual_params)/config/$(env VEHICLE_ID default)/sample_sensor_kit/imu_corrector.param.yaml"/>
+      <arg name="param_file" value="$(var imu_corrector_param_file)"/>
+    </include>
+
+    <include file="$(find-pkg-share imu_corrector)/launch/gyro_bias_estimator.launch.xml">
+      <arg name="input_imu_raw" value="$(var imu_raw_name)"/>
+      <arg name="input_odom" value="/localization/kinematic_state"/>
+      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
     </include>
   </group>
 </launch>