ROS的nav_msgs里四元数和Yaw角的转换

ROS的nav_msgs里四元数和Yaw角的转换

nav_msgs/Path.msg结构

std_msgs/Header header
geometry_msgs/PoseStamped[] poses

将以上结构展开如下：std_msgs/Header结构

uint32 seq
time stamp
string frame_id

geometry_msgs/PoseStamped[]结构:

std_msgs/Header header
geometry_msgs/Pose pose

geometry_msgs/Pose pose结构

# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w

Yaw角到Quaternion

 geometry_msgs::Quaternion geo_q = tf::createQuaternionMsgFromYaw(refLinePose_.yaw);

Quaternion到Yaw角

double yaw=tf::getYaw(i.pose.orientation); //

参考：（九）ROS在rviz中实时显示轨迹（nav_msgs/Path消息的使用）