Quaternion to Roll, Pitch, Yaw

edit

Hey, I am attempting to modify a source code (part of it given below) It currently just publishes the Quaternion but I also wish to display RPY. I referred to Rotation Methods and tried the getRPY method. But that doesn't work for me. I believe, since I am new to this (the C++ code, I have a very basic understanding) so I am not sure how to modify the code and publish the RPY values. Can anyone guide me or if this has been previously attempted before then I would appreciate it if someone could post the link. Thank you!

Code - Get values

void VRPN_CALLBACK track_target (void *, const vrpn_TRACKERCB t)
{
btQuaternion q_orig(t.quat[0], t.quat[1], t.quat[2], t.quat[3]);
btQuaternion q_fix(0.70710678, 0., 0., 0.70710678);
btQuaternion q_rot = q_fix * q_orig * q_fix.inverse();

btVector3 axis = q_rot.getAxis();
btVector3 pos(t.pos[0], -t.pos[2], t.pos[1]);

// verifying that each callback indeed gives fresh data.
if ( prev_vrpn_data.quat[0] == t.quat[0] and \
     prev_vrpn_data.quat[1] == t.quat[1] and \
     prev_vrpn_data.quat[2] == t.quat[2] and \
     prev_vrpn_data.quat[3] == t.quat[3] and \
     prev_vrpn_data.pos[0] == t.pos[0] and \
     prev_vrpn_data.pos[1] == t.pos[1] and \
     prev_vrpn_data.pos[2] == t.pos[2] )
    ROS_WARN("Repeated Values");

prev_vrpn_data = t;

target_state->target.transform.translation.x = pos.x();
target_state->target.transform.translation.y = pos.z();
target_state->target.transform.translation.z = -pos.y();

target_state->target.transform.rotation.x = q_rot.x();
target_state->target.transform.rotation.y = q_rot.y();
target_state->target.transform.rotation.z = q_rot.z();
target_state->target.transform.rotation.w = q_rot.w();

target_state->target.header.frame_id = "optitrak";
target_state->target.child_frame_id = frame_id;
target_state->target.header.stamp = ros::Time::now();

fresh_data = true;
}

Code which should Publish the values

int main(int argc, char* argv[])
{
ros::init(argc, argv, "vrpn_tracked_object_1");
ros::NodeHandle nh("~");
target_state = new TargetState;
frame_id = nh.getNamespace();
std::string vrpn_server_ip;
int vrpn_port;
std::string tracked_object_name;
nh.param<std::string>("vrpn_server_ip", vrpn_server_ip, std::string());
nh.param<int>("vrpn_port", vrpn_port, 3883);

std::cout<<"vrpn_server_ip:"<<vrpn_server_ip<<std::endl;
std::cout<<"vrpn_port:"<<vrpn_port<<std::endl;

Rigid_Body tool(nh, vrpn_server_ip, vrpn_port);

ros::Rate loop_rate(1000);

while(ros::ok())
{
    tool.step_vrpn();
    if (fresh_data == true)
    { // only publish when receive data over VRPN.
        tool.publish_target_state(target_state);
        fresh_data = false;
    }
    loop_rate.sleep();
}
return 0;
}

I hope posting this much code is not an issue. If anyone could give an insight/explanation into how to obtain and publish the RPY values, I would appreciate it. (Plus it would also help me understand the code much better) Thank you!

Edit: My incorrect solution

//The following was added to int main()

btQuaternion q(target_state->target.transform.rotation.x, target_state->target.transform.rotation.y, target_state->target.transform.rotation.z, target_state->target.transform.rotation.w);
double roll, pitch, yaw;
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

//The following was added to the while(ros::ok()) loop inside int main()

std::cout << "Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;

The above does give me certain values, but they are incorrect It gives me ... (more)