view.cpp
Go to the documentation of this file.
1 // Copyright (c) 2009, Willow Garage, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 //
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of Willow Garage, Inc. nor the names of its
13 // contributors may be used to endorse or promote products derived from
14 // this software without specific prior written permission.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 // POSSIBILITY OF SUCH DAMAGE.
27 
28 #include "rosbag/view.h"
29 #include "rosbag/bag.h"
31 
32 #include <boost/foreach.hpp>
33 #include <set>
34 #include <assert.h>
35 
36 #define foreach BOOST_FOREACH
37 
38 using std::map;
39 using std::string;
40 using std::vector;
41 using std::multiset;
42 
43 namespace rosbag {
44 
45 // View::iterator
46 
47 View::iterator::iterator() : view_(NULL), view_revision_(0), message_instance_(NULL) { }
48 
50 {
51  if (message_instance_ != NULL)
52  delete message_instance_;
53 }
54 
56  if (view != NULL && !end)
57  populate();
58 }
59 
61 
63  if (this != &i) {
64  view_ = i.view_;
65  iters_ = i.iters_;
67  if (message_instance_ != NULL) {
68  delete message_instance_;
70  }
71  }
72  return *this;
73 }
74 
76  assert(view_ != NULL);
77 
78  iters_.clear();
79  foreach(MessageRange const* range, view_->ranges_)
80  if (range->begin != range->end)
81  iters_.push_back(ViewIterHelper(range->begin, range));
82 
83  std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
85 }
86 
87 void View::iterator::populateSeek(multiset<IndexEntry>::const_iterator iter) {
88  assert(view_ != NULL);
89 
90  iters_.clear();
91  foreach(MessageRange const* range, view_->ranges_) {
92  multiset<IndexEntry>::const_iterator start = std::lower_bound(range->begin, range->end, iter->time, IndexEntryCompare());
93  if (start != range->end)
94  iters_.push_back(ViewIterHelper(start, range));
95  }
96 
97  std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
98  while (iter != iters_.back().iter)
99  increment();
100 
102 }
103 
104 bool View::iterator::equal(View::iterator const& other) const {
105  // We need some way of verifying these are actually talking about
106  // the same merge_queue data since we shouldn't be able to compare
107  // iterators from different Views.
108 
109  if (iters_.empty())
110  return other.iters_.empty();
111  if (other.iters_.empty())
112  return false;
113 
114  return iters_.back().iter == other.iters_.back().iter;
115 }
116 
118  assert(view_ != NULL);
119 
120  // Our message instance is no longer valid
121  if (message_instance_ != NULL)
122  {
123  delete message_instance_;
125  }
126 
127  view_->update();
128 
129  // Note, updating may have blown away our message-ranges and
130  // replaced them in general the ViewIterHelpers are no longer
131  // valid, but the iterator it stores should still be good.
133  populateSeek(iters_.back().iter);
134 
135  if (view_->reduce_overlap_)
136  {
137  std::multiset<IndexEntry>::const_iterator last_iter = iters_.back().iter;
138 
139  while (iters_.back().iter == last_iter)
140  {
141  iters_.back().iter++;
142  if (iters_.back().iter == iters_.back().range->end)
143  iters_.pop_back();
144 
145  std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
146  }
147 
148  } else {
149 
150  iters_.back().iter++;
151  if (iters_.back().iter == iters_.back().range->end)
152  iters_.pop_back();
153 
154  std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
155  }
156 }
157 
159  ViewIterHelper const& i = iters_.back();
160 
161  if (message_instance_ == NULL)
163 
164  return *message_instance_;
165 }
166 
167 // View
168 
169 View::View(bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) { }
170 
171 View::View(Bag const& bag, rs2rosinternal::Time const& start_time, rs2rosinternal::Time const& end_time, bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) {
172  addQuery(bag, start_time, end_time);
173 }
174 
175 View::View(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query, rs2rosinternal::Time const& start_time, rs2rosinternal::Time const& end_time, bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) {
176  addQuery(bag, query, start_time, end_time);
177 }
178 
180  foreach(MessageRange* range, ranges_)
181  delete range;
182  foreach(BagQuery* query, queries_)
183  delete query;
184 }
185 
186 
188 {
189  update();
190 
192 
194  {
195  if (range->begin->time < begin)
196  begin = range->begin->time;
197  }
198 
199  return begin;
200 }
201 
203 {
204  update();
205 
207 
209  {
210  std::multiset<IndexEntry>::const_iterator e = range->end;
211  e--;
212 
213  if (e->time > end)
214  end = e->time;
215  }
216 
217  return end;
218 }
219 
222  update();
223  return iterator(this);
224 }
225 
227 View::iterator View::end() { return iterator(this, true); }
228 
230 
231  update();
232 
234  {
235  size_cache_ = 0;
236 
237  foreach (MessageRange* range, ranges_)
238  {
239  size_cache_ += static_cast<uint32_t>(std::distance(range->begin, range->end));
240  }
241 
243  }
244 
245  return size_cache_;
246 }
247 
248 void View::addQuery(Bag const& bag, rs2rosinternal::Time const& start_time, rs2rosinternal::Time const& end_time) {
249  if ((bag.getMode() & bagmode::Read) != bagmode::Read)
250  throw BagException("Bag not opened for reading");
251 
253 
254  queries_.push_back(new BagQuery(&bag, Query(query, start_time, end_time), bag.bag_revision_));
255 
256  updateQueries(queries_.back());
257 }
258 
259 void View::addQuery(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query, rs2rosinternal::Time const& start_time, rs2rosinternal::Time const& end_time) {
260  if ((bag.getMode() & bagmode::Read) != bagmode::Read)
261  throw BagException("Bag not opened for reading");
262 
263  queries_.push_back(new BagQuery(&bag, Query(query, start_time, end_time), bag.bag_revision_));
264 
265  updateQueries(queries_.back());
266 }
267 
269  for (map<uint32_t, ConnectionInfo*>::const_iterator i = q->bag->connections_.begin(); i != q->bag->connections_.end(); i++) {
270  ConnectionInfo const* connection = i->second;
271 
272  // Skip if the query doesn't evaluate to true
273  if (!q->query.getQuery()(connection))
274  continue;
275 
276  map<uint32_t, multiset<IndexEntry> >::const_iterator j = q->bag->connection_indexes_.find(connection->id);
277 
278  // Skip if the bag doesn't have the corresponding index
279  if (j == q->bag->connection_indexes_.end())
280  continue;
281  multiset<IndexEntry> const& index = j->second;
282 
283  // lower_bound/upper_bound do a binary search to find the appropriate range of Index Entries given our time range
284 
285  std::multiset<IndexEntry>::const_iterator begin = index.lower_bound({ q->query.getStartTime(), 0, 0 });
286  std::multiset<IndexEntry>::const_iterator end = index.upper_bound({ q->query.getEndTime() , 0, 0 });
287 
288  // Make sure we are at the right beginning
289  while (begin != index.begin())
290  {
291  if (begin != index.end() && begin->time >= q->query.getStartTime())
292  break;
293 
294  begin--;
295  if (begin->time < q->query.getStartTime())
296  {
297  begin++;
298  break;
299  }
300  }
301 
302  if (begin != end)
303  {
304  // todo: make faster with a map of maps
305  bool found = false;
306  for (vector<MessageRange*>::iterator k = ranges_.begin(); k != ranges_.end(); k++) {
307  MessageRange* r = *k;
308 
309  // If the topic and query are already in our ranges, we update
310  if (r->bag_query == q && r->connection_info->id == connection->id) {
311  r->begin = begin;
312  r->end = end;
313  found = true;
314  break;
315  }
316  }
317  if (!found)
318  ranges_.push_back(new MessageRange(begin, end, connection, q));
319  }
320  }
321 
322  view_revision_++;
323 }
324 
325 void View::update() {
326  foreach(BagQuery* query, queries_) {
327  if (query->bag->bag_revision_ != query->bag_revision) {
328  updateQueries(query);
329  query->bag_revision = query->bag->bag_revision_;
330  }
331  }
332 }
333 
334 std::vector<const ConnectionInfo*> View::getConnections()
335 {
336  std::vector<const ConnectionInfo*> connections;
337 
338  foreach(MessageRange* range, ranges_)
339  {
340  connections.push_back(range->connection_info);
341  }
342 
343  return connections;
344 }
345 
346 MessageInstance* View::newMessageInstance(ConnectionInfo const* connection_info, IndexEntry const& index, Bag const& bag)
347 {
348  return new MessageInstance(connection_info, index, bag);
349 }
350 
351 
352 } // namespace rosbag
BagMode getMode() const
Get the mode the bag is in.
Definition: bag.cpp:185
boost::function< bool(ConnectionInfo const *)> const & getQuery() const
Get the query functor.
Definition: query.cpp:49
GLuint GLuint end
rs2rosinternal::Time getEndTime()
Definition: view.cpp:202
Query query
Definition: query.h:105
std::vector< const ConnectionInfo * > getConnections()
Definition: view.cpp:334
bool equal(iterator const &other) const
Definition: view.cpp:104
std::map< uint32_t, ConnectionInfo * > connections_
Definition: bag.h:295
ROSTIME_DECL const Time TIME_MAX
uint32_t size_revision_
Definition: view.h:172
The actual iterator data structure.
Definition: query.h:123
uint32_t view_revision_
Definition: view.h:169
MessageInstance * newMessageInstance(ConnectionInfo const *connection_info, IndexEntry const &index, Bag const &bag)
Definition: view.cpp:346
Base class for rosbag exceptions.
Definition: exceptions.h:43
GLsizei const GLchar *const * string
uint32_t view_revision_
Definition: view.h:89
An iterator that points to a MessageInstance from a bag.
Definition: view.h:60
e
Definition: rmse.py:177
rs2rosinternal::Time getBeginTime()
Definition: view.cpp:187
GLsizei GLsizei GLfloat distance
Definition: glext.h:10563
void sort(sort_type m_sort_type, const std::string &in, const std::string &out)
GLuint index
void update()
Definition: view.cpp:325
uint32_t size_cache_
Definition: view.h:171
iterator & operator=(iterator const &i)
Definition: view.cpp:62
uint32_t size()
Definition: view.cpp:229
uint32_t bag_revision
Definition: query.h:106
GLdouble GLdouble r
MessageInstance * message_instance_
Definition: view.h:90
void updateQueries(BagQuery *q)
Definition: view.cpp:268
unsigned int uint32_t
Definition: stdint.h:80
A class pointing into a bag file.
std::multiset< IndexEntry >::const_iterator iter
Definition: query.h:127
Time representation. May either represent wall clock time or ROS clock time.
Definition: time.h:177
ConnectionInfo const * connection_info
Definition: query.h:118
uint32_t bag_revision_
Definition: bag.h:280
GLuint start
GLint j
rs2rosinternal::Time const & getEndTime() const
Get the end-time.
Definition: query.cpp:54
BagQuery const * bag_query
pointer to vector of queries in View
Definition: query.h:119
std::map< uint32_t, std::multiset< IndexEntry > > connection_indexes_
Definition: bag.h:299
std::vector< ViewIterHelper > iters_
Definition: view.h:88
MessageRange const * range
pointer to vector of ranges in View
Definition: query.h:128
std::multiset< IndexEntry >::const_iterator begin
Definition: query.h:116
Pairs of queries and the bags they come from (used internally by View)
Definition: query.h:100
Bag const * bag
Definition: query.h:104
std::vector< MessageRange * > ranges_
Definition: view.h:167
rs2rosinternal::Time const & getStartTime() const
Get the start-time.
Definition: query.cpp:53
GLdouble GLdouble GLdouble q
ROSTIME_DECL const Time TIME_MIN
void populateSeek(std::multiset< IndexEntry >::const_iterator iter)
Definition: view.cpp:87
GLenum query
Definition: bag.h:66
friend class iterator
Definition: view.h:155
iterator end()
Default constructed iterator signifies end.
Definition: view.cpp:227
iterator begin()
Simply copy the merge_queue state into the iterator.
Definition: view.cpp:221
bool reduce_overlap_
Definition: view.h:174
std::vector< BagQuery * > queries_
Definition: view.h:168
GLsizei range
#define NULL
Definition: tinycthread.c:47
int i
View(bool const &reduce_overlap=false)
Create a view on a bag.
Definition: view.cpp:169
std::multiset< IndexEntry >::const_iterator end
Definition: query.h:117
MessageInstance & dereference() const
Definition: view.cpp:158
void addQuery(Bag const &bag, rs2rosinternal::Time const &start_time=rs2rosinternal::TIME_MIN, rs2rosinternal::Time const &end_time=rs2rosinternal::TIME_MAX)
Add a query to a view.
Definition: view.cpp:248
GeneratorWrapper< T > map(Func &&function, GeneratorWrapper< U > &&generator)
Definition: catch.hpp:4271


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:50:14