fix(aip_launcher_x2): use only front radar for radar faraway detection

aip_x1_launch/launch/gnss.launch.xml

@@ -2,7 +2,6 @@
 
   <arg name="use_gnss" default="false" />
   <arg name="launch_driver" default="true" />
-  <arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE" />
 
   <group if="$(var use_gnss)">
     <push-ros-namespace namespace="gnss"/>
@@ -22,7 +21,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_ublox_receiver" value="true" />
 
       <arg name="gnss_frame" value="gnss_link" />

aip_x1_launch/launch/imu.launch.xml

@@ -6,12 +6,19 @@
   <group>
     <push-ros-namespace namespace="imu"/>
 
+    <arg name="imu_raw_name" default="/sensing/lidar/front_center/livox/imu"/>
+    <arg name="imu_corrector_param_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x1/imu_corrector.param.yaml"/>
     <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
-      <arg name="input_topic" value="/sensing/lidar/front_center/livox/imu" />
-      <arg name="output_topic" value="/sensing/imu/imu_data" />
-      <arg name="param_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x1/imu_corrector.param.yaml" />
+      <arg name="input_topic" value="$(var imu_raw_name)"/>
+      <arg name="output_topic" value="imu_data"/>
+      <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_twist" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
+    </include>
   </group>
 
 </launch>

aip_x1_launch/launch/pointcloud_preprocessor.launch.py

@@ -30,7 +30,10 @@ def launch_setup(context, *args, **kwargs):
         package="pointcloud_preprocessor",
         plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
         name="concatenate_data",
-        remappings=[("output", "concatenated/pointcloud")],
+        remappings=[
+            ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
+            ("output", "concatenated/pointcloud"),
+        ],
         parameters=[
             {
                 "input_topics": [
@@ -41,6 +44,7 @@ def launch_setup(context, *args, **kwargs):
                 ],
                 "output_frame": LaunchConfiguration("base_frame"),
                 "timeout_sec": 1.0,
+                "input_twist_topic_type": "twist",
             }
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],

aip_x2_launch/config/blockage_diagnostics_param_file.yaml

@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    blockage_ratio_threshold: 0.1
+    blockage_count_threshold: 50
+    blockage_buffering_frames: 2
+    blockage_buffering_interval: 1
+    dust_ratio_threshold: 0.2
+    dust_count_threshold: 10
+    dust_kernel_size: 2
+    dust_buffering_frames: 10
+    dust_buffering_interval: 1

aip_x2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml

@@ -61,6 +61,12 @@
                       contains: [": pandar_ptp"]
                       timeout: 5.0
 
+                dust:
+                  type: diagnostic_aggregator/GenericAnalyzer
+                  path: dust
+                  contains: [": dust_validation"]
+                  timeout: 1.0
+
         gnss:
           type: diagnostic_aggregator/AnalyzerGroup
           path: gnss

aip_x2_launch/launch/gnss.launch.xml

@@ -1,7 +1,6 @@
 <launch>
 
   <arg name="launch_driver" default="true" />
-  <arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE" />
 
   <group>
     <push-ros-namespace namespace="gnss"/>
@@ -23,7 +22,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_ublox_receiver" value="true" />
 
       <arg name="gnss_frame" value="gnss_link" />

aip_x2_launch/launch/imu.launch.xml

@@ -21,10 +21,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)/aip_x2/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="output_topic" value="imu_data" />
-      <arg name="param_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/imu_corrector.param.yaml" />
+      <arg name="input_topic" value="$(var imu_raw_name)"/>
+      <arg name="output_topic" value="imu_data"/>
+      <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_twist" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
     </include>
   </group>
 

aip_x2_launch/launch/pandar_node_container.launch.py

@@ -28,16 +28,10 @@
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.actions import Node
 from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
 import yaml
 
 
