ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Hey, here is a c++ solution to your problem :) just in case you still have use for it! Needed the transformation from quaternion to rpy-angles too so here is what I put together. Used the code fragment from here for the actual transformation.
/****************************************************************************
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 geometry_msgs::Quaternion is transformed to a tf::Quaterion
tf::Quaternion quat;
tf::quaternionMsgToTF(msg, quat);
// the tf::Quaternion has a method to acess roll pitch and yaw
double roll, pitch, yaw;
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
// the found angles are written in a geometry_msgs::Vector3
geometry_msgs::Vector3 rpy;
rpy.x = roll;
rpy.y = pitch;
rpy.z = yaw;
// this Vector is then published:
rpy_publisher.publish(rpy);
ROS_INFO("published rpy angles: roll=%f pitch=%f yaw=%f", rpy.x, rpy.y, rpy.z);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker");
ros::NodeHandle n;
rpy_publisher = n.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);
quat_subscriber = n.subscribe("rotation_quaternion", 1000, MsgCallback);
// check for incoming quaternions untill ctrl+c is pressed
ROS_INFO("waiting for quaternion");
ros::spin();
return 0;
}