From c64b6b24188d6ddd29ce89ab8cb53adbf7bd68b1 Mon Sep 17 00:00:00 2001
From: TaikiYamada4 <taiki.yamada@tier4.jp>
Date: Tue, 16 Apr 2024 13:49:56 +0900
Subject: [PATCH 1/4] Added gyro_bias_estimator_param_path to all
 imu.launch.xml

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
---
 aip_x1_launch/launch/imu.launch.xml  | 2 ++
 aip_x2_launch/launch/imu.launch.xml  | 2 ++
 aip_xx1_launch/launch/imu.launch.xml | 2 ++
 3 files changed, 6 insertions(+)

diff --git a/aip_x1_launch/launch/imu.launch.xml b/aip_x1_launch/launch/imu.launch.xml
index c192c5a1..33901bf3 100644
--- a/aip_x1_launch/launch/imu.launch.xml
+++ b/aip_x1_launch/launch/imu.launch.xml
@@ -14,10 +14,12 @@
       <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
+    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share common_sensor_launch)/config/gyro_bias_estimator.param.yaml"/>
     <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_odom" value="/localization/kinematic_state"/>
       <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
+      <arg name="gyro_bias_estimator_param_file" value="$(var gyro_bias_estimator_param_file)"/>
     </include>
   </group>
 
diff --git a/aip_x2_launch/launch/imu.launch.xml b/aip_x2_launch/launch/imu.launch.xml
index 432316cb..d79ebaa9 100644
--- a/aip_x2_launch/launch/imu.launch.xml
+++ b/aip_x2_launch/launch/imu.launch.xml
@@ -29,10 +29,12 @@
       <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
+    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share common_sensor_launch)/config/gyro_bias_estimator.param.yaml"/>
     <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_odom" value="/localization/kinematic_state"/>
       <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
+      <arg name="gyro_bias_estimator_param_file" value="$(var gyro_bias_estimator_param_file)"/>
     </include>
   </group>
 
diff --git a/aip_xx1_launch/launch/imu.launch.xml b/aip_xx1_launch/launch/imu.launch.xml
index b764d598..9d2d97d3 100644
--- a/aip_xx1_launch/launch/imu.launch.xml
+++ b/aip_xx1_launch/launch/imu.launch.xml
@@ -23,10 +23,12 @@
       <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
+    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share common_sensor_launch)/config/gyro_bias_estimator.param.yaml"/>
     <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_odom" value="/localization/kinematic_state"/>
       <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
+      <arg name="gyro_bias_estimator_param_file" value="$(var gyro_bias_estimator_param_file)"/>
     </include>
   </group>
 

From 92e00353c42352b8248090dc74885d1c13f1525a Mon Sep 17 00:00:00 2001
From: TaikiYamada4 <taiki.yamada@tier4.jp>
Date: Tue, 16 Apr 2024 15:20:39 +0900
Subject: [PATCH 2/4] Added gyro_bias_estimator.param.yaml to
 common_sensor_launch

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
---
 common_sensor_launch/config/gyro_bias_estimator.param.yaml | 6 ++++++
 1 file changed, 6 insertions(+)
 create mode 100644 common_sensor_launch/config/gyro_bias_estimator.param.yaml

diff --git a/common_sensor_launch/config/gyro_bias_estimator.param.yaml b/common_sensor_launch/config/gyro_bias_estimator.param.yaml
new file mode 100644
index 00000000..9aa5f442
--- /dev/null
+++ b/common_sensor_launch/config/gyro_bias_estimator.param.yaml
@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    gyro_bias_threshold: 0.013 # [rad/s]
+    timer_callback_interval_sec: 0.5 # [sec]
+    diagnostics_updater_interval_sec: 0.5 # [sec]
+    straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]

From b051eff3eb0bd467e1d580252147f4b35e1764f7 Mon Sep 17 00:00:00 2001
From: TaikiYamada4 <taiki.yamada@tier4.jp>
Date: Tue, 16 Apr 2024 15:35:12 +0900
Subject: [PATCH 3/4] Fixed value in gyro_bias_estimator.param.yaml

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
---
 common_sensor_launch/config/gyro_bias_estimator.param.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/common_sensor_launch/config/gyro_bias_estimator.param.yaml b/common_sensor_launch/config/gyro_bias_estimator.param.yaml
index 9aa5f442..10f19269 100644
--- a/common_sensor_launch/config/gyro_bias_estimator.param.yaml
+++ b/common_sensor_launch/config/gyro_bias_estimator.param.yaml
@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    gyro_bias_threshold: 0.013 # [rad/s]
+    gyro_bias_threshold: 0.003 # [rad/s]
     timer_callback_interval_sec: 0.5 # [sec]
     diagnostics_updater_interval_sec: 0.5 # [sec]
     straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]

