ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
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. :-)