From e62ce04f5c91756d334f29347ae16f165bb67651 Mon Sep 17 00:00:00 2001
From: 1222-takeshi <m.takeshi1995@gmail.com>
Date: Wed, 13 Dec 2023 15:38:33 +0900
Subject: [PATCH] feat: add use_awsim option and add gnss imu

Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>
---
 aip_x1_description/urdf/sensor_kit.xacro | 27 ++++++++++++++++++++++++
 aip_x1_launch/launch/imu.launch.xml      |  3 ++-
 aip_x1_launch/launch/lidar.launch.xml    |  7 +++---
 aip_x1_launch/launch/sensing.launch.xml  |  5 +++++
 4 files changed, 38 insertions(+), 4 deletions(-)

diff --git a/aip_x1_description/urdf/sensor_kit.xacro b/aip_x1_description/urdf/sensor_kit.xacro
index 5ab2f15c..649ba65f 100644
--- a/aip_x1_description/urdf/sensor_kit.xacro
+++ b/aip_x1_description/urdf/sensor_kit.xacro
@@ -2,6 +2,7 @@
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:macro name="sensor_kit_macro" params="parent x y z roll pitch yaw">
     <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
+    <xacro:include filename="$(find imu_description)/urdf/imu.xacro"/>
 
     <xacro:arg name="gpu" default="false"/>
     <xacro:arg name="config_dir" default="$(find aip_x1_description)/config"/>
@@ -67,5 +68,31 @@
       pitch="${calibration['sensor_kit_base_link']['livox_front_right_base_link']['pitch']}"
       yaw="${calibration['sensor_kit_base_link']['livox_front_right_base_link']['yaw']}"
     />
+    <!-- gnss -->
+    <xacro:imu_macro
+      name="gnss"
+      parent="sensor_kit_base_link"
+      namespace=""
+      x="${calibration['sensor_kit_base_link']['gnss_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['gnss_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['gnss_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['gnss_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['gnss_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['gnss_link']['yaw']}"
+      fps="100"
+    />
+    <!-- imu -->
+    <xacro:imu_macro
+      name="tamagawa/imu"
+      parent="sensor_kit_base_link"
+      namespace=""
+      x="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['tamagawa/imu_link']['yaw']}"
+      fps="100"
+    />
   </xacro:macro>
 </robot>
diff --git a/aip_x1_launch/launch/imu.launch.xml b/aip_x1_launch/launch/imu.launch.xml
index 0771709b..f047436b 100644
--- a/aip_x1_launch/launch/imu.launch.xml
+++ b/aip_x1_launch/launch/imu.launch.xml
@@ -1,12 +1,13 @@
 <launch>
 
   <arg name="launch_driver" default="true" />
+  <arg name="input_topic" default="/sensing/lidar/front_center/livox/imu" />
 
   <group>
     <push-ros-namespace namespace="imu"/>
 
     <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
-      <arg name="input_topic" value="/sensing/lidar/front_center/livox/imu" />
+      <arg name="input_topic" value="$(var input_topic)" />
       <arg name="output_topic" value="/sensing/imu/imu_data" />
       <arg name="param_file" value="$(find-pkg-share individual_params)/config/$(env VEHICLE_ID default)/aip_x1/imu_corrector.param.yaml" />
     </include>
diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml
index cb00eab5..47521eeb 100644
--- a/aip_x1_launch/launch/lidar.launch.xml
+++ b/aip_x1_launch/launch/lidar.launch.xml
@@ -6,6 +6,7 @@
   <arg name="vehicle_mirror_param_file" />
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="use_awsim" default="false" description="launch awsim"/>
 
   <!-- x1 additional setting -->
   <arg name="livox_min_range" default="1.5" />
@@ -27,7 +28,7 @@
       </include>
     </group>
 
-    <group>
+    <group unless="$(var use_awsim)">
       <group>
 	      <push-ros-namespace namespace="front_right"/>
         <include file="$(find-pkg-share aip_x1_launch)/launch/new_livox_horizon.launch.py">
@@ -54,7 +55,7 @@
       </group>
     </group>
 
-    <include file="$(find-pkg-share aip_x1_launch)/launch/pointcloud_preprocessor.launch.py">
+    <include file="$(find-pkg-share aip_x1_launch)/launch/pointcloud_preprocessor.launch.py" unless="$(var use_awsim)">
       <arg name="base_frame" value="base_link" />
       <arg name="use_intra_process" value="true" />
       <arg name="use_multithread" value="false" />
@@ -62,6 +63,6 @@
       <arg name="container_name" value="$(var pointcloud_container_name)"/>
     </include>
 
-    <include file="$(find-pkg-share aip_x1_launch)/launch/topic_state_monitor.launch.py" />
+    <include file="$(find-pkg-share aip_x1_launch)/launch/topic_state_monitor.launch.py" unless="$(var use_awsim)" />
   </group>
 </launch>
diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml
index a8586c40..e1dc2417 100644
--- a/aip_x1_launch/launch/sensing.launch.xml
+++ b/aip_x1_launch/launch/sensing.launch.xml
@@ -4,6 +4,9 @@
   <arg name="vehicle_mirror_param_file" description="path to the file of vehicle mirror position yaml"/>
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name="use_awsim" default="false" description="launch awsim"/>
+  <let name="input_topic" value="tamagawa/imu_raw" if ="$(var use_awsim)"/>
+  <let name="input_topic" value="/sensing/lidar/front_center/livox/imu" unless ="$(var use_awsim)"/>
 
   <group>
 
@@ -13,6 +16,7 @@
       <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+      <arg name="use_awsim" value="$(var use_awsim)"/>
     </include>
 
     <!-- Camera Driver -->
@@ -23,6 +27,7 @@
     <!-- IMU Driver -->
     <include file="$(find-pkg-share aip_x1_launch)/launch/imu.launch.xml">
       <arg name="launch_driver" value="$(var launch_driver)" />
+      <arg name="input_topic" value="$(var input_topic)" />
     </include>
 
     <!-- GNSS Driver -->