From 52cec8737c2bef5faacbf788f7595d0a11eb1e3e Mon Sep 17 00:00:00 2001
From: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>
Date: Tue, 4 Feb 2025 11:34:12 +0900
Subject: [PATCH] feat: throttled the radar inputs (#371)

feat: throttled the radar inputs following indications from the documentation and switched to sensor time

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
---
 aip_x2_gen2_launch/config/ARS548.param.yaml |  6 +--
 aip_x2_gen2_launch/launch/radar.launch.xml  | 44 ++++++++++++---------
 2 files changed, 29 insertions(+), 21 deletions(-)

diff --git a/aip_x2_gen2_launch/config/ARS548.param.yaml b/aip_x2_gen2_launch/config/ARS548.param.yaml
index 1fe5e7e2..03943890 100644
--- a/aip_x2_gen2_launch/config/ARS548.param.yaml
+++ b/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
diff --git a/aip_x2_gen2_launch/launch/radar.launch.xml b/aip_x2_gen2_launch/launch/radar.launch.xml
index d793d8de..599076a9 100644
--- a/aip_x2_gen2_launch/launch/radar.launch.xml
+++ b/aip_x2_gen2_launch/launch/radar.launch.xml
@@ -7,6 +7,14 @@
   <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"/>
@@ -14,9 +22,9 @@
     <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"/>