From 6d66089d19019b8b399e7f6d9cbc01772adfbec8 Mon Sep 17 00:00:00 2001
From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com>
Date: Fri, 21 Jul 2023 18:58:47 +0900
Subject: [PATCH] fix(aip_x2_launcher): fix with ars408.launch.xml (#156)

* fix(aip_x2_launcher): fix with ars408.launch.xml

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix moving socket can to ars408.xml

Signed-off-by: scepter914 <scepter914@gmail.com>

* add simple_object_merger

Signed-off-by: scepter914 <scepter914@gmail.com>

* update simple_object_merger parameter

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix typo

Signed-off-by: scepter914 <scepter914@gmail.com>

---------

Signed-off-by: scepter914 <scepter914@gmail.com>
---
 aip_x2_launch/launch/ars408.launch.xml | 34 --------------------------
 aip_x2_launch/launch/radar.launch.xml  | 20 ++++++++++-----
 2 files changed, 14 insertions(+), 40 deletions(-)
 delete mode 100644 aip_x2_launch/launch/ars408.launch.xml

diff --git a/aip_x2_launch/launch/ars408.launch.xml b/aip_x2_launch/launch/ars408.launch.xml
deleted file mode 100644
index 30a6e7f1..00000000
--- a/aip_x2_launch/launch/ars408.launch.xml
+++ /dev/null
@@ -1,34 +0,0 @@
-<launch>
-  <arg name="launch_driver" default="true" />
-  <arg name="input/frame" default="from_can_bus" />
-  <arg name="output/objects" default="objects_raw" />
-  <arg name="output_frame" default="ars408_front_link" />
-  <arg name="interface" default="can0"/>
-  <arg name="receiver_interval_sec" default="0.01"/>
-
-  <group if="$(var launch_driver)">
-    <!-- socket can -->
-    <include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_receiver.launch.py">
-      <arg name="interface" value="$(var interface)"/>
-      <arg name="interval_sec" value="$(var receiver_interval_sec)"/>
-    </include>
-
-    <!-- ars408 driver-->
-    <include file="$(find-pkg-share pe_ars408_ros)/launch/continental_ars408.launch.xml">
-      <arg name="input/frame" value="$(var input/frame)" />
-      <arg name="output/objects" value="$(var output/objects)" />
-      <arg name="output_frame" value="$(var output_frame)" />
-      <arg name="launch_driver" value="$(var launch_driver)" />
-    </include>
-  </group>
-
-  <!-- *** TEMPORARY ***
-    * Launching for visualization purposes.
-    * This should be launched from perception. ex) radar_based_detection.launch.xml
-  -->
-  <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
-    <arg name="input/radar_objects" value="$(var output/objects)" />
-    <arg name="output/radar_detected_objects" value="debug/detected_objects"/>
-    <arg name="output/radar_tracked_objects" value="debug/tracked_objects"/>
-  </include>
-</launch>
diff --git a/aip_x2_launch/launch/radar.launch.xml b/aip_x2_launch/launch/radar.launch.xml
index 7677d74a..73e36ecf 100644
--- a/aip_x2_launch/launch/radar.launch.xml
+++ b/aip_x2_launch/launch/radar.launch.xml
@@ -7,7 +7,7 @@
 
     <group>
       <push-ros-namespace namespace="front_center" />
-      <include file="$(find-pkg-share aip_x2_launch)/launch/ars408.launch.xml">
+      <include file="$(find-pkg-share common_sensor_launch)/launch/ars408.launch.xml">
         <arg name="interface" value="pcan2"/>
         <arg name="output_frame" value="front_center/radar_link" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -16,7 +16,7 @@
 
     <group>
       <push-ros-namespace namespace="front_left" />
-      <include file="$(find-pkg-share aip_x2_launch)/launch/ars408.launch.xml">
+      <include file="$(find-pkg-share common_sensor_launch)/launch/ars408.launch.xml">
         <arg name="interface" value="pcan3"/>
         <arg name="output_frame" value="front_left/radar_link" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -25,7 +25,7 @@
 
     <group>
       <push-ros-namespace namespace="rear_left" />
-      <include file="$(find-pkg-share aip_x2_launch)/launch/ars408.launch.xml">
+      <include file="$(find-pkg-share common_sensor_launch)/launch/ars408.launch.xml">
         <arg name="interface" value="pcan4"/>
         <arg name="output_frame" value="rear_left/radar_link" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -34,7 +34,7 @@
 
     <group>
       <push-ros-namespace namespace="rear_center" />
-      <include file="$(find-pkg-share aip_x2_launch)/launch/ars408.launch.xml">
+      <include file="$(find-pkg-share common_sensor_launch)/launch/ars408.launch.xml">
         <arg name="interface" value="pcan5"/>
         <arg name="output_frame" value="rear_center/radar_link" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -43,7 +43,7 @@
 
     <group>
       <push-ros-namespace namespace="rear_right" />
-      <include file="$(find-pkg-share aip_x2_launch)/launch/ars408.launch.xml">
+      <include file="$(find-pkg-share common_sensor_launch)/launch/ars408.launch.xml">
         <arg name="interface" value="pcan6"/>
         <arg name="output_frame" value="rear_right/radar_link" />
         <arg name="launch_driver" value="$(var launch_driver)" />
@@ -52,12 +52,20 @@
 
     <group>
       <push-ros-namespace namespace="front_right" />
-      <include file="$(find-pkg-share aip_x2_launch)/launch/ars408.launch.xml">
+      <include file="$(find-pkg-share common_sensor_launch)/launch/ars408.launch.xml">
         <arg name="interface" value="pcan7"/>
         <arg name="output_frame" value="front_right/radar_link" />
         <arg name="launch_driver" value="$(var launch_driver)" />
       </include>
     </group>
 
+    <include file="$(find-pkg-share simple_object_merger)/launch/simple_object_merger.launch.xml">
+      <arg name="output/objects" value="detected_objects"/>
+      <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]"/>
+    </include>
+
   </group>
 </launch>