四元素以及欧拉角

四元素以及欧拉角

c++头文件#include "tf/LinearMath/Matrix3x3.h"#include "geometry_msgs/Quaternion.h"#include "tf/transform_datatypes.h"欧拉角到四元素sensor_msgs::Imu imu_msgtf::Matrix3x3 rotation_matrix; rotation_matrix.setEulerYPR( nodes[i].gAngle * 3.1415926 / 180.0 * -1.0, 0., 0. ); tf::Quaternion quat; rotation_matrix.getRotation( quat ); imu_msg.orientation.x = quat.getX(); imu_msg.orientation.y = quat.getY(); imu_msg.orientation.z = quat.getZ(); imu_msg.orientation.w = quat.getW();四元素到欧拉角geometry_msgs::Quaternion orientation = imu_msg.orientation; tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)); double yaw, pitch, roll; mat.getEulerYPR(yaw, pitch, roll); ROS_INFO("The combined_odom yaw is %f ",yaw * 180 / 3.1415926); /*tf::Quaternion quat1; tf::quaternionMsgToTF(imu_msg.orientation, quat1); double roll, pitch, yaw; tf::Matrix3x3(quat1).getRPY(roll, pitch, yaw); ROS_INFO_STREAM("The yaw is " << yaw * 180.0 / 3.1415926 << " " << roll << " " << pitch); pythonfrom tf.transformations import euler_from_quaternionfrom geometry_msgs.msg import Quaternion, Twist, Pose, QuaternionStampedfrom nav_msgs.msg import Odometryquaternion = Quaternion()quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(self.th / 2.0) quaternion.w = cos(self.th / 2.0)odom = Odometry() odom.header.frame_id = "odom" odom.child_frame_id = self.base_frame #odom.child_frame_id = "baselink" odom.header.stamp = now odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = vxy odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth quaternion_1 = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w] (roll, pitch, yaw) = euler_from_quaternion(quaternion_1) rospy.loginfo("The wheel odom is " + str(yaw * 180 / 3.1415926))self.odomPub.publish(odom)

版权声明：本文内容由网络用户投稿，版权归原作者所有，本站不拥有其著作权，亦不承担相应法律责任。如果您发现本站中有涉嫌抄袭或描述失实的内容，请联系我们jiasou666@gmail.com 处理，核实后本网站将在24小时内删除侵权内容。