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
00011 #include <pcl/conversions.h>
00012
00013 template<typename MessageType>
00014 class tfMessageReader
00015 {
00016
00017 public:
00021 tfMessageReader(std::string bagfilename, std::string message_topic_, std::string fixed_link_, std::string target_linkname_){
00022 message_topic = message_topic_;
00023 fixed_link = fixed_link_;
00024 target_linkname = target_linkname_;
00025
00026 fprintf(stderr,"Opening '%s'\n",bagfilename.c_str());
00027 bag.open(bagfilename, rosbag::bagmode::Read);
00028
00029 std::vector<std::string> topics;
00030 topics.push_back("/tf");
00031 topics.push_back(message_topic);
00032
00033 view = new rosbag::View(bag);
00034 I = view->begin();
00035 Itf = view->begin();
00036 }
00037
00038 bool bagEnd(){
00039 if(I == view->end()) return true;
00040 return false;
00041 }
00042
00046 bool getTf(const std::string &frame_name,const ros::Time &stamp, tf::Transform &pose){
00047 std::string schaiba;
00048 schaiba.resize(500);
00049 if(!transformer.canTransform(fixed_link, frame_name, stamp, &schaiba)){
00050 fprintf(stderr,"WTF:%s\n",schaiba.c_str());
00051 return false;
00052 }
00053
00054 tf::StampedTransform transform;
00055 transformer.lookupTransform(fixed_link, frame_name, stamp,transform);
00056 pose = transform;
00057 return true;
00058 }
00059
00060
00067 bool getNextMessage(MessageType &msg_out, tf::Transform &sensor_pose){
00068 if(I == view->end()){
00069 fprintf(stderr,"End of measurement file Reached!!\n");
00070 return false;
00071 }
00072
00073 bool gotMessage = false;
00074 ros::Time ts;
00075 rosbag::MessageInstance const m = *I;
00076
00077 typename MessageType::ConstPtr msg = m.instantiate<MessageType>();
00078
00079 if(msg != NULL){
00080 if(m.getTopic() == message_topic){
00081 msg_out = (*msg);
00082 ts = msg_out.header.stamp;
00083 gotMessage = true;
00084
00085 }else{
00086
00087 }
00088 }else{
00089
00090 }
00091
00092 readUntilTime(ts+ros::Duration(2.0));
00093
00094
00095 if(gotMessage){
00096 std::string schaiba;
00097 schaiba.resize(200);
00098 if(!transformer.canTransform(fixed_link, target_linkname, ts, &schaiba)){
00099 fprintf(stderr,"%s\n",schaiba.c_str());
00100 I++;
00101 return false;
00102 }
00103
00104 tf::StampedTransform transform;
00105 transformer.lookupTransform(fixed_link, target_linkname, ts,transform);
00106 sensor_pose = transform;
00107 I++;
00108 return true;
00109 }
00110
00111 I++;
00112 return false;
00113 }
00114
00115
00119 bool readUntilTime(ros::Time t){
00120 if(Itf == view->end()){
00121 return false;
00122 }
00123 if(last_read_tf>t){
00124
00125 return false;
00126 }
00127
00128 while(last_read_tf <= t && Itf != view->end()){
00129 rosbag::MessageInstance const m = *Itf;
00130
00132 tf::tfMessage::ConstPtr transform = m.instantiate<tf::tfMessage>();
00133 if (transform != NULL){
00134 if(m.getTopic() == "/tf"){
00135
00136 if(transform->transforms.size()<=0) continue;
00137
00138 for (unsigned int i = 0; i < transform->transforms.size(); i++){
00139
00140
00141 tf::StampedTransform stf;
00142 transformStampedMsgToTF(transform->transforms[i], stf);
00143 transformer.setTransform(stf);
00144 }
00145
00146 last_read_tf = transform->transforms[0].header.stamp;
00147 }
00148 }
00149 Itf++;
00151 }
00152 return true;
00153 }
00154
00155
00156
00157 rosbag::View *view;
00158 rosbag::View::iterator I;
00159 private:
00160 std::string fixed_link;
00161 std::string target_linkname;
00162 std::string message_topic;
00163 rosbag::Bag bag;
00164 rosbag::View::iterator Itf;
00165 tf::Transformer transformer;
00166 ros::Time last_read_tf;
00167 };
00168
00169
00170 #endif
00171
00172