[aerial_robot_estimation][imu] bug fix for singularity pose: yaw angl…

…e from groundtruth
jsk-ros-pkg · Jul 18, 2024 · 778b509 · 778b509
1 parent 07f9bc7
commit 778b509
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/aerial_robot_estimation/src/sensor/imu.cpp b/aerial_robot_estimation/src/sensor/imu.cpp
@@ -161,7 +161,7 @@ namespace sensor_plugin
     // 2. experiment estimate mode
     if(estimator_->getStateStatus(State::CoG::Rot, aerial_robot_estimation::GROUND_TRUTH))
       {
-        tf::Matrix3x3 gt_cog_rot = estimator_->getOrientation(Frame::COG, aerial_robot_estimation::GROUND_TRUTH);
+        tf::Matrix3x3 gt_cog_rot = estimator_->getOrientation(Frame::BASELINK, aerial_robot_estimation::GROUND_TRUTH) * cog2baselink_tf.getBasis().inverse();
         double r,p,y;
         gt_cog_rot.getRPY(r, p, y);
         cog_euler[2] = y;