Merge branch 'tier4/universe' into feature/xx1-gen2

aip_x1_launch/config/gyro_bias_estimator.param.yaml

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    gyro_bias_threshold: 0.008 # [rad/s]
+    timer_callback_interval_sec: 0.5 # [sec]
+    diagnostics_updater_interval_sec: 0.5 # [sec]
+    straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]

aip_x1_launch/launch/imu.launch.xml

@@ -14,10 +14,12 @@
       <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
+    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share aip_x1_launch)/config/gyro_bias_estimator.param.yaml"/>
     <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)"/>
+      <arg name="gyro_bias_estimator_param_file" value="$(var gyro_bias_estimator_param_file)"/>
     </include>
   </group>
 

aip_x1_launch/launch/lidar.launch.xml

@@ -4,7 +4,6 @@
   <arg name="use_concat_filter" default="true" />
   <arg name="use_radius_search" default="false" />
   <arg name="vehicle_mirror_param_file" />
-  <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
 
   <!-- x1 additional setting -->
@@ -57,7 +56,6 @@
       <arg name="base_frame" value="base_link" />
       <arg name="use_intra_process" value="true" />
       <arg name="use_multithread" value="false" />
-      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
       <arg name="container_name" value="$(var pointcloud_container_name)"/>
     </include>
 

aip_x1_launch/launch/new_livox_horizon.launch.py

@@ -52,12 +52,12 @@ def get_crop_box_min_range_component(context, livox_frame_id):
         name="crop_box_filter_min_range",
         remappings=[
             ("input", "livox/tag_filtered/lidar" if use_tag_filter else "livox/lidar"),
-            ("output", "min_range_cropped/pointcloud"),
+            ("output", "min_range_cropped/pointcloud_before_sync"),
         ],
         parameters=[
             {
                 "input_frame": livox_frame_id,
-                "output_frame": LaunchConfiguration("base_frame"),
+                "output_frame": LaunchConfiguration("frame_id"),
                 "min_x": 0.0,
                 "max_x": LaunchConfiguration("min_range"),
                 "min_y": -2.0,

aip_x1_launch/launch/pointcloud_preprocessor.launch.py

@@ -19,7 +19,6 @@
 from launch.conditions import IfCondition
 from launch.conditions import UnlessCondition
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
 
@@ -37,43 +36,27 @@ def launch_setup(context, *args, **kwargs):
         parameters=[
             {
                 "input_topics": [
-                    "/sensing/lidar/top/pointcloud",
-                    "/sensing/lidar/front_left/min_range_cropped/pointcloud",
-                    "/sensing/lidar/front_right/min_range_cropped/pointcloud",
-                    "/sensing/lidar/front_center/min_range_cropped/pointcloud",
+                    "/sensing/lidar/top/pointcloud_before_sync",
+                    "/sensing/lidar/front_left/min_range_cropped/pointcloud_before_sync",
+                    "/sensing/lidar/front_right/min_range_cropped/pointcloud_before_sync",
+                    "/sensing/lidar/front_center/min_range_cropped/pointcloud_before_sync",
                 ],
                 "output_frame": LaunchConfiguration("base_frame"),
                 "timeout_sec": 1.0,
                 "input_twist_topic_type": "twist",
+                "publish_synchronized_pointcloud": True,
             }
         ],
         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("individual_container_name"),
-        namespace="",
-        package="rclcpp_components",
-        executable=LaunchConfiguration("container_executable"),
-        composable_node_descriptions=[],
-        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
-        output="screen",
-    )
-
-    target_container = (
-        container
-        if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
-        else LaunchConfiguration("pointcloud_container_name")
-    )
-
     # load concat or passthrough filter
     concat_loader = LoadComposableNodes(
         composable_node_descriptions=[concat_component],
-        target_container=target_container,
+        target_container=LaunchConfiguration("pointcloud_container_name"),
     )
 
-    return [container, concat_loader]
+    return [concat_loader]
 
 
 def generate_launch_description():
@@ -85,9 +68,7 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("base_frame", "base_link")
     add_launch_arg("use_multithread", "False")
     add_launch_arg("use_intra_process", "False")
-    add_launch_arg("use_pointcloud_container", "False")
     add_launch_arg("pointcloud_container_name", "pointcloud_container")
-    add_launch_arg("individual_container_name", "concatenate_container")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",

aip_x1_launch/launch/sensing.launch.xml

@@ -2,7 +2,6 @@
 
   <arg name="launch_driver" default="true" description="do launch driver"/>
   <arg name="vehicle_mirror_param_file" description="path to the file of vehicle mirror position yaml"/>
-  <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
   <arg name ="vehicle_id" default="$(env VEHICLE_ID default)" />
 
@@ -12,7 +11,6 @@
     <include file="$(find-pkg-share aip_x1_launch)/launch/lidar.launch.xml">
       <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)" />
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
     </include>
 

