Revision history [back]

Could you be more specific on what does not work? Since you say getRPY does not work for you I take it you get a compile error? Or does the function simply give false values? I see you got some btQuaternions there. First you have to turn this quaternion in a rotation matrix and then use the accessor getRPY on this matrix. Lets assume btQuaternion quat is the quaternion you want to get roll pitch and yaw from:

double roll, pitch, yaw;
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

Now all you have to do is write this information in a message of your choice and publish it. For some reason the btMatrix3x3 does not seem to have the desired member function, so I use tf::Matrix3x3. Make sure you included all the needed stuff:

#include "tf/transform_datatypes.h"
#include "LinearMath/btMatrix3x3.h"

For the Publishing: If you want to publish inside the VRPN_CALLBACK function you have to create your publisher globally. This is not the best method, but it will probably work fine for you:

geometry_msgs::Vector3 rpy;
rpy.x = roll;
rpy.y = pitch;
rpy.z = yaw;
rpy_publisher.publish(rpy);

and globally:

#include "geometry_msgs/Vector3.h"
ros::Publisher rpy_publisher;

and below your node handle:

rpy_publisher = nh.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);

hope I did not forget something :)

Could you be more specific on what does not work? Since you say getRPY does not work for you I take it you get a compile error? Or does the function simply give false values? I see you got some btQuaternions there. First you have to turn this quaternion in a rotation matrix and then use the accessor getRPY on this matrix. Lets assume btQuaternion quat is the quaternion you want to get roll pitch and yaw from:

double roll, pitch, yaw;
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

Now all you have to do is write this information in a message of your choice and publish it. For some reason the btMatrix3x3 does not seem to have the desired member function, so I use tf::Matrix3x3. Make sure you included all the needed stuff:

#include "tf/transform_datatypes.h"
#include "LinearMath/btMatrix3x3.h"

For the Publishing: If you want to publish inside the VRPN_CALLBACK function you have to create your publisher globally. This is not the best method, but it will probably work fine for you:

geometry_msgs::Vector3 rpy;
rpy.x = roll;
rpy.y = pitch;
rpy.z = yaw;
rpy_publisher.publish(rpy);

and globally:

#include "geometry_msgs/Vector3.h"
ros::Publisher rpy_publisher;

and below your node handle:

rpy_publisher = nh.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);

hope I did not forget something :)

Could you be more specific on what does not work? Since you say getRPY does not work for you I take it you get a compile error? Or does the function simply give false values? I see you got some btQuaternions there. First you have to turn this quaternion in a rotation matrix and then use the accessor getRPY on this matrix. Lets assume btQuaternion quat is the quaternion you want to get roll pitch and yaw from:

double roll, pitch, yaw;
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

Now all you have to do is write this information in a message of your choice and publish it. For some reason the btMatrix3x3 does not seem to have the desired member function, so I use tf::Matrix3x3. Make sure you included all the needed stuff:

#include "tf/transform_datatypes.h"
#include "LinearMath/btMatrix3x3.h"

For the Publishing: If you want to publish inside the VRPN_CALLBACK function you have to create your publisher globally. This is not the best method, but it will probably work fine for you:

geometry_msgs::Vector3 rpy;
rpy.x = roll;
rpy.y = pitch;
rpy.z = yaw;
rpy_publisher.publish(rpy);

and globally:

#include "geometry_msgs/Vector3.h"
ros::Publisher rpy_publisher;

and below your node handle:

rpy_publisher = nh.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);

hope I did not forget something :)

edit: Oh you got it but there are incorrect values. Sorry, but I think I can not help you there. Maybe you could check if the error is in some way related to pi/2 or the values are exchanged. Then you could adapt your coordinate system...