From 2a06672f99c9086a324f44119ec2640154f6f4dc Mon Sep 17 00:00:00 2001
From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Date: Mon, 26 Aug 2024 17:37:34 +0900
Subject: [PATCH] fix: correct imu orientation (#45)

---
 .../imu_corrector/src/imu_corrector_core.cpp         | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp
index edc82f6d..5c84d470 100644
--- a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp
+++ b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp
@@ -42,6 +42,17 @@ geometry_msgs::msg::Vector3 transformVector3(
   return vec_stamped_transformed.vector;
 }
 
+geometry_msgs::msg::Quaternion transformQuaternion(
+  const geometry_msgs::msg::Quaternion & quat, const geometry_msgs::msg::TransformStamped & transform)
+{
+  geometry_msgs::msg::QuaternionStamped quat_stamped;
+  quat_stamped.quaternion = quat;
+
+  geometry_msgs::msg::QuaternionStamped quat_stamped_transformed;
+  tf2::doTransform(quat_stamped, quat_stamped_transformed, transform);
+  return quat_stamped_transformed.quaternion;
+}
+
 namespace imu_corrector
 {
 ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & node_options)
@@ -103,6 +114,7 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
   sensor_msgs::msg::Imu imu_msg_base_link;
   imu_msg_base_link.header.stamp = imu_msg_ptr->header.stamp;
   imu_msg_base_link.header.frame_id = output_frame_;
+  imu_msg_base_link.orientation = transformQuaternion(imu_msg.orientation, *tf_imu2base_ptr); 
   imu_msg_base_link.linear_acceleration =
     transformVector3(imu_msg.linear_acceleration, *tf_imu2base_ptr);
   imu_msg_base_link.linear_acceleration_covariance =