From 8ee46d4f3fd8cfaeec007be075f90bd1b857e3ee Mon Sep 17 00:00:00 2001
From: yoshiri <yoshiyoshidetteiu@gmail.com>
Date: Thu, 21 Mar 2024 13:06:05 +0900
Subject: [PATCH] feat: add radar feature

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 .../radar_simple_object_merger.param.yaml     |  6 ++
 aip_xx1_launch/launch/radar.launch.xml        | 73 +++++++++++++++++--
 2 files changed, 73 insertions(+), 6 deletions(-)
 create mode 100644 aip_xx1_launch/config/radar_simple_object_merger.param.yaml

diff --git a/aip_xx1_launch/config/radar_simple_object_merger.param.yaml b/aip_xx1_launch/config/radar_simple_object_merger.param.yaml
new file mode 100644
index 00000000..a817e0b7
--- /dev/null
+++ b/aip_xx1_launch/config/radar_simple_object_merger.param.yaml
@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    update_rate_hz: 20.0
+    new_frame_id: "base_link"
+    timeout_threshold: 1.0
+    input_topics: ["/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"]
diff --git a/aip_xx1_launch/launch/radar.launch.xml b/aip_xx1_launch/launch/radar.launch.xml
index 4005c41b..aba667d9 100644
--- a/aip_xx1_launch/launch/radar.launch.xml
+++ b/aip_xx1_launch/launch/radar.launch.xml
@@ -4,6 +4,7 @@
   <let name="odometry_topic" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
   <let name="acceleration_topic" value="/localization/acceleration"/>
   <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"/>
 
   <group>
     <push-ros-namespace namespace="radar"/>
@@ -14,7 +15,7 @@
 
     <group>
       <push-ros-namespace namespace="front_center"/>
-        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml">
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
           <arg name="odometry_topic" value="$(var odometry_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
@@ -33,11 +34,20 @@
 
           <arg name="new_plug_orientation" value="0"/>
       </include>
+        
+      <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+        <arg name="input/radar_objects" value="objects_raw"/>
+        <arg name="input/odometry" value="$(var odometry_topic)"/>
+        <arg name="output/radar_detected_objects" value="detected_objects"/>
+        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+        <arg name="use_twist_compensation" value="false"/>
+        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+      </include>
     </group>
 
     <group>
       <push-ros-namespace namespace="front_left"/>
-        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml">
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
           <arg name="odometry_topic" value="$(var odometry_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
@@ -56,11 +66,20 @@
 
           <arg name="new_plug_orientation" value="0"/>
       </include>
+
+      <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+        <arg name="input/radar_objects" value="objects_raw"/>
+        <arg name="input/odometry" value="$(var odometry_topic)"/>
+        <arg name="output/radar_detected_objects" value="detected_objects"/>
+        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+        <arg name="use_twist_compensation" value="false"/>
+        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+      </include>
     </group>
 
     <group>
       <push-ros-namespace namespace="front_right"/>
-        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml">
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
           <arg name="odometry_topic" value="$(var odometry_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
@@ -79,11 +98,20 @@
 
           <arg name="new_plug_orientation" value="0"/>
       </include>
+
+      <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+        <arg name="input/radar_objects" value="objects_raw"/>
+        <arg name="input/odometry" value="$(var odometry_topic)"/>
+        <arg name="output/radar_detected_objects" value="detected_objects"/>
+        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+        <arg name="use_twist_compensation" value="false"/>
+        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+      </include>      
     </group>
 
     <group>
       <push-ros-namespace namespace="rear_center"/>
-        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml">
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
           <arg name="odometry_topic" value="$(var odometry_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
@@ -102,11 +130,20 @@
 
           <arg name="new_plug_orientation" value="0"/>
       </include>
+
+      <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+        <arg name="input/radar_objects" value="objects_raw"/>
+        <arg name="input/odometry" value="$(var odometry_topic)"/>
+        <arg name="output/radar_detected_objects" value="detected_objects"/>
+        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+        <arg name="use_twist_compensation" value="false"/>
+        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+      </include>      
     </group>
 
     <group>
       <push-ros-namespace namespace="rear_left"/>
-        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml">
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
           <arg name="odometry_topic" value="$(var odometry_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
@@ -125,11 +162,20 @@
 
           <arg name="new_plug_orientation" value="0"/>
       </include>
+
+      <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+        <arg name="input/radar_objects" value="objects_raw"/>
+        <arg name="input/odometry" value="$(var odometry_topic)"/>
+        <arg name="output/radar_detected_objects" value="detected_objects"/>
+        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+        <arg name="use_twist_compensation" value="false"/>
+        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+      </include>      
     </group>
 
     <group>
       <push-ros-namespace namespace="rear_right"/>
-        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml">
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
 
           <arg name="odometry_topic" value="$(var odometry_topic)"/>
           <arg name="acceleration_topic" value="$(var acceleration_topic)"/>
@@ -148,6 +194,21 @@
 
           <arg name="new_plug_orientation" value="0"/>
       </include>
+
+      <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+        <arg name="input/radar_objects" value="objects_raw"/>
+        <arg name="input/odometry" value="$(var odometry_topic)"/>
+        <arg name="output/radar_detected_objects" value="detected_objects"/>
+        <arg name="output/radar_tracked_objects" value="tracked_objects"/>
+        <arg name="param_path" value="$(var radar_tracks_msgs_converter_param_path)"/>
+      </include>      
     </group>
+
+    <!-- merge radar objects -->
+    <let name="merger_param_path" value="$(find-pkg-share aip_xx1_launch)/config/radar_simple_object_merger.param.yaml"/>
+    <node pkg="simple_object_merger" exec="simple_object_merger_node" name="simple_object_merger" output="screen">  
+      <remap from="~/output/objects" to="detected_objects"/>
+      <param from="$(var merger_param_path)"/>
+    </node>
   </group>
 </launch>