aip_x1_launch/launch/topic_state_monitor.launch.py

@@ -155,7 +155,7 @@ def generate_launch_description():
         name="topic_state_monitor_rough_no_ground",
         parameters=[
             {
-                "topic": "/perception/obstacle_segmentation/single_frame/pointcloud_raw",
+                "topic": "/perception/obstacle_segmentation/single_frame/pointcloud",
                 "topic_type": "sensor_msgs/msg/PointCloud2",
                 "best_effort": True,
                 "diag_name": "sensing_topic_status",

aip_x1_launch/launch/topic_state_monitor.launch.xml

@@ -76,7 +76,7 @@
   <!-- Topic Monitor for Concat Filter -->
   <include file="$(find-pkg-share topic_state_monitor)/launch/topic_state_monitor.launch.xml">
     <arg name="node_name_suffix" value="rough_no_ground"/>
-    <arg name="topic" value="/perception/obstacle_segmentation/single_frame/pointcloud_raw"/>
+    <arg name="topic" value="/perception/obstacle_segmentation/single_frame/pointcloud"/>
     <arg name="topic_type" value="sensor_msgs/msg/PointCloud2"/>
     <arg name="best_effort" value="true"/>
     <arg name="diag_name" value="sensing_topic_status"/>

aip_x1_launch/launch/velodyne_node_container.launch.py

@@ -121,15 +121,23 @@ def create_parameter_dict(*args):
         )
     )
 
+    # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
+    if LaunchConfiguration("output_as_sensor_frame").perform(context):
+        ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")}
+    else:
+        ring_outlier_filter_parameters = {
+            "output_frame": ""
+        }  # keep the output frame as the input frame
     nodes.append(
         ComposableNode(
             package="pointcloud_preprocessor",
             plugin="pointcloud_preprocessor::RingOutlierFilterComponent",
             name="ring_outlier_filter",
             remappings=[
                 ("input", "rectified/pointcloud_ex"),
-                ("output", "pointcloud"),
+                ("output", "pointcloud_before_sync"),
             ],
+            parameters=[ring_outlier_filter_parameters],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
     )
@@ -218,6 +226,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     )
     add_launch_arg("use_multithread", "False", "use multithread")
     add_launch_arg("use_intra_process", "False", "use ROS2 component container communication")
+    add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",

aip_x2_launch/config/blockage_diagnostics_param_file.yaml

@@ -4,8 +4,11 @@
     blockage_count_threshold: 50
     blockage_buffering_frames: 2
     blockage_buffering_interval: 1
+    enable_dust_diag: false
+    publish_debug_image: false
     dust_ratio_threshold: 0.2
     dust_count_threshold: 10
     dust_kernel_size: 2
     dust_buffering_frames: 10
     dust_buffering_interval: 1
+    blockage_kernel: 10

aip_x2_launch/config/gyro_bias_estimator.param.yaml

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    gyro_bias_threshold: 0.008 # [rad/s]
+    timer_callback_interval_sec: 0.5 # [sec]
+    diagnostics_updater_interval_sec: 0.5 # [sec]
+    straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]

aip_x2_launch/launch/imu.launch.xml

@@ -29,10 +29,12 @@
       <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
+    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share aip_x2_launch)/config/gyro_bias_estimator.param.yaml"/>
     <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)"/>
+      <arg name="gyro_bias_estimator_param_file" value="$(var gyro_bias_estimator_param_file)"/>
     </include>
   </group>
 

aip_x2_launch/launch/lidar.launch.xml

@@ -4,9 +4,9 @@
   <arg name="use_concat_filter" default="true" />
   <arg name="vehicle_id" default="$(env VEHICLE_ID default)" />
   <arg name="vehicle_mirror_param_file" />
-  <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
   <arg name="dual_return_filter_param_file" default="$(find-pkg-share aip_x2_launch)/config/dual_return_filter.param.yaml"/>
+  <arg name="enable_blockage_diag" default="true"/>
 
   <group>
     <push-ros-namespace namespace="lidar" />
@@ -26,6 +26,9 @@
         <arg name="return_mode" value="Strongest" />
         <arg name="min_azimuth_deg" value="135.0"/>
         <arg name="max_azimuth_deg" value="225.0"/>
