tf_logger.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <tf/transform_listener.h>
00003 #include <string>
00004 #include <iostream>
00005 #include <fstream>
00006 #include <cmath>
00007 
00008 using namespace std;
00009 
00010 /*const string baseLinkFrame = "/base_link";
00011 const string odomFrame = "/odom";
00012 const string kinectFrame = "/openni_rgb_optical_frame";
00013 const string worldFrame = "/world";
00014 */
00015 
00016 // see http://www.ros.org/wiki/Clock for how to manage timing 
00017 /*
00018         Execute the following to use this program:
00019                 roscore
00020                 rosparam set /use_sim_time true
00021                 rosbag play ../data/2011-01-13-13-56-40.bag --clock
00022                 bin/logger
00023 */
00024 
00025 ostream& operator<< (ostream& os, const tf::Quaternion& quat)
00026 {
00027         os << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w();
00028         return os;
00029 }
00030 
00031 ostream& operator<< (ostream& os, const tf::Vector3& trans)
00032 {
00033         os << trans.x() << " " << trans.y() << " " << trans.z();
00034         return os;
00035 }
00036 
00037 int main(int argc, char** argv)
00038 {
00039         ros::init(argc, argv, "tf_logger");
00040 
00041         ros::NodeHandle node("~");
00042         
00043         string baseLinkFrame;
00044         node.param<string>("baseLinkFrame", baseLinkFrame, "/base_link");
00045         string odomFrame;
00046         node.param<string>("odomFrame", odomFrame, "/odom");
00047         string kinectFrame;
00048         node.param<string>("kinectFrame", kinectFrame, "/openni_rgb_optical_frame");
00049         string worldFrame;
00050         node.param<string>("worldFrame", worldFrame, "/world");
00051         string outputFileName;
00052         node.param<string>("outputFileName", outputFileName, "output.txt");
00053         cout << baseLinkFrame << " " << odomFrame << " " << kinectFrame << " " <<
00054                         worldFrame << " " << outputFileName << endl;
00055 
00056         tf::TransformListener t(ros::Duration(20));
00057         tf::StampedTransform tr_o, tr_i;
00058         
00059         ROS_INFO_STREAM("waiting for initial transforms");
00060         while (node.ok())
00061         {
00062                 ros::Time now(ros::Time::now());
00063                 //ROS_INFO_STREAM(now);
00064                 if (t.waitForTransform(baseLinkFrame, now, baseLinkFrame, now, odomFrame, ros::Duration(0.1)))
00065                         break;
00066                 //ROS_INFO("wait");
00067                 //ros::Duration(0.1).sleep();
00068         }
00069         ROS_INFO_STREAM("got first odom to baseLink");
00070         while (node.ok())
00071         {
00072                 ros::Time now(ros::Time::now());
00073                 //ROS_INFO_STREAM(now);
00074                 if (t.waitForTransform(kinectFrame, now, kinectFrame, now, worldFrame, ros::Duration(0.1)))
00075                         break;
00076                 //ROS_INFO("wait");
00077                 //ros::Duration(0.1).sleep();
00078         }
00079         ROS_INFO_STREAM("got first world to kinect");
00080         
00081         sleep(3);
00082         
00083         ros::Rate rate(0.5);
00084         ofstream ofs(outputFileName.c_str());
00085         while (node.ok())
00086         {
00087                 // sleep
00088                 ros::spinOnce();
00089                 rate.sleep();
00090                 
00091                 // get parameters from transforms
00092                 ros::Time curTime(ros::Time::now());
00093                 ros::Time lastTime = curTime - ros::Duration(2);
00094                 //ROS_INFO_STREAM("curTime: " << curTime << ", lastTime: " << lastTime);
00095                 if (!t.waitForTransform(baseLinkFrame, curTime, baseLinkFrame, lastTime, odomFrame, ros::Duration(3)))
00096                         break;
00097                 if (!t.waitForTransform(kinectFrame, curTime, kinectFrame, lastTime, worldFrame, ros::Duration(3)))
00098                         break;
00099                 ofs << curTime << " ";
00100                 t.lookupTransform(baseLinkFrame, curTime, baseLinkFrame, lastTime, odomFrame, tr_o);
00101                 ofs << tr_o.getOrigin() << " " << tr_o.getRotation() << " ";
00102                 t.lookupTransform(kinectFrame, curTime, kinectFrame, lastTime, worldFrame, tr_i);
00103                 ofs << tr_i.getOrigin() << " " << tr_i.getRotation() << endl;
00104         }
00105         
00106         return 0;
00107 }


ethzasl_extrinsic_calibration
Author(s): François Pomerleau, Francis Colas, and Stéphane Magnenat
autogenerated on Mon Oct 6 2014 10:28:40