BagReader.cpp
Go to the documentation of this file.
1 /******************************************************************************
2  * Copyright (C) 2015 by Ralf Kaestner *
3  * ralf.kaestner@gmail.com *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the Lesser GNU General Public License as published by*
7  * the Free Software Foundation; either version 3 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * Lesser GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the Lesser GNU General Public License *
16  * along with this program. If not, see <http://www.gnu.org/licenses/>. *
17  ******************************************************************************/
18 
19 #include <QApplication>
20 #include <QMutexLocker>
21 
22 #include <rosbag/bag.h>
23 #include <ros/console.h>
24 #include <rosbag/view.h>
25 
27 
29 
30 namespace rqt_multiplot {
31 
32 /*****************************************************************************/
33 /* Constructors and Destructor */
34 /*****************************************************************************/
35 
36 BagReader::BagReader(QObject* parent) :
37  MessageBroker(parent),
38  impl_(this) {
39  connect(&impl_, SIGNAL(started()), this, SLOT(threadStarted()));
40  connect(&impl_, SIGNAL(finished()), this, SLOT(threadFinished()));
41 }
42 
44  impl_.quit();
45  impl_.wait();
46 }
47 
48 BagReader::Impl::Impl(QObject* parent) :
49  QThread(parent) {
50 }
51 
53  terminate();
54  wait();
55 }
56 
57 /*****************************************************************************/
58 /* Accessors */
59 /*****************************************************************************/
60 
61 QString BagReader::getFileName() const {
62  return impl_.fileName_;
63 }
64 
65 QString BagReader::getError() const {
66  return impl_.error_;
67 }
68 
69 bool BagReader::isReading() const {
70  return impl_.isRunning();
71 }
72 
73 /*****************************************************************************/
74 /* Methods */
75 /*****************************************************************************/
76 
77 void BagReader::read(const QString& fileName) {
78  impl_.wait();
79 
80  impl_.fileName_ = fileName;
81  impl_.error_.clear();
82 
83  impl_.start();
84 }
85 
87  impl_.wait();
88 }
89 
90 bool BagReader::subscribe(const QString& topic, QObject* receiver, const
91  char* method, const PropertyMap& properties, Qt::ConnectionType type) {
92  QMutexLocker lock(&impl_.mutex_);
93 
94  QMap<QString, BagQuery*>::iterator it = impl_.queries_.find(topic);
95 
96  if (it == impl_.queries_.end()) {
97  it = impl_.queries_.insert(topic, new BagQuery(this));
98 
99  connect(it.value(), SIGNAL(aboutToBeDestroyed()), this,
100  SLOT(queryAboutToBeDestroyed()));
101  }
102 
103  return receiver->connect(it.value(), SIGNAL(messageRead(const
104  QString&, const Message&)), method, type);
105 }
106 
107 bool BagReader::unsubscribe(const QString& topic, QObject* receiver,
108  const char* method) {
109  QMutexLocker lock(&impl_.mutex_);
110 
111  QMap<QString, BagQuery*>::iterator it = impl_.queries_.find(topic);
112 
113  if (it != impl_.queries_.end()) {
114  return it.value()->disconnect(SIGNAL(messageRead(const QString&,
115  const Message&)), receiver, method);
116  }
117  else
118  return false;
119 }
120 
121 bool BagReader::event(QEvent* event) {
122  if (event->type() == ProgressChangeEvent::Type) {
123  ProgressChangeEvent* progressChangeEvent = static_cast<
125 
126  emit readingProgressChanged(progressChangeEvent->getProgress());
127 
128  return true;
129  }
130 
131  return QObject::event(event);
132 }
133 
135  if (queries_.isEmpty())
136  return;
137 
138  try {
139  rosbag::Bag bag;
140 
141  bag.open(fileName_.toStdString(), rosbag::bagmode::Read);
142 
143  std::vector<std::string> queriedTopics;
144  queriedTopics.reserve(queries_.count());
145 
146  for (QMap<QString, BagQuery*>::const_iterator jt = queries_.begin();
147  jt != queries_.end(); ++jt)
148  queriedTopics.push_back(jt.key().toStdString());
149 
150  rosbag::View view(bag, rosbag::TopicQuery(queriedTopics));
151 
152  for (rosbag::View::iterator it = view.begin(); it != view.end();
153  ++it) {
154  mutex_.lock();
155 
156  QMap<QString, BagQuery*>::const_iterator jt = queries_.find(
157  QString::fromStdString(it->getTopic()));
158 
159  if (jt != queries_.end())
160  jt.value()->callback(*it);
161 
162  mutex_.unlock();
163 
164  double progress = (it->getTime()-view.getBeginTime()).toSec()/
165  (view.getEndTime()-view.getBeginTime()).toSec();
166 
167  ProgressChangeEvent* progressChangeEvent = new
168  ProgressChangeEvent(progress);
169 
170  QApplication::postEvent(parent(), progressChangeEvent);
171  }
172  }
173  catch (const ros::Exception& exception) {
174  error_ = QString::fromStdString(exception.what());
175  }
176 }
177 
178 /*****************************************************************************/
179 /* Slots */
180 /*****************************************************************************/
181 
183  emit readingStarted();
184 }
185 
187  if (impl_.error_.isEmpty()) {
188  ROS_INFO_STREAM("Read bag from [file://" <<
189  impl_.fileName_.toStdString() << "]");
190 
191  emit readingFinished();
192  }
193  else {
194  ROS_ERROR_STREAM("Failed to read bag from [file://" <<
195  impl_.fileName_.toStdString() << "]: " <<
196  impl_.error_.toStdString());
197 
198  emit readingFailed(impl_.error_);
199  }
200 }
201 
203  for (QMap<QString, BagQuery*>::iterator it = impl_.queries_.begin();
204  it != impl_.queries_.end(); ++it) {
205  if (it.value() == static_cast<BagQuery*>(sender())) {
206  impl_.queries_.erase(it);
207  break;
208  }
209  }
210 }
211 
212 }
void read(const QString &fileName)
Definition: BagReader.cpp:77
void readingFailed(const QString &error)
void open(std::string const &filename, uint32_t mode=bagmode::Read)
bool unsubscribe(const QString &topic, QObject *receiver, const char *method=0)
Definition: BagReader.cpp:107
QMap< int, QVariant > PropertyMap
Definition: MessageBroker.h:33
bool subscribe(const QString &topic, QObject *receiver, const char *method, const PropertyMap &properties=PropertyMap(), Qt::ConnectionType type=Qt::AutoConnection)
Definition: BagReader.cpp:90
ros::Time getBeginTime()
QString getFileName() const
Definition: BagReader.cpp:61
bool event(QEvent *event)
Definition: BagReader.cpp:121
bool isReading() const
Definition: BagReader.cpp:69
ros::Time getEndTime()
#define ROS_INFO_STREAM(args)
iterator end()
void readingProgressChanged(double progress)
iterator begin()
BagReader(QObject *parent=0)
Definition: BagReader.cpp:36
QString getError() const
Definition: BagReader.cpp:65
#define ROS_ERROR_STREAM(args)
Impl(QObject *parent=0)
Definition: BagReader.cpp:48
QMap< QString, BagQuery * > queries_
Definition: BagReader.h:74
void messageRead(const QString &topic, const Message &message)


rqt_multiplot
Author(s): Ralf Kaestner
autogenerated on Wed Jul 10 2019 03:49:44