#include <tf2/transform_datatypes.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf/tf.h>
void publishOdom() {
tf::Quaternion q;
double roll, pitch, yaw;
q.setRPY(roll, pitch, yaw);
odom_msg.pose.pose.orientation.x = q.x();
odom_msg.pose.pose.orientation.y = q.y();
odom_msg.pose.pose.orientation.z = q.z();
odom_msg.pose.pose.orientation.w = q.w();
tf::Quaternion q2(odom_msg.pose.pose.orientation.x,
odom_msg.pose.pose.orientation.y,
odom_msg.pose.pose.orientation.z,
odom_msg.pose.pose.orientation.w);
tf::Matrix3x3 m2(q2);
m2.getRPY(roll, pitch, yaw);
}
ROS 四元数和欧拉角互转例子
于 2021-03-02 11:15:42 首次发布