From 3013e905f8052bb84c3a07224112b47f4b582182 Mon Sep 17 00:00:00 2001
From: Tomohito ANDO <tomohito.ando@tier4.jp>
Date: Tue, 22 Oct 2024 13:59:37 +0900
Subject: [PATCH] feat(aip_x2_gen2_launch): add launcher for ARS548 to pass
 some missing arguments (#334)

* feat: add launcher for radar to pass some missing arguments

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* remove unuesed transform node

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* remove unused parameter

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* Revert "remove unuesed transform node"

This reverts commit 87b5351154c582911c4b0fcfda8eedf79242a47c.

* remove unnecessary comments

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

---------

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 aip_x2_gen2_launch/config/ARS548.param.yaml | 17 ++++++++++
 aip_x2_gen2_launch/launch/ars548.launch.xml | 23 +++++++++++++
 aip_x2_gen2_launch/launch/radar.launch.xml  | 37 ++++++++-------------
 3 files changed, 53 insertions(+), 24 deletions(-)
 create mode 100644 aip_x2_gen2_launch/config/ARS548.param.yaml
 create mode 100644 aip_x2_gen2_launch/launch/ars548.launch.xml

diff --git a/aip_x2_gen2_launch/config/ARS548.param.yaml b/aip_x2_gen2_launch/config/ARS548.param.yaml
new file mode 100644
index 00000000..1fe5e7e2
--- /dev/null
+++ b/aip_x2_gen2_launch/config/ARS548.param.yaml
@@ -0,0 +1,17 @@
+/**:
+  ros__parameters:
+    host_ip: 10.13.1.166
+    sensor_ip: 10.13.1.114
+    data_port: 42102
+    base_frame: base_link
+    object_frame: base_link
+    launch_hw: true
+    multicast_ip: 224.0.2.2
+    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
+    configuration_vehicle_height: 3.08
+    configuration_vehicle_wheelbase: 4.76012
diff --git a/aip_x2_gen2_launch/launch/ars548.launch.xml b/aip_x2_gen2_launch/launch/ars548.launch.xml
new file mode 100644
index 00000000..29c03ecb
--- /dev/null
+++ b/aip_x2_gen2_launch/launch/ars548.launch.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="sensor_model" default="ARS548"/>
+    <arg name="launch_hw" default="true" description="Whether to connect to a real sensor (true) or to accept packet messages (false)."/>
+    <arg name="config_file" default="$(find-pkg-share aip_x2_gen2_launch)/config/ARS548.param.yaml"/>
+    <arg name="frame_id"/>
+    <arg name="sensor_ip"/>
+    <arg name="odometry_topic"/>
+    <arg name="acceleration_topic"/>
+    <arg name="steering_angle_topic"/>
+
+    <node pkg="nebula_ros" exec="continental_ars548_ros_wrapper_node" name="nebula_continental_ars548" output="screen">
+        <param from="$(var config_file)" allow_substs="true"/>
+        <param name="launch_hw" value="$(var launch_hw)"/>
+        <param name="frame_id" value="$(var frame_id)"/>
+        <param name="sensor_ip" value="$(var sensor_ip)"/>
+        <remap from="odometry_input" to="$(var odometry_topic)"/>
+        <remap from="acceleration_input" to="$(var acceleration_topic)"/>
+        <remap from="steering_angle_input" to="$(var steering_angle_topic)"/>
+        <remap from="diagnostics" to="/diagnostics"/>
+    </node>
+
+</launch>
diff --git a/aip_x2_gen2_launch/launch/radar.launch.xml b/aip_x2_gen2_launch/launch/radar.launch.xml
index c530f2a5..5fc16689 100644
--- a/aip_x2_gen2_launch/launch/radar.launch.xml
+++ b/aip_x2_gen2_launch/launch/radar.launch.xml
@@ -9,17 +9,15 @@
 
   <group>
     <push-ros-namespace namespace="radar"/>
-
-    <!--node pkg="tf2_ros" exec="static_transform_publisher" name="tf_broadcaster" output="screen" args="$(var wheel_base) 0 0 0 0 0 base_link autosar"/-->
-    <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_auto_vehicle_msgs std_msgs --wait-for-start"/>
-    <!-- ros2 run topic_tools transform /vehicle/status/steering_status /vehicle/status/steering_status_scalar std_msgs/msg/Float64 'std_msgs.msg.Float64(data=m.steering_tire_angle)'  \-\-import autoware_auto_vehicle_msgs std_msgs -->
+    <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"/>
 
     <group>
       <push-ros-namespace namespace="front_center"/>
-      <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
+      <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="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.114"/>
         <arg name="frame_id" value="front_center/radar_link"/>
@@ -31,8 +29,6 @@
         <arg name="data_port" value="42102"/>
         <arg name="configuration_host_port" value="42401"/>
         <arg name="configuration_sensor_port" value="42101"/>
-
-        <arg name="new_plug_orientation" value="0"/>
       </include>
 
 
@@ -48,11 +44,11 @@
 
     <group>
       <push-ros-namespace namespace="front_left"/>
-      <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
-
+      <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="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.115"/>
         <arg name="frame_id" value="front_left/radar_link"/>
@@ -64,8 +60,6 @@
         <arg name="data_port" value="42102"/>
         <arg name="configuration_host_port" value="42401"/>
         <arg name="configuration_sensor_port" value="42101"/>
-
-        <arg name="new_plug_orientation" value="0"/>
       </include>
 
       <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
@@ -80,11 +74,11 @@
 
     <group>
       <push-ros-namespace namespace="front_right"/>
-      <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
-
+      <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="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.116"/>
         <arg name="frame_id" value="front_right/radar_link"/>
@@ -96,8 +90,6 @@
         <arg name="data_port" value="42102"/>
         <arg name="configuration_host_port" value="42401"/>
         <arg name="configuration_sensor_port" value="42101"/>
-
-        <arg name="new_plug_orientation" value="0"/>
       </include>
 
       <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
@@ -112,10 +104,11 @@
 
     <group>
       <push-ros-namespace namespace="rear_center"/>
-      <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
+      <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="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.117"/>
         <arg name="frame_id" value="rear_center/radar_link"/>
@@ -127,8 +120,6 @@
         <arg name="data_port" value="42102"/>
         <arg name="configuration_host_port" value="42401"/>
         <arg name="configuration_sensor_port" value="42101"/>
-
-        <arg name="new_plug_orientation" value="0"/>
       </include>
 
       <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
@@ -143,10 +134,11 @@
 
     <group>
       <push-ros-namespace namespace="rear_left"/>
-      <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
+      <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="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.118"/>
         <arg name="frame_id" value="rear_left/radar_link"/>
@@ -158,8 +150,6 @@
         <arg name="data_port" value="42102"/>
         <arg name="configuration_host_port" value="42401"/>
         <arg name="configuration_sensor_port" value="42101"/>
-
-        <arg name="new_plug_orientation" value="0"/>
       </include>
 
       <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
@@ -174,10 +164,11 @@
 
     <group>
       <push-ros-namespace namespace="rear_right"/>
-      <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.xml" if="$(var launch_driver)">
+      <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="launch_hw" value="$(var launch_driver)"/>
 
         <arg name="sensor_ip" value="10.13.1.119"/>
         <arg name="frame_id" value="rear_right/radar_link"/>
@@ -189,8 +180,6 @@
         <arg name="data_port" value="42102"/>
         <arg name="configuration_host_port" value="42401"/>
         <arg name="configuration_sensor_port" value="42101"/>
-
-        <arg name="new_plug_orientation" value="0"/>
       </include>
 
       <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">