From 402f41bb1c6eba362aa434e7308d1b957a838a6c Mon Sep 17 00:00:00 2001
From: TaikiYamada4 <taiki.yamada@tier4.jp>
Date: Tue, 16 Apr 2024 18:06:04 +0900
Subject: [PATCH 4/4] Distribute gyro_bias_estimator.param.yaml to each
 aip_x1_launch, aip_x2_launch and aip_xx1_launch.

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
---
 .../config/gyro_bias_estimator.param.yaml                   | 2 +-
 aip_x1_launch/launch/imu.launch.xml                         | 2 +-
 aip_x2_launch/config/gyro_bias_estimator.param.yaml         | 6 ++++++
 aip_x2_launch/launch/imu.launch.xml                         | 2 +-
 aip_xx1_launch/config/gyro_bias_estimator.param.yaml        | 6 ++++++
 aip_xx1_launch/launch/imu.launch.xml                        | 2 +-
 6 files changed, 16 insertions(+), 4 deletions(-)
 rename {common_sensor_launch => aip_x1_launch}/config/gyro_bias_estimator.param.yaml (81%)
 create mode 100644 aip_x2_launch/config/gyro_bias_estimator.param.yaml
 create mode 100644 aip_xx1_launch/config/gyro_bias_estimator.param.yaml

diff --git a/common_sensor_launch/config/gyro_bias_estimator.param.yaml b/aip_x1_launch/config/gyro_bias_estimator.param.yaml
similarity index 81%
rename from common_sensor_launch/config/gyro_bias_estimator.param.yaml
rename to aip_x1_launch/config/gyro_bias_estimator.param.yaml
index 10f19269..d552569f 100644
--- a/common_sensor_launch/config/gyro_bias_estimator.param.yaml
+++ b/aip_x1_launch/config/gyro_bias_estimator.param.yaml
@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    gyro_bias_threshold: 0.003 # [rad/s]
+    gyro_bias_threshold: 0.008 # [rad/s]
     timer_callback_interval_sec: 0.5 # [sec]
     diagnostics_updater_interval_sec: 0.5 # [sec]
     straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]
diff --git a/aip_x1_launch/launch/imu.launch.xml b/aip_x1_launch/launch/imu.launch.xml
index 33901bf3..bf00b7ca 100644
--- a/aip_x1_launch/launch/imu.launch.xml
+++ b/aip_x1_launch/launch/imu.launch.xml
@@ -14,7 +14,7 @@
       <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
-    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share common_sensor_launch)/config/gyro_bias_estimator.param.yaml"/>
+    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share aip_x1_launch)/config/gyro_bias_estimator.param.yaml"/>
     <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_odom" value="/localization/kinematic_state"/>
diff --git a/aip_x2_launch/config/gyro_bias_estimator.param.yaml b/aip_x2_launch/config/gyro_bias_estimator.param.yaml
new file mode 100644
index 00000000..d552569f
--- /dev/null
+++ b/aip_x2_launch/config/gyro_bias_estimator.param.yaml
@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    gyro_bias_threshold: 0.008 # [rad/s]
+    timer_callback_interval_sec: 0.5 # [sec]
+    diagnostics_updater_interval_sec: 0.5 # [sec]
+    straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]
diff --git a/aip_x2_launch/launch/imu.launch.xml b/aip_x2_launch/launch/imu.launch.xml
index d79ebaa9..e3f1a056 100644
--- a/aip_x2_launch/launch/imu.launch.xml
+++ b/aip_x2_launch/launch/imu.launch.xml
@@ -29,7 +29,7 @@
       <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
-    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share common_sensor_launch)/config/gyro_bias_estimator.param.yaml"/>
+    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share aip_x2_launch)/config/gyro_bias_estimator.param.yaml"/>
     <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_odom" value="/localization/kinematic_state"/>
diff --git a/aip_xx1_launch/config/gyro_bias_estimator.param.yaml b/aip_xx1_launch/config/gyro_bias_estimator.param.yaml
new file mode 100644
index 00000000..d552569f
--- /dev/null
+++ b/aip_xx1_launch/config/gyro_bias_estimator.param.yaml
@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    gyro_bias_threshold: 0.008 # [rad/s]
+    timer_callback_interval_sec: 0.5 # [sec]
+    diagnostics_updater_interval_sec: 0.5 # [sec]
+    straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]
diff --git a/aip_xx1_launch/launch/imu.launch.xml b/aip_xx1_launch/launch/imu.launch.xml
index 9d2d97d3..85b620d7 100644
--- a/aip_xx1_launch/launch/imu.launch.xml
+++ b/aip_xx1_launch/launch/imu.launch.xml
@@ -23,7 +23,7 @@
       <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
-    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share common_sensor_launch)/config/gyro_bias_estimator.param.yaml"/>
+    <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share aip_xx1_launch)/config/gyro_bias_estimator.param.yaml"/>
     <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_odom" value="/localization/kinematic_state"/>