00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <cstdio>
00031 #include "tf/transform_listener.h"
00032 #include "ros/ros.h"
00033
00034 class echoListener
00035 {
00036 public:
00037
00038 tf::TransformListener tf;
00039
00040
00041 echoListener()
00042 {
00043
00044 };
00045
00046 ~echoListener()
00047 {
00048
00049 };
00050
00051 private:
00052
00053 };
00054
00055
00056 int main(int argc, char ** argv)
00057 {
00058
00059 ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName);
00060
00061 if (argc != 3)
00062 {
00063 printf("Usage: tf_echo source_frame target_frame\n\n");
00064 printf("This will echo the transform from the coordinate frame of the source_frame\n");
00065 printf("to the coordinate frame of the target_frame. \n");
00066 printf("Note: This is the transform to get data from target_frame into the source_frame.\n");
00067 return -1;
00068 }
00069
00070 ros::NodeHandle nh;
00071
00072 echoListener echoListener;
00073
00074
00075 std::string source_frameid = std::string(argv[1]);
00076 std::string target_frameid = std::string(argv[2]);
00077
00078
00079 echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0));
00080
00081
00082
00083
00084 while(nh.ok())
00085 {
00086 try
00087 {
00088 tf::StampedTransform echo_transform;
00089 echoListener.tf.lookupTransform(source_frameid, target_frameid, ros::Time(), echo_transform);
00090 std::cout.precision(3);
00091 std::cout.setf(std::ios::fixed,std::ios::floatfield);
00092 std::cout << "At time " << echo_transform.stamp_.toSec() << std::endl;
00093 double yaw, pitch, roll;
00094 echo_transform.getBasis().getRPY(roll, pitch, yaw);
00095 tf::Quaternion q = echo_transform.getRotation();
00096 tf::Vector3 v = echo_transform.getOrigin();
00097 std::cout << "- Translation: [" << v.getX() << ", " << v.getY() << ", " << v.getZ() << "]" << std::endl;
00098 std::cout << "- Rotation: in Quaternion [" << q.getX() << ", " << q.getY() << ", "
00099 << q.getZ() << ", " << q.getW() << "]" << std::endl
00100 << " in RPY [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl;
00101
00102
00103 }
00104 catch(tf::TransformException& ex)
00105 {
00106 std::cout << "Failure at "<< ros::Time::now() << std::endl;
00107 std::cout << "Exception thrown:" << ex.what()<< std::endl;
00108 std::cout << "The current list of frames is:" <<std::endl;
00109 std::cout << echoListener.tf.allFramesAsString()<<std::endl;
00110
00111 }
00112 sleep(1);
00113 }
00114
00115 return 0;
00116 };
00117