Ask Your Question
0

Get RPY from tfListener

asked 2014-11-03 15:18:16 -0500

rcj1986 gravatar image

updated 2014-11-03 15:23:02 -0500

tfoote gravatar image

Hi, I want to get the RPY data from my transformation. I have the next code:

include <ros/ros.h>
include <tf/tf.h>
include <tf/transform_listener.h>
include <geometry_msgs/TransformStamped.h>

int main(int argc, char **argv) {

    ros::init(argc, argv, "tf_listener");
    ros::NodeHandle nh;
    ROS_INFO_STREAM("Started node tf_listener.");

    std::string target_frame, source_frame;
    tf::TransformListener tf_listener;
    tf::StampedTransform transform;
    ros::Rate rate(1.); 

    if (argc < 3) {
        return -1;
    }
    else {
        target_frame = argv[1];
        source_frame = argv[2];
    }    

    while (ros::ok()) {
        try {
            tf_listener.lookupTransform(target_frame, source_frame, ros::Time(), transform);

            geometry_msgs::Transform buffer;
            tf::transformTFToMsg(transform, buffer);
            ROS_INFO_STREAM("Transform from " << target_frame << " to " << source_frame << ": " << std::endl << buffer);
            ROS_INFO_STREAM("Distance between transforms: " << transform.getOrigin().length() << " meters.");
        }
        catch (...) {
            ROS_WARN_STREAM("Waiting for transform from " << target_frame << " to " << source_frame);
        }
        rate.sleep();
    }
    return 0;
}

How can I get it??

Thanks.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2014-11-03 15:30:50 -0500

tfoote gravatar image

Please use the block quote method for code blocks. I've edited your question for you.

The tf::Transforms has a getBasis method to get the tf::Matrix3x3 datatype. On which there are lots of RPY methods including getRPY

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-11-03 15:18:16 -0500

Seen: 301 times

Last updated: Nov 03 '14