From 6bd6ad9d1447f4268b50cda47d83cf79b7831bef Mon Sep 17 00:00:00 2001
From: taikitanaka3 <taiki.tanaka@tier4.jp>
Date: Mon, 2 Sep 2024 01:06:56 +0900
Subject: [PATCH 1/4] feat: add sensor kit change for simulation

---
 .../launch/reference.launch.xml                   |  3 ++-
 .../config/awsim_sensor_kit_calibration.yaml      | 15 +++++++++++++++
 .../config/sensor_kit_calibration.yaml            |  6 +++---
 .../urdf/sensor_kit.xacro                         |  8 ++++++--
 .../launch/aichallenge_system.launch.xml          |  1 +
 5 files changed, 27 insertions(+), 6 deletions(-)
 create mode 100644 aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/awsim_sensor_kit_calibration.yaml

diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
index 5ab2d4b3..60373f6f 100644
--- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
@@ -6,6 +6,7 @@
   <arg name="map_path" default="/aichallenge/mapfile"/>
   <arg name="vehicle_model" default="racing_kart"  description="vehicle model name"/>
   <arg name="sensor_model" description="sensor model name"/>
+  <arg name="simulation" default="true"/>
 
   <!-- Optional parameters -->
   <!-- Map -->
@@ -29,7 +30,7 @@
     <arg name="model_file" default="$(find-pkg-share tier4_vehicle_launch)/urdf/vehicle.xacro" description="path to the file of model settings (*.xacro)"/>
     <arg name="config_dir" default="$(find-pkg-share racing_kart_sensor_kit_description)/config"/>
     <node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
-      <param name="robot_description" value="$(command 'xacro $(var model_file) vehicle_model:=$(var vehicle_model) sensor_model:=$(var sensor_model) config_dir:=$(var config_dir)' 'warn')"/>
+      <param name="robot_description" value="$(command 'xacro $(var model_file) vehicle_model:=$(var vehicle_model) sensor_model:=$(var sensor_model) config_dir:=$(var config_dir) simulation:=$(var simulation)' 'warn')"/>
     </node>
   </group>
 
diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/awsim_sensor_kit_calibration.yaml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/awsim_sensor_kit_calibration.yaml
new file mode 100644
index 00000000..1778c130
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/awsim_sensor_kit_calibration.yaml
@@ -0,0 +1,15 @@
+sensor_kit_base_link:
+  gnss_link:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
+  imu_link:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
index 1778c130..9951bfa3 100644
--- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
+++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
@@ -1,15 +1,15 @@
 sensor_kit_base_link:
   gnss_link:
-    x: 0.0
+    x: -0.26
     y: 0.0
     z: 0.0
     roll: 0.0
     pitch: 0.0
     yaw: 0.0
   imu_link:
-    x: 0.0
+    x: 0.85
     y: 0.0
     z: 0.0
     roll: 0.0
     pitch: 0.0
-    yaw: 0.0
+    yaw: -1.5707963268
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/urdf/sensor_kit.xacro b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/urdf/sensor_kit.xacro
index 4f2ca4fa..97df9526 100644
--- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/urdf/sensor_kit.xacro
+++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/urdf/sensor_kit.xacro
@@ -5,6 +5,7 @@
 
     <xacro:arg name="gpu" default="false"/>
     <xacro:arg name="config_dir" default="$(find racing_kart_sensor_kit_description)/config"/>
+    <xacro:arg name="simulation" default="false"/>
 
     <xacro:property name="sensor_kit_base_link" default="sensor_kit_base_link"/>
 
@@ -18,8 +19,11 @@
     </link>
 
     <!-- sensor -->
-    <xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/sensor_kit_calibration.yaml')}"/>
-
+    <xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/sensor_kit_calibration.yaml')}"/>   
+    <!-- over write calibration file for simulation -->
+    <xacro:if value="$(arg simulation)">
+      <xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/awsim_sensor_kit_calibration.yaml')}"/>    
+    </xacro:if>
     <!-- gnss -->
     <xacro:imu_macro
       name="gnss"
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml
index d68f1bac..65b63fd7 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml
@@ -13,6 +13,7 @@
         <arg name="sensor_model" value="$(var sensor_model)"/>
         <arg name="use_sim_time" value="$(var use_sim_time)"/>
         <arg name="launch_vehicle_interface" value="$(var launch_vehicle_interface)"/>
