ROS四元数与欧拉角的相互转换

tf2::Quaternion my_quat(

pose_msg.pose.orientation.x,

pose_msg.pose.orientation.y,

pose_msg.pose.orientation.z,

pose_msg.pose.orientation.w);

double my_roll, my_pitch, my_yaw;//定义存储r\p\y的容器

tf2::Matrix3x3 matrix(my_quat);

matrix.getRPY(my_roll, my_pitch, my_yaw);//进行转换

tf2::Quaternion q;

q.setRPY(target_roll, target_pitch, target_yaw);

geometry_msgs::Pose target_pose1;

target_pose1.orientation = tf2::toMsg(q);