tfMessageReader.h
Go to the documentation of this file.
00001 #ifndef _TF_MESSAGE_READER_
00002 #define _TF_MESSAGE_READER_
00003 #include <ros/ros.h>
00004 #include <rosbag/bag.h>
00005 #include <rosbag/view.h>
00006 #include <boost/foreach.hpp>
00007 #include <tf/message_filter.h>
00008 #include <string>
00009 #include <vector>
00010 #include <pcl/ros/conversions.h>
00011 
00012 template<typename MessageType>
00013 class tfMessageReader
00014 {
00015         public:
00019                 tfMessageReader(std::string bagfilename, std::string message_topic_, std::string fixed_link_, std::string target_linkname_){
00020                         message_topic = message_topic_;
00021                         fixed_link = fixed_link_;
00022                         target_linkname = target_linkname_;
00023                         
00024                         fprintf(stderr,"Opening '%s'\n",bagfilename.c_str());
00025                         bag.open(bagfilename, rosbag::bagmode::Read);
00026                         
00027                         std::vector<std::string> topics;
00028                         topics.push_back("/tf");
00029                         topics.push_back(message_topic); 
00030                         
00031                         view = new rosbag::View(bag); //, rosbag::TopicQuery(topics));
00032                         I = view->begin();
00033                         Itf = view->begin();
00034                 }
00035                 
00036                 bool bagEnd(){
00037                         if(I == view->end()) return true;
00038                         return false;
00039                 }
00040                 
00044                 bool getTf(const std::string &frame_name,const ros::Time &stamp, tf::Transform &pose){
00045                         std::string schaiba;
00046                         schaiba.resize(500);
00047                         if(!transformer.canTransform(fixed_link, frame_name, stamp, &schaiba)){
00048                                 fprintf(stderr,"WTF:%s\n",schaiba.c_str());
00049                                 return false;
00050                         }
00051                         
00052                         tf::StampedTransform transform;
00053                         transformer.lookupTransform(fixed_link, frame_name, stamp,transform);
00054                         pose = transform;
00055                         return true;
00056                 }
00057                 
00058                 
00065                 bool getNextMessage(MessageType &msg_out,  tf::Transform &sensor_pose){
00066                         if(I == view->end()){
00067                                 fprintf(stderr,"End of measurement file Reached!!\n");
00068                                 return false;
00069                         }
00070                         
00071                         bool gotMessage = false;
00072                         ros::Time ts;
00073                         rosbag::MessageInstance const m = *I;
00074                         //sensor_msgs::LaserScan::ConstPtr scan = m.instantiate<sensor_msgs::LaserScan>();
00075                         typename MessageType::ConstPtr msg = m.instantiate<MessageType>();
00076                         
00077                         if(msg != NULL){
00078                                 if(m.getTopic() == message_topic){
00079                                         msg_out = (*msg);
00080                                         ts = msg_out.header.stamp;
00081                                         gotMessage = true;
00082                                         //fprintf(stderr,"Got Message (ts=%lf)\n",ts.toSec());
00083                                 }else{
00084                                         //fprintf(stderr,"Topic does not match!\n");
00085                                 }
00086                         }else{
00087                                 //fprintf(stderr,"msg NULL\n");
00088                         }
00089                         
00090                         readUntilTime(ts+ros::Duration(2.0));
00091                         
00092                         
00093                         if(gotMessage){ 
00094                                 std::string schaiba;
00095                                 schaiba.resize(200);
00096                                 if(!transformer.canTransform(fixed_link, target_linkname, ts, &schaiba)){
00097                                         fprintf(stderr,"%s\n",schaiba.c_str());
00098                                         I++;
00099                                         return false;
00100                                 }
00101                                 
00102                                 tf::StampedTransform transform;
00103                                 transformer.lookupTransform(fixed_link, target_linkname, ts,transform);
00104                                 sensor_pose = transform;
00105                                 I++;
00106                                 return true;
00107                         }
00108                         
00109                         I++;
00110                         return false;
00111                 }
00112                 
00113                 
00117                 bool readUntilTime(ros::Time t){
00118                         if(Itf == view->end()){
00119                                 return false;
00120                         }
00121                         if(last_read_tf>t){
00122                                 //fprintf(stderr,"No need to read!\n");
00123                                 return false; 
00124                         }
00125                         
00126                         while(last_read_tf <= t && Itf != view->end()){
00127                                 rosbag::MessageInstance const m = *Itf;
00128                                 //fprintf(stderr,"READING\n");
00130                                 tf::tfMessage::ConstPtr transform = m.instantiate<tf::tfMessage>();
00131                                 if (transform != NULL){
00132                                         if(m.getTopic() == "/tf"){
00133                                                 
00134                                                 if(transform->transforms.size()<=0) continue;
00135                                                 
00136                                                 for (unsigned int i = 0; i < transform->transforms.size(); i++){
00137                                                                 //std::cout << "frame_id : " << transform->transforms[i].header.frame_id;
00138                                                                 //std::cout << " cframe_id : " << transform->transforms[i].child_frame_id << std::endl;
00139                                                                 tf::StampedTransform stf;
00140                                                                 transformStampedMsgToTF(transform->transforms[i], stf);
00141                                                                 transformer.setTransform(stf);
00142                                                         }
00143                                                         
00144                                                         last_read_tf = transform->transforms[0].header.stamp;
00145                                         }
00146                                 }
00147                                 Itf++;
00149                         }
00150                         return true;
00151                 }
00152                 
00153                 
00154                 
00155                 rosbag::View *view;
00156                 rosbag::View::iterator I;
00157         private:
00158                 std::string fixed_link;
00159                 std::string target_linkname;
00160                 std::string message_topic;
00161                 rosbag::Bag bag;        
00162                 rosbag::View::iterator Itf;
00163                 tf::Transformer transformer;
00164                 ros::Time last_read_tf;
00165 };
00166 
00167 
00168 #endif
00169 
00170 


ndt_mcl
Author(s): jari
autogenerated on Mon Jan 6 2014 11:32:07