-def get_dual_return_filter_info(context):
-    path = LaunchConfiguration("dual_return_filter_param_file").perform(context)
-    with open(path, "r") as f:
-        p = yaml.safe_load(f)["/**"]["ros__parameters"]
-    return p
-
-
 def get_pandar_monitor_info():
     path = os.path.join(
         get_package_share_directory("pandar_monitor"),
@@ -67,14 +61,11 @@ def get_vehicle_info(context):
     return p
 
 
-def get_vehicle_mirror_info(context):
-    path = LaunchConfiguration("vehicle_mirror_param_file").perform(context)
-    with open(path, "r") as f:
-        p = yaml.safe_load(f)["/**"]["ros__parameters"]
-    return p
-
-
 def launch_setup(context, *args, **kwargs):
+    def load_composable_node_param(param_path):
+        with open(LaunchConfiguration(param_path).perform(context), "r") as f:
+            return yaml.safe_load(f)["/**"]["ros__parameters"]
+
     def create_parameter_dict(*args):
         result = {}
         for x in args:
@@ -129,7 +120,6 @@ def create_parameter_dict(*args):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
-    dual_return_filter_info = get_dual_return_filter_info(context)
     cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
     cropbox_parameters["negative"] = True
 
@@ -153,7 +143,7 @@ def create_parameter_dict(*args):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
-    mirror_info = get_vehicle_mirror_info(context)
+    mirror_info = load_composable_node_param("vehicle_mirror_param_file")
     right = mirror_info["right"]
     cropbox_parameters.update(
         min_x=right["min_longitudinal_offset"],
@@ -238,7 +228,7 @@ def create_parameter_dict(*args):
                 "max_azimuth_deg": LaunchConfiguration("max_azimuth_deg"),
             }
         ]
-        + [dual_return_filter_info],
+        + [load_composable_node_param("dual_return_filter_param_file")],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
@@ -254,14 +244,11 @@ def create_parameter_dict(*args):
             {
                 "angle_range": LaunchConfiguration("angle_range"),
                 "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
-                "blockage_ratio_threshold": LaunchConfiguration("blockage_ratio_threshold"),
                 "vertical_bins": LaunchConfiguration("vertical_bins"),
                 "model": LaunchConfiguration("model"),
-                "blockage_count_threshold": LaunchConfiguration("blockage_count_threshold"),
-                "buffering_frames": LaunchConfiguration("buffering_frames"),
-                "buffering_interval": LaunchConfiguration("buffering_interval"),
             }
-        ],
+        ]
+        + [load_composable_node_param("blockage_diagnostics_param_file")],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
@@ -335,15 +322,15 @@ 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(
+        "blockage_diagnostics_param_file",
+        [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"],
+    )
     add_launch_arg("vehicle_mirror_param_file")
     add_launch_arg("use_multithread", "true")
     add_launch_arg("use_intra_process", "true")
     add_launch_arg("vertical_bins", "40")
-    add_launch_arg("blockage_ratio_threshold", "0.1")
     add_launch_arg("horizontal_ring_id", "12")
-    add_launch_arg("blockage_count_threshold", "50")
-    add_launch_arg("buffering_frames", "2")
-    add_launch_arg("buffering_interval", "1")
 
     add_launch_arg("min_azimuth_deg", "135.0")
     add_launch_arg("max_azimuth_deg", "225.0")

aip_x2_launch/launch/pointcloud_preprocessor.launch.py

@@ -50,6 +50,7 @@ def launch_setup(context, *args, **kwargs):
                 "input_offset": [0.025, 0.025, 0.01, 0.0, 0.05, 0.05, 0.05, 0.05],
                 "timeout_sec": 0.075,
                 "output_frame": LaunchConfiguration("base_frame"),
+                "input_twist_topic_type": "twist",
             }
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],

aip_x2_launch/launch/radar.launch.xml

