[aerial_robot_estimation] normalize quaternions before converting to …

…message to suppress warning message
jsk-ros-pkg · Oct 18, 2024 · 47e6c27 · 47e6c27
1 parent c0d532b
commit 47e6c27
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 0 deletions.
diff --git 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
@@ -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 */