ROS中四元数转换欧拉角

ThinkOver33

已于 2024-01-27 21:48:00 修改

阅读量8.2k

点赞数 5

分类专栏： c++ ROS 文章标签： ROS C

于 2018-09-16 10:48:58 首次发布

本文链接：https://blog.csdn.net/daybreak222/article/details/82721423

版权

C++代码：

/****************************************************************************

Conversion from a quaternion to roll, pitch and yaw.

Nodes:
subscribed /rotation_quaternion (message of type geometry_msgs::Quaternion)
published /rpy_angles (message oftype geometry_msgs::Vector3.h)

****************************************************************************/

#include "ros/ros.h"
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"
#include "tf/transform_datatypes.h"
#include "LinearMath/btMatrix3x3.h"

// Here I use global publisher and subscriber, since I want to access the
// publisher in the function MsgCallback:
ros::Publisher rpy_publisher;
ros::Subscriber quat_subscriber;

// Function for conversion of quaternion to roll pitch and yaw. The angles
// are published here too.
void MsgCallback(const geometry_msgs::Quaternion msg)
{
    // the incoming g