From c714228111dc628e41ba501838f15471a08f8e16 Mon Sep 17 00:00:00 2001
From: j4tfwm6z <j4tfwm6z@tier4.jp>
Date: Mon, 11 Mar 2024 14:26:33 +0900
Subject: [PATCH] feat: added the radars to the aip launcher and description

Signed-off-by: j4tfwm6z <j4tfwm6z@tier4.jp>
---
 .../config/sensors_calibration.yaml           |  12 +-
 aip_xx1_description/urdf/sensors.xacro        |   4 +-
 aip_xx1_launch/launch/radar.launch.xml        | 142 ++++++++++++++++--
 3 files changed, 141 insertions(+), 17 deletions(-)

diff --git a/aip_xx1_description/config/sensors_calibration.yaml b/aip_xx1_description/config/sensors_calibration.yaml
index 9d4bbed6..a57d3ea9 100644
--- a/aip_xx1_description/config/sensors_calibration.yaml
+++ b/aip_xx1_description/config/sensors_calibration.yaml
@@ -27,42 +27,42 @@ base_link:
   #   roll: -0.02
   #   pitch: 0.7281317
   #   yaw: 3.141592
-  ars548_front_center_base_link:
+  front_center/radar_link:
     x: 3.520 # Design Value
     y: 0.0 # Design Value
     z: 0.6352
     roll: 0.0 # Design Value
     pitch: 0.0 # Design Value
     yaw: 0.0 # Design Value
-  ars548_front_right_base_link:
+  front_right/radar_link:
     x: 3.384 # Design Value
     y: -0.7775 # Design Value
     z: 0.410
     roll: 0.0 # Design Value
     pitch: 0.0 # Design Value
     yaw: -1.22173 # Design Value
-  ars548_front_left_base_link:
+  front_left/radar_link:
     x: 3.384 # Design Value
     y: 0.7775 # Design Value
     z: 0.410
     roll: 0.0 # Design Value
     pitch: 0.0 # Design Value
     yaw: 1.22173 # Design Value
-  ars548_back_center_base_link:
+  rear_center/radar_link:
     x: -0.858 # Design Value
     y: 0.0 # Design Value
     z: 0.520
     roll: 0.0 # Design Value
     pitch: 0.0 # Design Value
     yaw: 3.141592 # Design Value
-  ars548_back_right_base_link:
+  rear_right/radar_link:
     x: -0.782 # Design Value
     y: -0.761 # Design Value
     z: 0.520
     roll: 0.0 # Design Value
     pitch: 0.0 # Design Value
     yaw: -2.0944 # Design Value
-  ars548_back_left_base_link:
+  rear_left/radar_link:
     x: -0.782 # Design Value
     y: 0.761 # Design Value
     z: 0.520
diff --git a/aip_xx1_description/urdf/sensors.xacro b/aip_xx1_description/urdf/sensors.xacro
index b92bb19b..f6899f14 100644
--- a/aip_xx1_description/urdf/sensors.xacro
+++ b/aip_xx1_description/urdf/sensors.xacro
@@ -57,7 +57,7 @@
   />
 
   <!-- embedded radar sensors -->
-  <!-- <xacro:include filename="$(find radar_description)/urdf/radar.xacro"/>
+  <xacro:include filename="$(find radar_description)/urdf/radar.xacro"/>
   <xacro:radar_macro
     name="radar_front_center"
     parent="base_link"
@@ -117,5 +117,5 @@
     roll="${calibration['base_link']['rear_left/radar_link']['roll']}"
     pitch="${calibration['base_link']['rear_left/radar_link']['pitch']}"
     yaw="${calibration['base_link']['rear_left/radar_link']['yaw']}"
-  /> -->
+  />
 </robot>
diff --git a/aip_xx1_launch/launch/radar.launch.xml b/aip_xx1_launch/launch/radar.launch.xml
index 44d24722..6b842d4f 100644
--- a/aip_xx1_launch/launch/radar.launch.xml
+++ b/aip_xx1_launch/launch/radar.launch.xml
@@ -1,23 +1,147 @@
 <launch>
-  <arg name="launch_driver" default="true" />
-  <arg name="input/frame" default="from_can_bus" />
-  <arg name="radar_can_device" default="canRadar" />
+  <arg name="launch_driver" default="true" description="do launch driver"/>
+
+  <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"/>
 
   <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 -->
+
     <group>
       <push-ros-namespace namespace="front_center"/>
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.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="sensor_ip" value="10.13.1.114"/>
+          <arg name="frame_id" value="front_center/radar_link"/>
+
+          <arg name="sensor_model" value="ARS548"/>
+          <arg name="host_ip" value="10.13.1.166"/>
+          <arg name="multicast_ip" value="224.0.2.2"/>
+          <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>
+    </group>
+
+    <group>
+      <push-ros-namespace namespace="front_left"/>
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.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)"/>
 
-      <include file="$(find-pkg-share common_sensor_launch)/launch/ars408.launch.xml">
-        <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="interface" value="$(var radar_can_device)" />
-        <arg name="input/frame" value="$(var input/frame)" />
-        <arg name="output_frame" value="ars408_front_center" />
+          <arg name="sensor_ip" value="10.13.1.115"/>
+          <arg name="frame_id" value="front_left/radar_link"/>
+
+          <arg name="sensor_model" value="ARS548"/>
+          <arg name="host_ip" value="10.13.1.166"/>
+          <arg name="multicast_ip" value="224.0.2.2"/>
+          <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>
     </group>
 
-    <node pkg="topic_tools" exec="relay" name="radar_relay" output="log" args="front_center/detected_objects detected_objects"/>
+    <group>
+      <push-ros-namespace namespace="front_right"/>
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.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="sensor_ip" value="10.13.1.116"/>
+          <arg name="frame_id" value="front_right/radar_link"/>
+
+          <arg name="sensor_model" value="ARS548"/>
+          <arg name="host_ip" value="10.13.1.166"/>
+          <arg name="multicast_ip" value="224.0.2.2"/>
+          <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>
+    </group>
+
+    <group>
+      <push-ros-namespace namespace="rear_center"/>
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.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="sensor_ip" value="10.13.1.117"/>
+          <arg name="frame_id" value="rear_center/radar_link"/>
+
+          <arg name="sensor_model" value="ARS548"/>
+          <arg name="host_ip" value="10.13.1.166"/>
+          <arg name="multicast_ip" value="224.0.2.2"/>
+          <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>
+    </group>
+
+    <group>
+      <push-ros-namespace namespace="rear_left"/>
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.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="sensor_ip" value="10.13.1.118"/>
+          <arg name="frame_id" value="rear_left/radar_link"/>
+
+          <arg name="sensor_model" value="ARS548"/>
+          <arg name="host_ip" value="10.13.1.166"/>
+          <arg name="multicast_ip" value="224.0.2.2"/>
+          <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>
+    </group>
+
+    <group>
+      <push-ros-namespace namespace="rear_right"/>
+        <include file="$(find-pkg-share nebula_ros)/launch/continental_launch_all_hw.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="sensor_ip" value="10.13.1.119"/>
+          <arg name="frame_id" value="rear_right/radar_link"/>
+
+          <arg name="sensor_model" value="ARS548"/>
+          <arg name="host_ip" value="10.13.1.166"/>
+          <arg name="multicast_ip" value="224.0.2.2"/>
+          <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>
+    </group>
   </group>
 </launch>