[ros-users] tf and quaternions

[Raphael Favier]

Dear Wim, dear Stu,

I finally started to get something working using KDL and its rotation
objects. Thanks! 
Still, there are a few cases when the value of yaw pitch and roll cannot be
guessed.

Anyway, all I wanted to do was to get the Z rotation value of one observed
frame. If feels strange to have gone all the way with using KDL for that.
Here is the code (not optimized) I came up with. Do you see a more efficient
way to do this?

    //compute roll pitch yaw using KDL
    tf::Vector3 axis=quat.getAxis();
    float anglekd = quat.getAngle();
    KDL::Vector vectkd = KDL::Vector(axis.x(), axis.y(), axis.z());
    KDL::Rotation rot = KDL::Rotation();
    rot = rot.Rot(vectkd, anglekd);
    double rx, ry, rz;
    rot.GetEulerZYX(rz, ry, rx);
   
    //broadcast the Kinect frame
    tf::StampedTransform transform2;
    transform2.setRotation(tf::Quaternion(-rz , 0, -halfPI));
    transform2.setOrigin( tf::Vector3(1.0, 0.0 , 0.0 ));
    br.sendTransform(tf::StampedTransform(transform2, ros::Time::now(),
"world", "kinect_rgb"));


Thanks for your inputs and help!

Raph
-- 
View this message in context: http://ros-users.122217.n3.nabble.com/tf-and-quaternions-tp2206128p2234112.html
Sent from the ROS-Users mailing list archive at Nabble.com.