From 80363d9d97a57d098d420392f6e2b73d07ec6f62 Mon Sep 17 00:00:00 2001
From: add <cneves@idmind.pt>
Date: Wed, 27 Apr 2022 16:11:39 +0100
Subject: [PATCH] Add orientation conversion passing from quaternion to euler
to quaternion again
---
include/imu_msg_fix_alg_node.h | 3 +++
src/imu_msg_fix_alg_node.cpp | 24 +++++++++++++++++++++++-
2 files changed, 26 insertions(+), 1 deletion(-)
diff --git a/include/imu_msg_fix_alg_node.h b/include/imu_msg_fix_alg_node.h
index 9a3b16b..adadbfe 100755
--- a/include/imu_msg_fix_alg_node.h
+++ b/include/imu_msg_fix_alg_node.h
@@ -32,6 +32,9 @@
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/Imu.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
// [service client headers]
// [action server client headers]
diff --git a/src/imu_msg_fix_alg_node.cpp b/src/imu_msg_fix_alg_node.cpp
index de0786c..46f9f9b 100755
--- a/src/imu_msg_fix_alg_node.cpp
+++ b/src/imu_msg_fix_alg_node.cpp
@@ -105,10 +105,32 @@ void ImuMsgFixAlgNode::imu_in_callback(const sensor_msgs::Imu::ConstPtr& msg)
this->imu_out_Imu_msg_.linear_acceleration_covariance[3]=msg->linear_acceleration_covariance[1];
this->imu_out_Imu_msg_.linear_acceleration_covariance[8]=msg->linear_acceleration_covariance[8];
}
- this->imu_out_Imu_msg_.orientation.x=msg->orientation.y;
+
+ /*this->imu_out_Imu_msg_.orientation.x=msg->orientation.y;
this->imu_out_Imu_msg_.orientation.y=msg->orientation.x;
this->imu_out_Imu_msg_.orientation.z=-msg->orientation.z;
this->imu_out_Imu_msg_.orientation.w=-msg->orientation.w;
+ */
+
+ geometry_msgs::Quaternion quat_msg = msg->orientation;
+ double yaw;
+ double pitch;
+ double roll;
+ tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
+
+ //TODO: modify yaw, pitch or roll
+ double tmp_pitch = pitch;
+
+ pitch=roll;
+ roll=tmp_pitch;
+ yaw= -yaw;
+
+ tf2::Quaternion quat_tf;
+ quat_tf.setRPY(roll, pitch, yaw);
+ tf2::convert(quat_tf, quat_msg);
+
+ this->imu_out_Imu_msg_.orientation = quat_msg;
+
this->imu_out_Imu_msg_.angular_velocity.x=msg->angular_velocity.y;
this->imu_out_Imu_msg_.angular_velocity.y=msg->angular_velocity.x;
--
GitLab