+        <arg name="simulation" value="$(var simulation)"/>
     </include>
     
     <!-- Workaround because the simulator cannot use the service. -->

From 1ff246d93a23c95d208de8c4cc3099819055f306 Mon Sep 17 00:00:00 2001
From: taikitanaka3 <taiki.tanaka@tier4.jp>
Date: Mon, 2 Sep 2024 01:08:30 +0900
Subject: [PATCH 2/4] chore: add ignore and pre-commit fix

---
 aichallenge/workspace/.gitignore                                | 1 +
 .../config/sensor_kit_calibration.yaml                          | 2 +-
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/aichallenge/workspace/.gitignore b/aichallenge/workspace/.gitignore
index 4c288e3e..d0703e7b 100644
--- a/aichallenge/workspace/.gitignore
+++ b/aichallenge/workspace/.gitignore
@@ -1,3 +1,4 @@
 /build
 /install
 /log
+/depends
diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
index 9951bfa3..f57e1627 100644
--- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
+++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
@@ -12,4 +12,4 @@ sensor_kit_base_link:
     z: 0.0
     roll: 0.0
     pitch: 0.0
-    yaw: -1.5707963268
\ No newline at end of file
+    yaw: -1.5707963268

From e9e57d4d6bc7c3a43b35bfc1b8051ab27d12287a Mon Sep 17 00:00:00 2001
From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Date: Wed, 4 Sep 2024 15:56:31 +0900
Subject: [PATCH 3/4] chore: aichallenge/workspace/.gitignore

Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
---
 aichallenge/workspace/.gitignore | 1 -
 1 file changed, 1 deletion(-)

diff --git a/aichallenge/workspace/.gitignore b/aichallenge/workspace/.gitignore
index d0703e7b..4c288e3e 100644
--- a/aichallenge/workspace/.gitignore
+++ b/aichallenge/workspace/.gitignore
@@ -1,4 +1,3 @@
 /build
 /install
 /log
-/depends

From 5486eea848e69c4b3926ddff917397757c6eba55 Mon Sep 17 00:00:00 2001
From: taikitanaka3 <taiki.tanaka@tier4.jp>
Date: Wed, 4 Sep 2024 16:09:20 +0900
Subject: [PATCH 4/4] fix: add simulation arg

---
 .../launch/aichallenge_submit.launch.xml                        | 2 ++
 .../aichallenge_submit_launch/launch/reference.launch.xml       | 2 +-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml
index 7626f4b7..f713745d 100644
--- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml
@@ -4,6 +4,7 @@
   <arg name="use_sim_time" default="true"/>
   <arg name="launch_vehicle_interface" default="false"/>
   <arg name="sensor_model" default="racing_kart_sensor_kit"/>
+  <arg name="simulation" default="true"/>
   <log message="echo launch param use_sim_time: $(var use_sim_time) launch_vehicle_interface: $(var launch_vehicle_interface) sensor_model: $(var sensor_model)"/>
 
   <include file="$(find-pkg-share aichallenge_submit_launch)/launch/reference.launch.xml" >
@@ -11,6 +12,7 @@
     <arg name="sensor_model" value="$(var sensor_model)"/>
     <arg name="map_path" value="$(find-pkg-share aichallenge_submit_launch)/map"/>
     <arg name="use_sim_time" value="$(var use_sim_time)"/>
+    <arg name="simulation" value="$(var simulation)"/>
     <arg name="launch_vehicle_interface" value="$(var launch_vehicle_interface)"/>
   </include>
 
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
index 60373f6f..13fd54cc 100644
--- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
@@ -6,7 +6,7 @@
   <arg name="map_path" default="/aichallenge/mapfile"/>
   <arg name="vehicle_model" default="racing_kart"  description="vehicle model name"/>
   <arg name="sensor_model" description="sensor model name"/>
-  <arg name="simulation" default="true"/>
+  <arg name="simulation" description="used for sensor kit param"/>
 
   <!-- Optional parameters -->
   <!-- Map -->