ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

I managed to find a workaround. I researched on other possible answers. I realized that there are two representations of the angles -- quaternion and euler.

What I inputted was expressed in euler -- RPY -- but then when we get the rotation, it returns a quaternion.

On the steps that I did, I did not use the constructor in tf::Quaternion even though it did accept parameters as yaw, pitch, roll. I tried that but the RPY keeps on changing.. Instead I used the member function of tf::Quaternion in setting the yaw, pitch, roll which totally worked.

So here are the steps:

 1. To broadcast RPY transformation in radians

    tf::Quaternion q;
    q.setRPY(roll,pitch,yaw);
    transform.setRotation(q);

  2. To read the RPY in radians

    tf.waitForTransform("turtle1", "carrot1", now, ros::Duration(3) );
    tf.lookupTransform("turtle1", "carrot1", now, transform_);
    transform_.getBasis().getRPY(roll,pitch,yaw);

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/LinearMath/Matrix3x3.h>

int main(int argc, char**argv){
    ros::init(argc, argv, "sample");
    ros::NodeHandle node;

    tf::TransformBroadcaster br;
    tf::StampedTransform transform;
    tf::StampedTransform transform_;
    tf::TransformListener tf;

    ros::Rate rate(10.0);
    int i = 0;
    double roll = 0, pitch = 1, yaw = 3.14/180*90;
    while(node.ok()){

        transform.setOrigin( tf::Vector3(0.0,0.0,0.0));
        tf::Quaternion q;
        q.setRPY(roll,pitch,yaw);
        transform.setRotation(q);
        ros::Time now = ros::Time::now();
        br.sendTransform(tf::StampedTransform(transform, now, "turtle1", "carrot1"));

        std::cerr << "One: " << roll << "," << pitch << "," << yaw << std::endl;

        tf.waitForTransform("turtle1", "carrot1", now, ros::Duration(3) );
        tf.lookupTransform("turtle1", "carrot1", now, transform_);

        transform_.getBasis().getRPY(roll,pitch,yaw);

        std::cerr << "Two: " << roll << "," << pitch << "," << yaw << std::endl;
        rate.sleep();
    }
    return 0;
}

Hope this helps other people too. :-)