From 47e6c27150ea0d87facfd0f49548196e42972d77 Mon Sep 17 00:00:00 2001 From: sugikazu75 Date: Fri, 18 Oct 2024 20:40:55 +0900 Subject: [PATCH] [aerial_robot_estimation] normalize quaternions before converting to message to suppress warning message --- aerial_robot_estimation/src/sensor/imu.cpp | 1 + aerial_robot_estimation/src/state_estimation.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/aerial_robot_estimation/src/sensor/imu.cpp b/aerial_robot_estimation/src/sensor/imu.cpp index 2e8fd9fb8..3de56673a 100644 --- a/aerial_robot_estimation/src/sensor/imu.cpp +++ b/aerial_robot_estimation/src/sensor/imu.cpp @@ -441,6 +441,7 @@ namespace sensor_plugin imu_data.header.stamp = imu_stamp_; tf::Quaternion q; base_rot_.at(0).getRotation(q); + q.normalize(); tf::quaternionTFToMsg(q, imu_data.orientation); tf::vector3TFToMsg(omega_, imu_data.angular_velocity); tf::vector3TFToMsg(acc_b_, imu_data.linear_acceleration); diff --git a/aerial_robot_estimation/src/state_estimation.cpp b/aerial_robot_estimation/src/state_estimation.cpp index 162e20dbf..bc89a7130 100644 --- a/aerial_robot_estimation/src/state_estimation.cpp +++ b/aerial_robot_estimation/src/state_estimation.cpp @@ -142,6 +142,7 @@ void StateEstimator::statePublish(const ros::TimerEvent & e) /* Baselink */ /* Rotation */ tf::Quaternion q; getOrientation(Frame::BASELINK, estimate_mode_).getRotation(q); + q.normalize(); tf::quaternionTFToMsg(q, odom_state.pose.pose.orientation); tf::vector3TFToMsg(getAngularVel(Frame::BASELINK, estimate_mode_), odom_state.twist.twist.angular); @@ -171,6 +172,7 @@ void StateEstimator::statePublish(const ros::TimerEvent & e) /* COG */ /* Rotation */ getOrientation(Frame::COG, estimate_mode_).getRotation(q); + q.normalize(); tf::quaternionTFToMsg(q, odom_state.pose.pose.orientation); tf::vector3TFToMsg(getAngularVel(Frame::COG, estimate_mode_), odom_state.twist.twist.angular); /* Translation */