view.cpp
Go to the documentation of this file.
00001 // Copyright (c) 2009, Willow Garage, Inc.
00002 // All rights reserved.
00003 // 
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 // 
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of Willow Garage, Inc. nor the names of its
00013 //       contributors may be used to endorse or promote products derived from
00014 //       this software without specific prior written permission.
00015 // 
00016 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026 // POSSIBILITY OF SUCH DAMAGE.
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 // View::iterator
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     // We need some way of verifying these are actually talking about
00092     // the same merge_queue data since we shouldn't be able to compare
00093     // iterators from different Views.
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     // Our message instance is no longer valid
00107     if (message_instance_ != NULL)
00108     {
00109       delete message_instance_;
00110       message_instance_ = NULL;
00111     }
00112 
00113     view_->update();
00114 
00115     // Note, updating may have blown away our message-ranges and
00116     // replaced them in general the ViewIterHelpers are no longer
00117     // valid, but the iterator it stores should still be good.
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 // View
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         // Skip if the query doesn't evaluate to true
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         // Skip if the bag doesn't have the corresponding index
00265         if (j == q->bag->connection_indexes_.end())
00266             continue;
00267         multiset<IndexEntry> const& index = j->second;
00268 
00269         // lower_bound/upper_bound do a binary search to find the appropriate range of Index Entries given our time range
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         // Make sure we are at the right beginning
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             // todo: make faster with a map of maps
00288             bool found = false;
00289             for (vector<MessageRange*>::iterator k = ranges_.begin(); k != ranges_.end(); k++) {
00290                 MessageRange* r = *k;
00291                 
00292                 // If the topic and query are already in our ranges, we update
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 } // namespace rosbag


rosbag
Author(s): Tim Field, Jeremy Leibs, James Bowman
autogenerated on Mon Oct 6 2014 11:47:09