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 #include "rosbag/view.h"
00029 #include "rosbag/bag.h"
00030 #include "rosbag/message_instance.h"
00031
00032 #include <boost/foreach.hpp>
00033 #include <set>
00034
00035 #define foreach BOOST_FOREACH
00036
00037 using std::map;
00038 using std::string;
00039 using std::vector;
00040 using std::multiset;
00041
00042 namespace rosbag {
00043
00044
00045
00046 View::iterator::iterator() : view_(NULL), view_revision_(0), message_instance_(NULL) { }
00047
00048 View::iterator::~iterator()
00049 {
00050 if (message_instance_ != NULL)
00051 delete message_instance_;
00052 }
00053
00054 View::iterator::iterator(View* view, bool end) : view_(view), view_revision_(0), message_instance_(NULL) {
00055 if (view != NULL && !end)
00056 populate();
00057 }
00058
00059 View::iterator::iterator(const iterator& i) : view_(i.view_), iters_(i.iters_), view_revision_(i.view_revision_), message_instance_(NULL) { }
00060
00061 void View::iterator::populate() {
00062 ROS_ASSERT(view_ != NULL);
00063
00064 iters_.clear();
00065 foreach(MessageRange const* range, view_->ranges_)
00066 if (range->begin != range->end)
00067 iters_.push_back(ViewIterHelper(range->begin, range));
00068
00069 std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
00070 view_revision_ = view_->view_revision_;
00071 }
00072
00073 void View::iterator::populateSeek(multiset<IndexEntry>::const_iterator iter) {
00074 ROS_ASSERT(view_ != NULL);
00075
00076 iters_.clear();
00077 foreach(MessageRange const* range, view_->ranges_) {
00078 multiset<IndexEntry>::const_iterator start = std::lower_bound(range->begin, range->end, iter->time, IndexEntryCompare());
00079 if (start != range->end)
00080 iters_.push_back(ViewIterHelper(start, range));
00081 }
00082
00083 std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
00084 while (iter != iters_.back().iter)
00085 increment();
00086
00087 view_revision_ = view_->view_revision_;
00088 }
00089
00090 bool View::iterator::equal(View::iterator const& other) const {
00091
00092
00093
00094
00095 if (iters_.empty())
00096 return other.iters_.empty();
00097 if (other.iters_.empty())
00098 return false;
00099
00100 return iters_.back().iter == other.iters_.back().iter;
00101 }
00102
00103 void View::iterator::increment() {
00104 ROS_ASSERT(view_ != NULL);
00105
00106
00107 if (message_instance_ != NULL)
00108 {
00109 delete message_instance_;
00110 message_instance_ = NULL;
00111 }
00112
00113 view_->update();
00114
00115
00116
00117
00118 if (view_revision_ != view_->view_revision_)
00119 populateSeek(iters_.back().iter);
00120
00121 if (view_->reduce_overlap_)
00122 {
00123 std::multiset<IndexEntry>::const_iterator last_iter = iters_.back().iter;
00124
00125 while (iters_.back().iter == last_iter)
00126 {
00127 iters_.back().iter++;
00128 if (iters_.back().iter == iters_.back().range->end)
00129 iters_.pop_back();
00130
00131 std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
00132 }
00133
00134 } else {
00135
00136 iters_.back().iter++;
00137 if (iters_.back().iter == iters_.back().range->end)
00138 iters_.pop_back();
00139
00140 std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
00141 }
00142 }
00143
00144 MessageInstance& View::iterator::dereference() const {
00145 ViewIterHelper const& i = iters_.back();
00146
00147 if (message_instance_ == NULL)
00148 message_instance_ = view_->newMessageInstance(i.range->connection_info, *(i.iter), *(i.range->bag_query->bag));
00149
00150 return *message_instance_;
00151 }
00152
00153
00154
00155 View::View(bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) { }
00156
00157 View::View(Bag const& bag, ros::Time const& start_time, ros::Time const& end_time, bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) {
00158 addQuery(bag, start_time, end_time);
00159 }
00160
00161 View::View(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query, ros::Time const& start_time, ros::Time const& end_time, bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) {
00162 addQuery(bag, query, start_time, end_time);
00163 }
00164
00165 View::~View() {
00166 foreach(MessageRange* range, ranges_)
00167 delete range;
00168 foreach(BagQuery* query, queries_)
00169 delete query;
00170 }
00171
00172
00173 ros::Time View::getBeginTime()
00174 {
00175 update();
00176
00177 ros::Time begin = ros::TIME_MAX;
00178
00179 foreach (rosbag::MessageRange* range, ranges_)
00180 {
00181 if (range->begin->time < begin)
00182 begin = range->begin->time;
00183 }
00184
00185 return begin;
00186 }
00187
00188 ros::Time View::getEndTime()
00189 {
00190 update();
00191
00192 ros::Time end = ros::TIME_MIN;
00193
00194 foreach (rosbag::MessageRange* range, ranges_)
00195 {
00196 std::multiset<IndexEntry>::const_iterator e = range->end;
00197 e--;
00198
00199 if (e->time > end)
00200 end = e->time;
00201 }
00202
00203 return end;
00204 }
00205
00207 View::iterator View::begin() {
00208 update();
00209 return iterator(this);
00210 }
00211
00213 View::iterator View::end() { return iterator(this, true); }
00214
00215 uint32_t View::size() {
00216
00217 update();
00218
00219 if (size_revision_ != view_revision_)
00220 {
00221 size_cache_ = 0;
00222
00223 foreach (MessageRange* range, ranges_)
00224 {
00225 size_cache_ += std::distance(range->begin, range->end);
00226 }
00227
00228 size_revision_ = view_revision_;
00229 }
00230
00231 return size_cache_;
00232 }
00233
00234 void View::addQuery(Bag const& bag, ros::Time const& start_time, ros::Time const& end_time) {
00235 if ((bag.getMode() & bagmode::Read) != bagmode::Read)
00236 throw BagException("Bag not opened for reading");
00237
00238 boost::function<bool(ConnectionInfo const*)> query = TrueQuery();
00239
00240 queries_.push_back(new BagQuery(&bag, Query(query, start_time, end_time), bag.bag_revision_));
00241
00242 updateQueries(queries_.back());
00243 }
00244
00245 void View::addQuery(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query, ros::Time const& start_time, ros::Time const& end_time) {
00246 if ((bag.getMode() & bagmode::Read) != bagmode::Read)
00247 throw BagException("Bag not opened for reading");
00248
00249 queries_.push_back(new BagQuery(&bag, Query(query, start_time, end_time), bag.bag_revision_));
00250
00251 updateQueries(queries_.back());
00252 }
00253
00254 void View::updateQueries(BagQuery* q) {
00255 for (map<uint32_t, ConnectionInfo*>::const_iterator i = q->bag->connections_.begin(); i != q->bag->connections_.end(); i++) {
00256 ConnectionInfo const* connection = i->second;
00257
00258
00259 if (!q->query.getQuery()(connection))
00260 continue;
00261
00262 map<uint32_t, multiset<IndexEntry> >::const_iterator j = q->bag->connection_indexes_.find(connection->id);
00263
00264
00265 if (j == q->bag->connection_indexes_.end())
00266 continue;
00267 multiset<IndexEntry> const& index = j->second;
00268
00269
00270
00271 std::multiset<IndexEntry>::const_iterator begin = std::lower_bound(index.begin(), index.end(), q->query.getStartTime(), IndexEntryCompare());
00272 std::multiset<IndexEntry>::const_iterator end = std::upper_bound(index.begin(), index.end(), q->query.getEndTime(), IndexEntryCompare());
00273
00274
00275 while (begin != index.begin() && begin->time >= q->query.getStartTime())
00276 {
00277 begin--;
00278 if (begin->time < q->query.getStartTime())
00279 {
00280 begin++;
00281 break;
00282 }
00283 }
00284
00285 if (begin != end)
00286 {
00287
00288 bool found = false;
00289 for (vector<MessageRange*>::iterator k = ranges_.begin(); k != ranges_.end(); k++) {
00290 MessageRange* r = *k;
00291
00292
00293 if (r->bag_query == q && r->connection_info->id == connection->id) {
00294 r->begin = begin;
00295 r->end = end;
00296 found = true;
00297 break;
00298 }
00299 }
00300 if (!found)
00301 ranges_.push_back(new MessageRange(begin, end, connection, q));
00302 }
00303 }
00304
00305 view_revision_++;
00306 }
00307
00308 void View::update() {
00309 foreach(BagQuery* query, queries_) {
00310 if (query->bag->bag_revision_ != query->bag_revision) {
00311 updateQueries(query);
00312 query->bag_revision = query->bag->bag_revision_;
00313 }
00314 }
00315 }
00316
00317 std::vector<const ConnectionInfo*> View::getConnections()
00318 {
00319 std::vector<const ConnectionInfo*> connections;
00320
00321 foreach(MessageRange* range, ranges_)
00322 {
00323 connections.push_back(range->connection_info);
00324 }
00325
00326 return connections;
00327 }
00328
00329 MessageInstance* View::newMessageInstance(ConnectionInfo const* connection_info, IndexEntry const& index, Bag const& bag)
00330 {
00331 return new MessageInstance(connection_info, index, bag);
00332 }
00333
00334
00335 }