From 6ae8172ff2482b947b40d4ef08eee330a1e8d0e2 Mon Sep 17 00:00:00 2001
From: kminoda <koji.minoda@tier4.jp>
Date: Fri, 25 Aug 2023 10:24:35 +0900
Subject: [PATCH] feat(imu_corrector): add gyro_bias_estimator

Signed-off-by: kminoda <koji.minoda@tier4.jp>
---
 aip_x1_launch/launch/imu.launch.xml  | 13 ++++++++++---
 aip_x2_launch/launch/imu.launch.xml  | 14 +++++++++++---
 aip_xx1_launch/launch/imu.launch.xml | 14 +++++++++++---
 3 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/aip_x1_launch/launch/imu.launch.xml b/aip_x1_launch/launch/imu.launch.xml
index 65adb040..424126f9 100644
--- a/aip_x1_launch/launch/imu.launch.xml
+++ b/aip_x1_launch/launch/imu.launch.xml
@@ -6,12 +6,19 @@
   <group>
     <push-ros-namespace namespace="imu"/>
 
+    <arg name="imu_raw_name" default="/sensing/lidar/front_center/livox/imu"/>
+    <arg name="imu_corrector_param_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x1/imu_corrector.param.yaml"/>
     <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="output_topic" value="/sensing/imu/imu_data" />
-      <arg name="param_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x1/imu_corrector.param.yaml" />
+      <arg name="input_topic" value="$(var imu_raw_name)"/>
+      <arg name="output_topic" value="imu_data"/>
+      <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
+    <include file="$(find-pkg-share imu_corrector)/launch/gyro_bias_estimator.launch.xml">
+      <arg name="input_imu_raw" value="$(var imu_raw_name)"/>
+      <arg name="input_twist" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
+    </include>
   </group>
 
 </launch>
diff --git a/aip_x2_launch/launch/imu.launch.xml b/aip_x2_launch/launch/imu.launch.xml
index dd408973..5dad00b4 100644
--- a/aip_x2_launch/launch/imu.launch.xml
+++ b/aip_x2_launch/launch/imu.launch.xml
@@ -21,10 +21,18 @@
       </node>
     </group>
 
+    <arg name="imu_raw_name" default="tamagawa/imu_raw"/>
+    <arg name="imu_corrector_param_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/imu_corrector.param.yaml"/>
     <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
-      <arg name="input_topic" value="tamagawa/imu_raw" />
-      <arg name="output_topic" value="imu_data" />
-      <arg name="param_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/imu_corrector.param.yaml" />
+      <arg name="input_topic" value="$(var imu_raw_name)"/>
+      <arg name="output_topic" value="imu_data"/>
+      <arg name="param_file" value="$(var imu_corrector_param_file)"/>
+    </include>
+
+    <include file="$(find-pkg-share imu_corrector)/launch/gyro_bias_estimator.launch.xml">
+      <arg name="input_imu_raw" value="$(var imu_raw_name)"/>
+      <arg name="input_twist" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
     </include>
   </group>
 
diff --git a/aip_xx1_launch/launch/imu.launch.xml b/aip_xx1_launch/launch/imu.launch.xml
index 72f70ad0..2d4a52b5 100644
--- a/aip_xx1_launch/launch/imu.launch.xml
+++ b/aip_xx1_launch/launch/imu.launch.xml
@@ -15,10 +15,18 @@
       </node>
     </group>
 
+    <arg name="imu_raw_name" default="tamagawa/imu_raw"/>
+    <arg name="imu_corrector_param_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_xx1/imu_corrector.param.yaml"/>
     <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
-      <arg name="input_topic" value="tamagawa/imu_raw" />
-      <arg name="output_topic" value="imu_data" />
-      <arg name="param_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_xx1/imu_corrector.param.yaml" />
+      <arg name="input_topic" value="$(var imu_raw_name)"/>
+      <arg name="output_topic" value="imu_data"/>
+      <arg name="param_file" value="$(var imu_corrector_param_file)"/>
+    </include>
+
+    <include file="$(find-pkg-share imu_corrector)/launch/gyro_bias_estimator.launch.xml">
+      <arg name="input_imu_raw" value="$(var imu_raw_name)"/>
+      <arg name="input_twist" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
     </include>
   </group>