From ce3a4313a57150af7d96a8c35b1c6953157c1521 Mon Sep 17 00:00:00 2001
From: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
Date: Mon, 23 Oct 2023 10:14:48 +0900
Subject: [PATCH] Changed input topic of GyroBiasEstimator from pose to odom

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>
---
 .../launch/gyro_bias_estimator.launch.xml           |  4 ++--
 sensing/imu_corrector/src/gyro_bias_estimator.cpp   | 13 ++++++-------
 sensing/imu_corrector/src/gyro_bias_estimator.hpp   |  6 ++++--
 3 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml
index 0d7cba9faa3a6..e16ccef446aba 100644
--- a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml
+++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml
@@ -1,14 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <arg name="input_imu_raw" default="imu_raw"/>
-  <arg name="input_pose" default="pose"/>
+  <arg name="input_odom" default="odom"/>
   <arg name="output_gyro_bias" default="gyro_bias"/>
   <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share imu_corrector)/config/gyro_bias_estimator.param.yaml"/>
   <arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>
 
   <node pkg="imu_corrector" exec="gyro_bias_estimator" name="gyro_bias_estimator" output="screen">
     <remap from="~/input/imu_raw" to="$(var input_imu_raw)"/>
-    <remap from="~/input/pose" to="$(var input_pose)"/>
+    <remap from="~/input/odom" to="$(var input_odom)"/>
     <remap from="~/output/gyro_bias" to="$(var output_gyro_bias)"/>
     <param from="$(var gyro_bias_estimator_param_file)"/>
     <param from="$(var imu_corrector_param_file)"/>
diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp
index 21bb51dc5f1f1..50e4a702ec949 100644
--- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp
+++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp
@@ -43,9 +43,9 @@ GyroBiasEstimator::GyroBiasEstimator()
   imu_sub_ = create_subscription<Imu>(
     "~/input/imu_raw", rclcpp::SensorDataQoS(),
     [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); });
-  pose_sub_ = create_subscription<PoseWithCovarianceStamped>(
-    "~/input/pose", rclcpp::SensorDataQoS(),
-    [this](const PoseWithCovarianceStamped::ConstSharedPtr msg) { callback_pose(msg); });
+  odom_sub_ = create_subscription<Odometry>(
+    "~/input/odom", rclcpp::SensorDataQoS(),
+    [this](const Odometry::ConstSharedPtr msg) { callback_odom(msg); });
   gyro_bias_pub_ = create_publisher<Vector3Stamped>("~/output/gyro_bias", rclcpp::SensorDataQoS());
 
   auto timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this);
@@ -88,12 +88,11 @@ void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr)
   }
 }
 
-void GyroBiasEstimator::callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr)
+void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr)
 {
-  // push pose_msg to queue
   geometry_msgs::msg::PoseStamped pose;
-  pose.header = pose_msg_ptr->header;
-  pose.pose = pose_msg_ptr->pose.pose;
+  pose.header = odom_msg_ptr->header;
+  pose.pose = odom_msg_ptr->pose.pose;
   pose_buf_.push_back(pose);
 }
 
diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp
index 7eb06bcdcb365..821cba0b213ff 100644
--- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp
+++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp
@@ -22,6 +22,7 @@
 
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/vector3_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 
 #include <memory>
@@ -38,6 +39,7 @@ class GyroBiasEstimator : public rclcpp::Node
   using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
   using Vector3Stamped = geometry_msgs::msg::Vector3Stamped;
   using Vector3 = geometry_msgs::msg::Vector3;
+  using Odometry = nav_msgs::msg::Odometry;
 
 public:
   GyroBiasEstimator();
@@ -45,7 +47,7 @@ class GyroBiasEstimator : public rclcpp::Node
 private:
   void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
   void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr);
-  void callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr);
+  void callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr);
   void timer_callback();
 
   static geometry_msgs::msg::Vector3 transform_vector3(
@@ -55,7 +57,7 @@ class GyroBiasEstimator : public rclcpp::Node
   const std::string output_frame_ = "base_link";
 
   rclcpp::Subscription<Imu>::SharedPtr imu_sub_;
-  rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr pose_sub_;
+  rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
   rclcpp::Publisher<Vector3Stamped>::SharedPtr gyro_bias_pub_;
   rclcpp::TimerBase::SharedPtr timer_;