@@ -64,7 +64,9 @@
       <arg name="update_rate_hz" value="20.0"/>
       <arg name="new_frame_id" value="base_link"/>
       <arg name="timeout_threshold" value="1.0"/>
-      <arg name="input_topics" value="[/sensing/radar/front_center/detected_objects, /sensing/radar/front_left/detected_objects, /sensing/radar/rear_left/detected_objects, /sensing/radar/rear_center/detected_objects, /sensing/radar/rear_right/detected_objects, /sensing/radar/front_right/detected_objects]"/>
+      <!-- To reduce calculation cost, use only front radar for radar detection -->
+      <!--<arg name="input_topics" value="[/sensing/radar/front_center/detected_objects, /sensing/radar/front_left/detected_objects, /sensing/radar/rear_left/detected_objects, /sensing/radar/rear_center/detected_objects, /sensing/radar/rear_right/detected_objects, /sensing/radar/front_right/detected_objects]"/>-->
+      <arg name="input_topics" value="[/sensing/radar/front_center/detected_objects, /sensing/radar/front_left/detected_objects, /sensing/radar/front_right/detected_objects]"/>
     </include>
 
   </group>

aip_xx1_launch/launch/gnss.launch.xml

@@ -2,7 +2,6 @@
 
   <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"/>
@@ -35,7 +34,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_ublox_receiver" value="true" />
 
       <arg name="gnss_frame" value="gnss_link" />

aip_xx1_launch/launch/imu.launch.xml

@@ -15,10 +15,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)/aip_xx1/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="output_topic" value="imu_data" />
-      <arg name="param_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_xx1/imu_corrector.param.yaml" />
+      <arg name="input_topic" value="$(var imu_raw_name)"/>
+      <arg name="output_topic" value="imu_data"/>
+      <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_twist" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
     </include>
   </group>
 

aip_xx1_launch/launch/lidar.launch.xml

@@ -16,7 +16,7 @@
       <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="sensor_ip" value="192.168.1.201"/>
         <arg name="host_ip" value="$(var host_ip)"/>
         <arg name="data_port" value="2368"/>
         <arg name="scan_phase" value="300.0"/>
@@ -32,7 +32,7 @@
       <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="sensor_ip" value="192.168.1.202"/>
         <arg name="host_ip" value="$(var host_ip)"/>
         <arg name="data_port" value="2369"/>
         <arg name="scan_phase" value="180.0"/>
@@ -50,7 +50,7 @@
       <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="sensor_ip" value="192.168.1.203"/>
         <arg name="host_ip" value="$(var host_ip)"/>
         <arg name="data_port" value="2370"/>
         <arg name="scan_phase" value="180.0"/>
@@ -68,7 +68,7 @@
       <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="sensor_ip" value="192.168.1.204"/>
         <arg name="host_ip" value="$(var host_ip)"/>
         <arg name="data_port" value="2371"/>
         <arg name="scan_phase" value="180.0"/>

aip_xx1_launch/launch/pointcloud_preprocessor.launch.py

@@ -51,6 +51,7 @@ def launch_setup(context, *args, **kwargs):
                     0.025,
                 ],  # each sensor will wait 60, 70, 70, 70ms
                 "timeout_sec": 0.095,  # set shorter than 100ms
+                "input_twist_topic_type": "twist",
             }
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],

common_sensor_launch/launch/hesai_XT32.launch.xml

@@ -6,7 +6,7 @@
   <arg name="model" default="PandarXT32"/>
   <arg name="sensor_frame" default="pandar"/>
   <arg name="return_mode" default="Strongest"/>
-  <arg name="device_ip" default="192.168.1.201"/>
+  <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"/>
@@ -22,7 +22,7 @@
     <arg name="sensor_model" value="$(var model)"/>
     <arg name="return_mode" value="$(var return_mode)"/>
     <arg name="frame_id" value="$(var sensor_frame)"/>
-    <arg name="device_ip" value="$(var device_ip)"/>
+    <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)"/>