Merge branch 'tier4/universe' into fix/with_nebula_main_20250128

aip_x1_launch/config/dummy_diag_publisher/sensor_kit.param.yaml

@@ -12,6 +12,19 @@
 /**:
   ros__parameters:
     required_diags:
+      # sensing
+      sensing_topic_status: default
+
+      # velodyne
+      velodyne_connection: default
+      velodyne_temperature: default
+      velodyne_rpm: default
+
+      # pandar
+      pandar_connection: default
+      pandar_temperature: default
+      pandar_ptp: default
+
       # gnss
       gnss_connection: default
       gnss_data: default
@@ -21,17 +34,5 @@
       gnss_jamming: default
       fix topic status: default
 
-      # pandar
-      pandar_connection: default
-      pandar_temperature: default
-      pandar_ptp: default
-
-      # velodyne
-      velodyne_connection: default
-      velodyne_temperature: default
-      velodyne_rpm: default
-
-      sensing_topic_status: default
-
       # imu
       gyro_bias_estimator: default

aip_x1_launch/launch/lidar.launch.xml

@@ -3,7 +3,6 @@
   <arg name="launch_driver" default="true" />
   <arg name="host_ip" default="127.0.0.1"/>
   <arg name="use_concat_filter" default="true" />
-  <arg name="use_radius_search" default="false" />
   <arg name="vehicle_id" default="$(env VEHICLE_ID default)" />
   <arg name="vehicle_mirror_param_file" />
   <arg name="pointcloud_container_name" default="pointcloud_container"/>

aip_x2_gen2_launch/config/ARS548.param.yaml

@@ -10,8 +10,8 @@
     sensor_model: ARS548
     configuration_host_port: 42401
     configuration_sensor_port: 42101
-    use_sensor_time: false
-    configuration_vehicle_length: 7.2369
-    configuration_vehicle_width: 2.2916
+    use_sensor_time: true
+    configuration_vehicle_length: 7.2369 # wheel_base + front_overhang + rear_overhang
+    configuration_vehicle_width: 2.2916 # wheel_tread + left_overhang + right_overhang
     configuration_vehicle_height: 3.08
     configuration_vehicle_wheelbase: 4.76012

aip_x2_gen2_launch/config/concatenate_and_time_sync_node.param.yaml

@@ -0,0 +1,28 @@
+/**:
+  ros__parameters:
+    debug_mode: false
+    has_static_tf_only: false
+    rosbag_length: 10.0
+    maximum_queue_size: 5
+    timeout_sec: 0.2
+    is_motion_compensated: true
+    publish_synchronized_pointcloud: true
+    keep_input_frame_in_synchronized_pointcloud: true
+    publish_previous_but_late_pointcloud: false
+    synchronized_pointcloud_postfix: pointcloud
+    input_twist_topic_type: twist
+    input_topics: [
+                    "/sensing/lidar/rear_upper/pointcloud_before_sync", # 0.047
+                    "/sensing/lidar/rear_lower/pointcloud_before_sync", # 0.048
+                    "/sensing/lidar/left_upper/pointcloud_before_sync", # 0.048
+                    "/sensing/lidar/left_lower/pointcloud_before_sync", # 0.047
+                    "/sensing/lidar/front_upper/pointcloud_before_sync", # 0.072
+                    "/sensing/lidar/front_lower/pointcloud_before_sync", # 0.072
+                    "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.073
+                    "/sensing/lidar/right_lower/pointcloud_before_sync" # 0.097
+                ]
+    output_frame: base_link
+    matching_strategy:
+      type: advanced
+      lidar_timestamp_offsets: [0.0, 0.001, 0.001, 0.0, 0.025, 0.025, 0.026, 0.050]
+      lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]

aip_x2_gen2_launch/launch/hesai_OT128.launch.xml

@@ -21,7 +21,6 @@
   <arg name="setup_sensor" default="true"/>
   <arg name="min_range" default="0.3"/>
   <arg name="max_range" default="230.0"/>
-  <arg name="distance_range" default="[0.3, 230.0]"/>
   <arg name="blockage_range" default="[90.0, 270.0]"/>
   <arg name="vertical_bins" default ="128" />
   <arg name="horizontal_ring_id" default="12" />
@@ -60,7 +59,6 @@
 
     <arg name="min_range" value="$(var min_range)"/>
     <arg name="max_range" value="$(var max_range)"/>
-    <arg name="distance_range" value="$(var distance_range)"/>
     <arg name="blockage_range" value="$(var blockage_range)"/>
     <arg name="vertical_bins" value ="$(var vertical_bins)"/>
     <arg name="horizontal_ring_id" value="$(var horizontal_ring_id)" />

aip_x2_gen2_launch/launch/hesai_QT128.launch.xml

@@ -22,7 +22,6 @@
   <arg name="container_name" default="hesai_node_container"/>
   <arg name="min_range" default="0.05"/>
   <arg name="max_range" default="50.0"/>
-  <arg name="distance_range" default="[0.05, 50.0]"/>
   <arg name="blockage_range" default="[90.0, 270.0]"/>
   <arg name="vertical_bins" default ="128" />
   <arg name="horizontal_ring_id" default="12" />
@@ -59,7 +58,6 @@
     <arg name="container_name" value="$(var container_name)"/>
     <arg name="min_range" value="$(var min_range)"/>
     <arg name="max_range" value="$(var max_range)"/>
-    <arg name="distance_range" value="$(var distance_range)"/>
     <arg name="blockage_range" value="$(var blockage_range)"/>
     <arg name="vertical_bins" value ="$(var vertical_bins)"/>
     <arg name="horizontal_ring_id" value="$(var horizontal_ring_id)" />

aip_x2_gen2_launch/launch/nebula_node_container.launch.py

@@ -26,6 +26,7 @@
 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
 
@@ -76,7 +77,7 @@ def str2vector(string):
 
     glog_component = ComposableNode(
         package="autoware_glog_component",
-        plugin="GlogComponent",
+        plugin="autoware::glog_component::GlogComponent",
         name="glog_component",
     )
 
@@ -161,14 +162,27 @@ def str2vector(string):
         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_filter_component = ComposableNode(
         package="autoware_pointcloud_preprocessor",
         plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent",
         name="ring_outlier_filter",
         remappings=[
             ("input", "rectified/pointcloud_ex"),
-            ("output", "pointcloud"),
+            ("output", "pointcloud_before_sync"),
         ],
+        parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
@@ -178,7 +192,7 @@ def str2vector(string):
         name="dual_return_filter",
         remappings=[
             ("input", "rectified/pointcloud_ex"),
-            ("output", "pointcloud"),
+            ("output", "pointcloud_before_sync"),
         ],
         parameters=[
             {
@@ -191,7 +205,6 @@ def str2vector(string):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
-    distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range")))
     blockage_diag_component = ComposableNode(
         package="autoware_pointcloud_preprocessor",
         plugin="autoware::pointcloud_preprocessor::BlockageDiagComponent",
@@ -206,7 +219,7 @@ def str2vector(string):
                 "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
                 "vertical_bins": LaunchConfiguration("vertical_bins"),
                 "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
-                "max_distance_range": distance_range[1],
+                "max_distance_range": LaunchConfiguration("max_range"),
                 "horizontal_resolution": LaunchConfiguration("horizontal_resolution"),
             }
         ]
@@ -299,6 +312,10 @@ def add_launch_arg(name: str, default_value=None, description=None):
         "blockage_diagnostics_param_file",
         [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics.param.yaml"],
     )
+    add_launch_arg(
+        "ring_outlier_filter_node_param_file",
+        [FindPackageShare("common_sensor_launch"), "/config/ring_outlier_filter_node.param.yaml"],
+    )
     add_launch_arg(
         "distortion_corrector_node_param_file",
         [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"],
@@ -313,6 +330,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("point_filters_param_file")
 
     add_launch_arg("calibration_file", "")
+    add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",

aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py

@@ -12,7 +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
@@ -22,9 +24,18 @@
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterFile
 
 
 def launch_setup(context, *args, **kwargs):
+    # concatenate node parameters
+    concatenate_and_time_sync_node_param = ParameterFile(
+        param_file=LaunchConfiguration("concatenate_and_time_sync_node_param_path").perform(
+            context
+        ),
+        allow_substs=True,
+    )
+
     # set concat filter as a component
     concat_component = ComposableNode(
         package="autoware_pointcloud_preprocessor",
@@ -34,30 +45,15 @@ def launch_setup(context, *args, **kwargs):
             ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
             ("output", "concatenated/pointcloud"),
         ],
-        parameters=[
-            {
-                "input_topics": [
-                    "/sensing/lidar/front_upper/pointcloud",
-                    "/sensing/lidar/front_lower/pointcloud",
-                    "/sensing/lidar/left_upper/pointcloud",
-                    "/sensing/lidar/left_lower/pointcloud",
-                    "/sensing/lidar/right_upper/pointcloud",
-                    "/sensing/lidar/right_lower/pointcloud",
-                    "/sensing/lidar/rear_upper/pointcloud",
-                    "/sensing/lidar/rear_lower/pointcloud",
-                ],
-                "input_offset": [0.005, 0.025, 0.050, 0.005, 0.050, 0.005, 0.005, 0.025],
-                "timeout_sec": 0.075,
-                "output_frame": LaunchConfiguration("base_frame"),
-                "input_twist_topic_type": "twist",
-            }
-        ],
+        parameters=[concatenate_and_time_sync_node_param],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    # load concat or passthrough filter
     concat_loader = LoadComposableNodes(
         composable_node_descriptions=[concat_component],
         target_container=LaunchConfiguration("pointcloud_container_name"),
+        condition=IfCondition(LaunchConfiguration("use_concat_filter")),
     )
 
     return [concat_loader]
@@ -69,10 +65,19 @@ def generate_launch_description():
     def add_launch_arg(name: str, default_value=None):
         launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
 
-    add_launch_arg("base_frame", "base_link")
+    aip_x2_gen2_launch_share_dir = get_package_share_directory("aip_x2_gen2_launch")
+
     add_launch_arg("use_multithread", "True")
     add_launch_arg("use_intra_process", "True")
     add_launch_arg("pointcloud_container_name", "pointcloud_container")
+    add_launch_arg(
+        "concatenate_and_time_sync_node_param_path",
+        os.path.join(
+            aip_x2_gen2_launch_share_dir,
+            "config",
+            "concatenate_and_time_sync_node.param.yaml",
+        ),
+    )
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",

aip_x2_gen2_launch/launch/radar.launch.xml

@@ -7,16 +7,24 @@
   <let name="steering_angle_topic" value="/vehicle/status/steering_status_scalar"/>
   <arg name="radar_tracks_msgs_converter_param_path" default="$(find-pkg-share common_sensor_launch)/config/radar_tracks_msgs_converter.param.yaml"/>
 
+  <let name="odometry_throttled_topic" value="$(var odometry_topic)_throttled"/>
+  <let name="acceleration_throttled_topic" value="$(var acceleration_topic)_throttled"/>
+  <let name="steering_angle_throttled_topic" value="$(var steering_angle_topic)_throttled"/>
+
+  <node pkg="topic_tools" exec="throttle" name="odometry_throttler" output="screen" args="messages $(var odometry_topic) 25.0 $(var odometry_throttled_topic)"/>
+  <node pkg="topic_tools" exec="throttle" name="acceleration_throttler" output="screen" args="messages $(var acceleration_topic) 25.0 $(var acceleration_throttled_topic)"/>
+  <node pkg="topic_tools" exec="throttle" name="steering_angle_throttler" output="screen" args="messages $(var steering_angle_topic) 25.0 $(var steering_angle_throttled_topic)"/>
+
   <group>
     <push-ros-namespace namespace="radar"/>
     <node pkg="topic_tools" exec="transform" name="steer_angle_transform" output="screen" args="/vehicle/status/steering_status /vehicle/status/steering_status_scalar std_msgs/msg/Float32 'std_msgs.msg.Float32(data=m.steering_tire_angle)'  --import autoware_vehicle_msgs std_msgs --wait-for-start"/>
 
     <group>
       <push-ros-namespace namespace="front_center"/>
       <include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
-        <arg name="odometry_topic" value="$(var odometry_topic)"/>
-        <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
-        <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
+        <arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
+        <arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
+        <arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
         <arg name="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.114"/>
@@ -45,9 +53,9 @@
     <group>
       <push-ros-namespace namespace="front_left"/>
       <include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
-        <arg name="odometry_topic" value="$(var odometry_topic)"/>
-        <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
-        <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
+        <arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
+        <arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
+        <arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
         <arg name="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.115"/>
@@ -75,9 +83,9 @@
     <group>
       <push-ros-namespace namespace="front_right"/>
       <include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
-        <arg name="odometry_topic" value="$(var odometry_topic)"/>
-        <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
-        <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
+        <arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
+        <arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
+        <arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
         <arg name="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.116"/>
@@ -105,9 +113,9 @@
     <group>
       <push-ros-namespace namespace="rear_center"/>
       <include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
-        <arg name="odometry_topic" value="$(var odometry_topic)"/>
-        <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
-        <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
+        <arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
+        <arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
+        <arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
         <arg name="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.117"/>
@@ -135,9 +143,9 @@
     <group>
       <push-ros-namespace namespace="rear_left"/>
       <include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
-        <arg name="odometry_topic" value="$(var odometry_topic)"/>
-        <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
-        <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
+        <arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
+        <arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
+        <arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
         <arg name="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.118"/>
@@ -165,9 +173,9 @@
     <group>
       <push-ros-namespace namespace="rear_right"/>
       <include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
-        <arg name="odometry_topic" value="$(var odometry_topic)"/>
-        <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
-        <arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
+        <arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
+        <arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
+        <arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
         <arg name="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.119"/>