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


rosbag_storage
Author(s):
autogenerated on Thu Jun 6 2019 21:09:52