Merge pull request #182 from tier4/feat/imu_corrector/update_gyro_bia…

…s_algorithm feat(imu_corrector): update gyro bias algorithm
tier4 · Oct 19, 2023 · c0f921a · c0f921a
2 parents 696105b + 7c6de44
commit c0f921a
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 10 deletions.
diff --git 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_pose" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
+    </include>
   </group>
 
 </launch>
diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py
@@ -32,7 +32,7 @@ def launch_setup(context, *args, **kwargs):
         name="concatenate_data",
         remappings=[
             ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
-            ("output", "concatenated/pointcloud")
+            ("output", "concatenated/pointcloud"),
         ],
         parameters=[
             {

diff --git 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_pose" value="/localization/pose_estimator/pose_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
@@ -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_pose" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
     </include>
   </group>