bag_view.cpp
Go to the documentation of this file.
1 // Filtering view on (multiple) bag files
2 // Author: Max Schwarz <max.schwarz@ais.uni-bonn.de>
3 
4 #include "bag_view.h"
5 
6 #include <rosbag/bag.h>
7 #include <std_msgs/Header.h>
8 #include <std_msgs/UInt8.h>
9 #include "doctest.h"
10 
11 namespace rosbag_fancy
12 {
13 
15 {
16 public:
17  void addBag(BagReader* reader, const std::function<bool(const BagReader::Connection&)>& connectionPredicate)
18  {
19  auto& handle = m_bags.emplace_back();
20  handle.reader = reader;
21  handle.filtering = true;
22 
23  for(auto& conn : reader->connections())
24  {
25  if(handle.connectionIDs.size() <= conn.first)
26  handle.connectionIDs.resize(conn.first+1, false);
27 
28  if(connectionPredicate(conn.second))
29  handle.connectionIDs[conn.first] = true;
30  }
31  }
32 
33  void addBag(BagReader* reader)
34  {
35  auto& handle = m_bags.emplace_back();
36  handle.reader = reader;
37  handle.filtering = false;
38  }
39 
41  {
42  if(m_bags.empty())
43  return {};
44 
45  ros::Time start = m_bags.front().reader->startTime();
46  for(auto& bag : m_bags)
47  start = std::min(start, bag.reader->startTime());
48 
49  return start;
50  }
51 
53  {
54  ros::Time end;
55  for(auto& bag : m_bags)
56  end = std::max(end, bag.reader->endTime());
57 
58  return end;
59  }
60 
61 private:
63 
64  struct BagHandle
65  {
67  bool filtering;
68  std::vector<bool> connectionIDs;
69  };
70  std::vector<BagHandle> m_bags;
71 };
72 
74 {
75 public:
77  {
78  for(auto& handle : view->m_bags)
79  {
80  auto& state = m_state.emplace_back();
81  state.handle = &handle;
82  state.it = handle.reader->begin();
83 
84  if(handle.filtering)
85  state.skipToNext(false);
86  }
87  }
88 
89  Private(const BagView::Private* view, const ros::Time& time)
90  {
91  for(auto& handle : view->m_bags)
92  {
93  auto& state = m_state.emplace_back();
94  state.handle = &handle;
95  state.it = handle.reader->findTime(time);
96 
97  if(handle.filtering)
98  state.skipToNext(false);
99  }
100  }
101 
102  struct BagState
103  {
105  int chunk = -1;
107 
108  void skipToNext(bool advance)
109  {
110  // We need to skip to the next valid message in this bag.
111  auto messageIsInteresting = [&](const BagReader::Message& msg){
112  return handle->connectionIDs[msg.connection->id];
113  };
114  auto connectionIsInteresting = [&](const BagReader::ConnectionInfo& conn){
115  return handle->connectionIDs[conn.id];
116  };
117 
118  if(advance)
119  it.advanceWithPredicates(connectionIsInteresting, messageIsInteresting);
120  else
121  it.findNextWithPredicates(connectionIsInteresting, messageIsInteresting);
122  }
123  };
124 
125  std::vector<BagState> m_state;
128 };
129 
131  : m_d{std::make_shared<Private>(view->m_d.get())}
132 {
133  ++(*this);
134 }
135 
137  : m_d{std::make_shared<Private>(view->m_d.get(), time)}
138 {
139  ++(*this);
140 }
141 
143 {}
144 
146 {
147  if(!m_d)
148  throw std::logic_error{"Attempt to dereference invalid BagView::Iterator"};
149 
150  return m_d->m_msg;
151 }
152 
154 {
155  if(!m_d)
156  return *this;
157 
158  if(m_d->m_nextBag)
159  {
160  auto* bag = m_d->m_nextBag;
161 
162  // We need to skip to the next valid message in this bag.
163  if(bag->handle->filtering)
164  bag->skipToNext(true);
165  else
166  ++bag->it;
167  }
168 
169  // Figure out the earliest available message from all the bags
170  ros::Time earliestStamp;
171  m_d->m_nextBag = nullptr;
172  std::size_t bagIndex = 0;
173 
174  for(std::size_t i = 0; i < m_d->m_state.size(); ++i)
175  {
176  auto& state = m_d->m_state[i];
177 
178  if(state.it == state.handle->reader->end())
179  continue;
180 
181  if(!m_d->m_nextBag || state.it->stamp < earliestStamp)
182  {
183  m_d->m_nextBag = &state;
184  earliestStamp = state.it->stamp;
185  bagIndex = i;
186  }
187  }
188 
189  if(!m_d->m_nextBag)
190  {
191  // End reached, invalidate
192  m_d.reset();
193  return *this;
194  }
195 
196  // Found a message!
197  m_d->m_msg.msg = &*m_d->m_nextBag->it;
198  m_d->m_msg.bagIndex = bagIndex;
199 
200  return *this;
201 }
202 
204 {
205  // NOTE: This way view.begin() != view.begin(), but I don't care.
206  return a.m_d == b.m_d;
207 }
208 
210 {
211  // NOTE: This way view.begin() != view.begin(), but I don't care.
212  return a.m_d != b.m_d;
213 }
214 
216  : m_d{std::make_unique<Private>()}
217 {}
218 
220 {}
221 
222 void BagView::addBag(BagReader* reader, const std::function<bool(const BagReader::Connection&)>& connectionPredicate)
223 {
224  m_d->addBag(reader, connectionPredicate);
225 }
226 
228 {
229  m_d->addBag(reader);
230 }
231 
233 {
234  return m_d->startTime();
235 }
236 
238 {
239  return m_d->endTime();
240 }
241 
243 {
244  return BagView::Iterator{this};
245 }
246 
248 {
249  return {};
250 }
251 
253 {
254  return BagView::Iterator{this, time};
255 }
256 
257 
258 TEST_CASE("BagView: One file")
259 {
260  // Generate a bag file
261  char bagfileName[] = "/tmp/rosbag_fancy_test_XXXXXX";
262  {
263  int fd = mkstemp(bagfileName);
264  REQUIRE(fd >= 0);
265  close(fd);
266 
267  rosbag::Bag bag{bagfileName, rosbag::BagMode::Write};
268 
269  {
270  std_msgs::Header msg;
271  msg.frame_id = "a";
272  bag.write("/topicA", ros::Time(1000, 0), msg);
273  }
274  {
275  std_msgs::Header msg;
276  msg.frame_id = "b";
277  bag.write("/topicB", ros::Time(1001, 0), msg);
278  }
279  {
280  std_msgs::UInt8 msg;
281  msg.data = 123;
282  bag.write("/topicC", ros::Time(1002, 0), msg);
283  }
284 
285  bag.close();
286  }
287 
288  // Open bagfile
289  BagReader reader{bagfileName};
290 
291  SUBCASE("No selection")
292  {
293  BagView view;
294  view.addBag(&reader);
295 
296  auto it = view.begin();
297 
298  REQUIRE(it != view.end());
299  CHECK(it->msg->connection->topicInBag == "/topicA");
300 
301  ++it; REQUIRE(it != view.end());
302  CHECK(it->msg->connection->topicInBag == "/topicB");
303 
304  ++it; REQUIRE(it != view.end());
305  CHECK(it->msg->connection->topicInBag == "/topicC");
306 
307  ++it; CHECK(it == view.end());
308  }
309 
310  SUBCASE("select by topic")
311  {
312  BagView view;
313  view.addBag(&reader, [&](const BagReader::Connection& con){
314  return con.topicInBag == "/topicB";
315  });
316 
317  int num = 0;
318  for(auto& pmsg : view)
319  {
320  CHECK(pmsg.msg->connection->topicInBag == "/topicB");
321 
322  auto msg = pmsg.msg->instantiate<std_msgs::Header>();
323  REQUIRE(msg);
324  CHECK(msg->frame_id == "b");
325  num++;
326  }
327 
328  CHECK(num == 1);
329  }
330 
331  SUBCASE("select by type")
332  {
333  BagView view;
334  view.addBag(&reader, [&](const BagReader::Connection& con){
335  return con.type == "std_msgs/UInt8";
336  });
337 
338  int num = 0;
339  for(auto& pmsg : view)
340  {
341  CHECK(pmsg.msg->connection->topicInBag == "/topicC");
342 
343  auto msg = pmsg.msg->instantiate<std_msgs::UInt8>();
344  REQUIRE(msg);
345  CHECK(msg->data == 123);
346  num++;
347  }
348 
349  CHECK(num == 1);
350  }
351 
352  // Remove temp file
353  unlink(bagfileName);
354 }
355 
356 }
rosbag::Bag
rosbag_fancy::BagView::Iterator::Private::m_msg
MultiBagMessage m_msg
Definition: bag_view.cpp:127
SUBCASE
#define SUBCASE(name)
Definition: doctest.h:2903
rosbag_fancy::BagView::Private::BagHandle
Definition: bag_view.cpp:64
bag_view.h
rosbag_fancy::BagView::Iterator::~Iterator
~Iterator()
Definition: bag_view.cpp:142
rosbag_fancy::BagView::BagView
BagView()
Definition: bag_view.cpp:215
rosbag_fancy::operator==
bool operator==(const BagView::Iterator &a, const BagView::Iterator &b)
Definition: bag_view.cpp:203
rosbag_fancy::BagReader::Connection
Definition: bag_reader.h:32
rosbag_fancy::BagView::Private
Definition: bag_view.cpp:14
rosbag_fancy::BagView::m_d
std::unique_ptr< Private > m_d
Definition: bag_view.h:75
rosbag_fancy::BagView::~BagView
~BagView()
Definition: bag_view.cpp:219
rosbag_fancy::BagView::Private::startTime
ros::Time startTime() const
Definition: bag_view.cpp:40
rosbag_fancy::BagView::Iterator::Private::Private
Private(const BagView::Private *view)
Definition: bag_view.cpp:76
rosbag_fancy
Definition: bag_reader.cpp:240
rosbag_fancy::BagView::startTime
ros::Time startTime() const
Definition: bag_view.cpp:232
rosbag_fancy::BagView::Iterator::Private::BagState::chunk
int chunk
Definition: bag_view.cpp:105
rosbag_fancy::BagView::Iterator::Private
Definition: bag_view.cpp:73
rosbag_fancy::BagView::Iterator::Private::Private
Private(const BagView::Private *view, const ros::Time &time)
Definition: bag_view.cpp:89
rosbag_fancy::BagReader::Message
Definition: bag_reader.h:58
rosbag_fancy::BagView::Private::BagHandle::filtering
bool filtering
Definition: bag_view.cpp:67
rosbag_fancy::BagView
Definition: bag_view.h:12
bag.h
std::size_t
decltype(sizeof(void *)) typede size_t)
Definition: doctest.h:501
rosbag_fancy::BagReader::ConnectionInfo
Definition: bag_reader.h:49
rosbag_fancy::BagView::Private::BagHandle::reader
BagReader * reader
Definition: bag_view.cpp:66
REQUIRE
#define REQUIRE(...)
Definition: doctest.h:2934
rosbag_fancy::BagView::endTime
ros::Time endTime() const
Definition: bag_view.cpp:237
rosbag_fancy::BagReader::Iterator::advanceWithPredicates
void advanceWithPredicates(const std::function< bool(const ConnectionInfo &)> &connPredicate, const std::function< bool(const Message &)> &messagePredicate)
Definition: bag_reader.cpp:518
rosbag_fancy::BagView::findTime
Iterator findTime(const ros::Time &time) const
Definition: bag_view.cpp:252
rosbag_fancy::BagView::Iterator::Private::BagState::skipToNext
void skipToNext(bool advance)
Definition: bag_view.cpp:108
rosbag_fancy::BagView::Iterator::Private::BagState
Definition: bag_view.cpp:102
rosbag_fancy::BagView::begin
Iterator begin() const
Definition: bag_view.cpp:242
rosbag_fancy::BagView::end
Iterator end() const
Definition: bag_view.cpp:247
rosbag_fancy::BagReader::connections
const ConnectionMap & connections() const
Definition: bag_reader.cpp:759
rosbag_fancy::BagView::Iterator
Definition: bag_view.h:24
CHECK
#define CHECK(...)
Definition: doctest.h:2927
rosbag_fancy::BagView::Private::addBag
void addBag(BagReader *reader, const std::function< bool(const BagReader::Connection &)> &connectionPredicate)
Definition: bag_view.cpp:17
rosbag_fancy::BagView::Iterator::Private::BagState::handle
const BagView::Private::BagHandle * handle
Definition: bag_view.cpp:104
rosbag_fancy::BagView::Private::endTime
ros::Time endTime() const
Definition: bag_view.cpp:52
rosbag_fancy::BagReader::Iterator::findNextWithPredicates
void findNextWithPredicates(const std::function< bool(const ConnectionInfo &)> &connPredicate, const std::function< bool(const Message &)> &messagePredicate)
Definition: bag_reader.cpp:537
rosbag_fancy::BagView::MultiBagMessage
Definition: bag_view.h:17
rosbag_fancy::BagView::Iterator::operator*
reference operator*()
Definition: bag_view.cpp:145
rosbag_fancy::BagView::addBag
void addBag(BagReader *reader)
Definition: bag_view.cpp:227
rosbag_fancy::BagReader
Definition: bag_reader.h:21
rosbag_fancy::BagReader::Iterator
Definition: bag_reader.h:125
start
ROSCPP_DECL void start()
rosbag_fancy::TEST_CASE
TEST_CASE("BagView: One file")
Definition: bag_reader.cpp:831
rosbag::Bag::write
void write(std::string const &topic, ros::MessageEvent< T > const &event)
rosbag_fancy::BagView::Iterator::m_d
std::shared_ptr< Private > m_d
Definition: bag_view.h:53
rosbag_fancy::BagView::Iterator::Iterator
Iterator()
Definition: bag_view.h:32
rosbag_fancy::BagView::Iterator::Private::m_state
std::vector< BagState > m_state
Definition: bag_view.cpp:125
ros::Time
doctest.h
rosbag_fancy::BagView::Iterator::operator++
Iterator & operator++()
Definition: bag_view.cpp:153
rosbag_fancy::BagView::Iterator::Private::BagState::it
BagReader::Iterator it
Definition: bag_view.cpp:106
rosbag_fancy::operator!=
bool operator!=(const BagView::Iterator &a, const BagView::Iterator &b)
Definition: bag_view.cpp:209
rosbag_fancy::BagView::Iterator::Private::m_nextBag
BagState * m_nextBag
Definition: bag_view.cpp:126
rosbag_fancy::BagView::Private::BagHandle::connectionIDs
std::vector< bool > connectionIDs
Definition: bag_view.cpp:68
rosbag_fancy::BagView::Private::addBag
void addBag(BagReader *reader)
Definition: bag_view.cpp:33
rosbag_fancy::BagView::Private::m_bags
std::vector< BagHandle > m_bags
Definition: bag_view.cpp:70


rosbag_fancy
Author(s):
autogenerated on Tue Feb 20 2024 03:20:59