def _message_recorded(self, topic, msg, t):
if self._timeline_frame._start_stamp is None:
self._timeline_frame._start_stamp = t
self._timeline_frame._end_stamp = t
self._timeline_frame._playhead = t
elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp:
self._timeline_frame._end_stamp = t
if not self._timeline_frame.topics or topic not in self._timeline_frame.topics:
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
)
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
if self._timeline_frame._stamp_left is None:
self.reset_zoom()
# Notify the index caching thread that it has work to do
with self._timeline_frame.index_cache_cv:
self._timeline_frame.invalidated_caches.add(topic)
self._timeline_frame.index_cache_cv.notify()
if topic in self._listeners:
for listener in self._listeners[topic]:
try:
listener.timeline_changed()
except Exception as ex:
qWarning('Error calling timeline_changed on %s: %s' %
(type(listener), str(ex)))
def _create_player(self):
if not self._player:
try:
self._player = Player(self)
if self._publish_clock:
self._player.start_clock_publishing()
except Exception as ex:
qWarning('Error starting player; aborting publish: %s' %
str(ex))
return False
return True
def _run_export_region(self, export_bag, topics, start_stamp, end_stamp,
bag_entries):
"""
Threaded function that saves the current selection to a new bag file
:param export_bag: bagfile to write to, ''rosbag.bag''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
total_messages = len(bag_entries)
update_step = max(1, total_messages / 100)
message_num = 1
progress = 0
# Write out the messages
for bag, entry in bag_entries:
if self.background_task_cancel:
break
try:
topic, msg, t = self.read_message(bag, entry.position)
export_bag.write(topic, msg, t)
except Exception as ex:
qWarning('Error exporting message at position %s: %s' %
(str(entry.position), str(ex)))
export_bag.close()
self.stop_background_task()
return
if message_num % update_step == 0 or message_num == total_messages:
new_progress = int(100.0 *
(float(message_num) / total_messages))
if new_progress != progress:
progress = new_progress
if not self.background_task_cancel:
self.background_progress = progress
self.status_bar_changed_signal.emit()
message_num += 1
# Close the bag
try:
self.background_progress = 0
self.status_bar_changed_signal.emit()
export_bag.close()
except Exception as ex:
QMessageBox(
QMessageBox.Warning, 'rqt_bag',
'Error closing bag file [%s]: %s' %
(export_bag.filename, str(ex)), QMessageBox.Ok).exec_()
self.stop_background_task()
def initializeAudio(self):
self.m_pullTimer = QTimer(self)
self.m_pullTimer.timeout.connect(self.pullTimerExpired)
self.m_pullMode = True
self.m_format = QAudioFormat()
self.m_format.setSampleRate(self.DataSampleRateHz)
self.m_format.setChannelCount(1)
self.m_format.setSampleSize(16)
self.m_format.setCodec('audio/pcm')
self.m_format.setByteOrder(QAudioFormat.LittleEndian)
self.m_format.setSampleType(QAudioFormat.SignedInt)
info = QAudioDeviceInfo(QAudioDeviceInfo.defaultOutputDevice())
if not info.isFormatSupported(self.m_format):
qWarning("Default format not supported - trying to use nearest")
self.m_format = info.nearestFormat(self.m_format)
self.m_generator = Generator(self.m_format,
self.DurationSeconds * 1000000, self.ToneSampleRateHz, self)
self.createAudioOutput()
def toggleSuspendResume(self):
if self.m_audioOutput.state() == QAudio.SuspendedState:
qWarning("status: Suspended, resume()")
self.m_audioOutput.resume()
self.m_suspendResumeButton.setText(self.SUSPEND_LABEL)
elif self.m_audioOutput.state() == QAudio.ActiveState:
qWarning("status: Active, suspend()")
self.m_audioOutput.suspend()
self.m_suspendResumeButton.setText(self.RESUME_LABEL)
elif self.m_audioOutput.state() == QAudio.StoppedState:
qWarning("status: Stopped, resume()")
self.m_audioOutput.resume()
self.m_suspendResumeButton.setText(self.SUSPEND_LABEL)
elif self.m_audioOutput.state() == QAudio.IdleState:
qWarning("status: IdleState")
def toggleSuspendResume(self):
if self.m_audioOutput.state() == QAudio.SuspendedState:
qWarning("status: Suspended, resume()")
self.m_audioOutput.resume()
self.m_suspendResumeButton.setText(self.SUSPEND_LABEL)
elif self.m_audioOutput.state() == QAudio.ActiveState:
qWarning("status: Active, suspend()")
self.m_audioOutput.suspend()
self.m_suspendResumeButton.setText(self.RESUME_LABEL)
elif self.m_audioOutput.state() == QAudio.StoppedState:
qWarning("status: Stopped, resume()")
self.m_audioOutput.resume()
self.m_suspendResumeButton.setText(self.SUSPEND_LABEL)
elif self.m_audioOutput.state() == QAudio.IdleState:
qWarning("status: IdleState")
def run(self):
"""
Thread body. loops and notifies the listener of new messages
"""
while not self._stop_flag:
# Wait for a new message
cv = self.timeline._messages_cvs[self.topic]
with cv:
while (self.topic not in self.timeline._messages) or (
self.bag_msg_data
== self.timeline._messages[self.topic]):
cv.wait()
if self._stop_flag:
return
bag_msg_data = self.timeline._messages[self.topic]
# View the message
self.bag_msg_data = bag_msg_data
try:
event = ListenerEvent(bag_msg_data)
QCoreApplication.postEvent(self.listener, event)
except Exception as ex:
qWarning('Error notifying listener %s: %s' %
(type(self.listener), str(ex)))
def record_bag(self, filename, all=True, topics=[], regex=False, limit=0):
try:
self._recorder = Recorder(filename,
bag_lock=self._bag_lock,
all=all,
topics=topics,
regex=regex,
limit=limit)
except Exception as ex:
qWarning('Error opening bag for recording [%s]: %s' %
(filename, str(ex)))
return
self._recorder.add_listener(self._message_recorded)
self.add_bag(self._recorder.bag)
self._recorder.start()
self.wrap = False
self._timeline_frame._index_cache_thread.period = 0.1
self.update()
def load_bag(self, filename):
qDebug("Loading '%s'..." % filename.encode(errors='replace'))
# QProgressBar can EITHER: show text or show a bouncing loading bar,
# but apparently the text is hidden when the bounding loading bar is
# shown
#self.progress_bar.setRange(0, 0)
self.set_status_text.emit("Loading '%s'..." % filename)
#progress_format = self.progress_bar.format()
#progress_text_visible = self.progress_bar.isTextVisible()
#self.progress_bar.setFormat("Loading %s" % filename)
#self.progress_bar.setTextVisible(True)
try:
bag = rosbag.Bag(filename)
self.play_button.setEnabled(True)
self.thumbs_button.setEnabled(True)
self.zoom_in_button.setEnabled(True)
self.zoom_out_button.setEnabled(True)
self.zoom_all_button.setEnabled(True)
self.next_button.setEnabled(True)
self.previous_button.setEnabled(True)
self.faster_button.setEnabled(True)
self.slower_button.setEnabled(True)
self.begin_button.setEnabled(True)
self.end_button.setEnabled(True)
self.save_button.setEnabled(True)
self.record_button.setEnabled(False)
self._timeline.add_bag(bag)
qDebug("Done loading '%s'" % filename.encode(errors='replace'))
# put the progress bar back the way it was
self.set_status_text.emit("")
except rosbag.ROSBagException as e:
qWarning("Loading '%s' failed due to: %s" %
(filename.encode(errors='replace'), e))
self.set_status_text.emit("Loading '%s' failed due to: %s" %
(filename, e))
def addTask(self, name):
try:
self._tasks.add(Task(name))
except Exception as e:
qWarning(str(e))
def handleStateChanged(self, state):
qWarning("state = " + self.stateMap.get(state, "Unknown"))
def notified(self):
qWarning(
"bytesFree = %d, elapsedUSecs = %d, processedUSecs = %d" %
(self.m_audioOutput.bytesFree(), self.m_audioOutput.elapsedUSecs(),
self.m_audioOutput.processedUSecs()))
def _resample_thread(self):
# TODO:
# * look into doing partial display updates for long resampling
# operations
# * add a progress bar for resampling operations
x = {}
y = {}
for path in self.resample_fields:
x[path] = []
y[path] = []
# bag object is not thread-safe; lock it while we resample
with self.timeline._bag_lock:
try:
msgdata = self.load_data()
except ValueError:
# bag is closed or invalid; we're done here
self.resampling_active = False
return
for entry in msgdata:
# detect if we're cancelled and return early
if not self.resampling_active:
return
for path in self.resample_fields:
# this resampling method is very unstable, because it picks
# representative points rather than explicitly representing
# the minimum and maximum values present within a sample
# If the data has spikes, this is particularly bad because they
# will be missed entirely at some resolutions and offsets
if x[path]==[] or (entry[2]-self.start_stamp).to_sec()-x[path][-1] >= self.timestep:
y_value = entry[1]
for field in path.split('.'):
index = None
if field.endswith(']'):
field = field[:-1]
field, _, index = field.rpartition('[')
y_value = getattr(y_value, field)
if index:
index = int(index)
y_value = y_value[index]
y[path].append(y_value)
x[path].append((entry[2]-self.start_stamp).to_sec())
# TODO: incremental plot updates would go here...
# we should probably do incremental updates based on time;
# that is, push new data to the plot maybe every .5 or .1
# seconds
# time is a more useful metric than, say, messages loaded or
# percentage, because it will give a reasonable refresh rate
# without overloading the computer
# if we had a progress bar, we could emit a signal to update it here
# update the plot with final resampled data
for path in self.resample_fields:
if len(x[path]) < 1:
qWarning("Resampling resulted in 0 data points for %s" % path)
else:
if path in self.paths_on:
self.plot.clear_values(path)
self.plot.update_values(path, x[path], y[path])
else:
self.plot.add_curve(path, path, x[path], y[path])
self.paths_on.add(path)
self.plot.redraw()
self.resample_fields.clear()
self.resampling_active = False
def handleStateChanged(self, state):
qWarning("state = " + self.stateMap.get(state, "Unknown"))
def notified(self):
qWarning("bytesFree = %d, elapsedUSecs = %d, processedUSecs = %d" % (
self.m_audioOutput.bytesFree(),
self.m_audioOutput.elapsedUSecs(),
self.m_audioOutput.processedUSecs()))