+        <arg name="is_channel_order_top2down" value="true"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
@@ -46,6 +49,9 @@
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="40" />
         <arg name="return_mode" value="First" />
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.6"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
@@ -68,6 +74,9 @@
         <arg name="horizontal_ring_id" value="12" />
         <arg name="min_azimuth_deg" value="225.0"/>
         <arg name="max_azimuth_deg" value="315.0"/>
+        <arg name="is_channel_order_top2down" value="true"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="Dual" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -89,6 +98,9 @@
         <arg name="distance_range" value="[0.1, 7.5]"/>
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="54" />
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.6"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="First" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -112,6 +124,9 @@
         <arg name="horizontal_ring_id" value="12" />
         <arg name="min_azimuth_deg" value="45.0"/>
         <arg name="max_azimuth_deg" value="135.0"/>
+        <arg name="is_channel_order_top2down" value="true"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="Dual" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -133,6 +148,9 @@
         <arg name="distance_range" value="[0.1, 7.5]"/>
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="54" />
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.6"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="First" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -156,6 +174,9 @@
         <arg name="horizontal_ring_id" value="12" />
         <arg name="min_azimuth_deg" value="135.0"/>
         <arg name="max_azimuth_deg" value="225.0"/>
+        <arg name="is_channel_order_top2down" value="true"/>
+        <arg name="horizontal_resolution" value="0.4"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="Strongest" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -177,6 +198,9 @@
         <arg name="distance_range" value="[0.1, 7.5]"/>
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="40" />
+        <arg name="is_channel_order_top2down" value="false"/>
+        <arg name="horizontal_resolution" value="0.6"/>
+        <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
         <arg name="return_mode" value="First" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -189,7 +213,6 @@
       <arg name="base_frame" value="base_link" />
       <arg name="use_intra_process" value="true" />
       <arg name="use_multithread" value="true" />
-      <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
     </include>
 

aip_x2_launch/launch/pandar_node_container.launch.py

@@ -43,6 +43,10 @@ def get_pandar_monitor_info():
     return p
 
 
+def str2vector(string):
+    return [float(x) for x in string.strip("[]").split(",")]
+
+
 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
@@ -202,14 +206,22 @@ def create_parameter_dict(*args):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
+    if LaunchConfiguration("output_as_sensor_frame").perform(context):
+        ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")}
+    else:
+        ring_outlier_filter_parameters = {
+            "output_frame": ""
+        }  # keep the output frame as the input frame
     ring_outlier_filter_component = ComposableNode(
         package="pointcloud_preprocessor",
         plugin="pointcloud_preprocessor::RingOutlierFilterComponent",
         name="ring_outlier_filter",
         remappings=[
             ("input", "rectified/pointcloud_ex"),
-            ("output", "pointcloud"),
+            ("output", "pointcloud_before_sync"),
         ],
+        parameters=[ring_outlier_filter_parameters],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
@@ -219,7 +231,7 @@ def create_parameter_dict(*args):
         name="dual_return_filter",
         remappings=[
             ("input", "rectified/pointcloud_ex"),
-            ("output", "pointcloud"),
+            ("output", "pointcloud_before_sync"),
         ],
         parameters=[
             {
@@ -232,6 +244,7 @@ def create_parameter_dict(*args):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range")))
     blockage_diag_component = ComposableNode(
         package="pointcloud_preprocessor",
         plugin="pointcloud_preprocessor::BlockageDiagComponent",
@@ -245,7 +258,9 @@ def create_parameter_dict(*args):
                 "angle_range": LaunchConfiguration("angle_range"),
                 "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
                 "vertical_bins": LaunchConfiguration("vertical_bins"),
-                "model": LaunchConfiguration("model"),
+                "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
+                "max_distance_range": distance_range[1],
+                "horizontal_resolution": LaunchConfiguration("horizontal_resolution"),
             }
         ]
         + [load_composable_node_param("blockage_diagnostics_param_file")],
@@ -323,6 +338,7 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("input_frame", LaunchConfiguration("base_frame"))
     add_launch_arg("output_frame", LaunchConfiguration("base_frame"))
     add_launch_arg("dual_return_filter_param_file")
+    add_launch_arg("horizontal_resolution", "0.4")
     add_launch_arg(
         "blockage_diagnostics_param_file",
         [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"],
@@ -336,6 +352,7 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("min_azimuth_deg", "135.0")
     add_launch_arg("max_azimuth_deg", "225.0")
     add_launch_arg("enable_blockage_diag", "true")
+    add_launch_arg("output_as_sensor_frame", "True")
     set_container_executable = SetLaunchConfiguration(
         "container_executable",
         "component_container",