tf_logger.cpp
Go to the documentation of this file.
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 #include <ros/ros.h>
00030 #include <tf/transform_listener.h>
00031 #include <string>
00032 #include <iostream>
00033 #include <fstream>
00034 
00035 std::ostream& operator<<(std::ostream& os, const tf::Transform& transform)
00036 {
00037   using std::setw;
00038   tf::Vector3 origin = transform.getOrigin();
00039   tf::Quaternion quat = transform.getRotation();
00040   os << setw(10) << origin.x() << " " << setw(10) << origin.y() << " " << setw(10) << origin.z() << " ";
00041   os << setw(10) << quat.x() << " " << setw(10) << quat.y() << " " << setw(10) << quat.z() << " " << setw(10) << quat.w();
00042   return os;
00043 }
00044 
00045 void printHeader(std::ostream& os, const std::string& frame_id)
00046 {
00047   os << frame_id << ".x " << frame_id << ".y " << frame_id << ".z "
00048      << frame_id << ".qx " << frame_id << ".qy " << frame_id << ".qz " << frame_id << ".qw ";
00049 }
00050 
00051 struct FramePair
00052 {
00053   FramePair(const std::string& rframe, const std::string& lframe) :
00054     reference_frame(rframe), log_frame(lframe)
00055   {}
00056   std::string reference_frame;
00057   std::string log_frame;
00058 };
00059 
00060 int main(int argc, char** argv)
00061 {
00062   ros::init(argc, argv, "tf_logger");
00063 
00064   if (argc < 4 || argc % 2 != 0)
00065   {
00066     std::cerr << "Wrong number of arguments." << std::endl;
00067     std::cerr << "USAGE: " << argv[0] 
00068       << " FREQUENCY REF_FRAME LOG_FRAME [REF_FRAME LOG_FRAME ...]" << std::endl;
00069     std::cerr << "The transforms should be published at a rate >= 2*FREQUENCY " << std::endl;
00070     return 1;
00071   }
00072     
00073   float frequency = atof(argv[1]);
00074 
00075   std::vector<FramePair> frame_pairs;
00076 
00077   std::cout << "#timestamp ";
00078   for (int i = 2; i < argc; i+=2)
00079   {
00080     std::string ref_frame(argv[i]);
00081     std::string log_frame(argv[i+1]);
00082     printHeader(std::cout, log_frame);
00083     frame_pairs.push_back(FramePair(ref_frame, log_frame));
00084   }
00085   std::cout << std::endl;
00086   
00087   ros::Duration cache_time(10);
00088   tf::TransformListener tf_listener(cache_time);
00089   
00090   ros::Rate rate(frequency);
00091   while (ros::ok())
00092   {
00093     rate.sleep();
00094     if (!ros::Time::isValid()) continue; // wait for clock
00095     ros::Time requested_time(ros::Time::now() - ros::Duration(2 / frequency));
00096     std::vector<tf::StampedTransform> transforms(frame_pairs.size());
00097     try
00098     {
00099       // first look up all transforms, this could throw exceptions
00100       for (size_t i = 0; i < frame_pairs.size(); ++i)
00101       {
00102         ros::Duration timeout(2 / frequency);
00103         ros::Duration polling_sleep_duration(0.5 / frequency);
00104         tf_listener.waitForTransform(
00105             frame_pairs[i].reference_frame, 
00106             frame_pairs[i].log_frame, requested_time,
00107             timeout, polling_sleep_duration);
00108         tf::StampedTransform transform;
00109         tf_listener.lookupTransform(
00110             frame_pairs[i].reference_frame, 
00111             frame_pairs[i].log_frame, requested_time, transforms[i]);
00112       }
00113       // no exception was thrown, print out set
00114       std::cout << requested_time.toNSec() << " ";
00115       for (size_t i = 0; i < transforms.size(); ++i)
00116       {
00117         std::cout << transforms[i] << " ";
00118       }
00119       std::cout << std::endl;
00120     }
00121     catch (const tf::TransformException& e)
00122     {
00123       std::cerr << "TransformException caught: " << e.what() << std::endl;
00124     }
00125   }
00126   return 0;
00127 }
00128 


tf_tools
Author(s): Stephan Wirth , Miquel Massot
autogenerated on Mon Oct 6 2014 07:48:15