Add orientation conversion passing from quaternion to euler to quaternion again

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]

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;