rxbag: re-publish messages #2067

This commit is contained in:
Tim Field 2010-05-20 04:31:20 +00:00
parent 6b740998ac
commit ecf89a48b2
5 changed files with 344 additions and 134 deletions

97
tools/rxbag/src/rxbag/player.py Executable file
View File

@ -0,0 +1,97 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$
from __future__ import with_statement
PKG = 'rxbag'
import roslib; roslib.load_manifest(PKG)
import threading
import time
import rosbag
import rosgraph.masterapi
import rospy
import sys
class Player(object):
def __init__(self, timeline):
self.timeline = timeline
self._publishing = set()
self._publishers = {}
if not self.timeline.parent.app.connect_to_ros():
raise Exception('Error connecting to ROS')
def is_publishing(self, topic): return topic in self._publishing
def start_publishing(self, topic):
if topic in self._publishing:
return
self.timeline.add_listener(topic, self)
self._publishing.add(topic)
def stop_publishing(self, topic):
if topic not in self._publishing:
return
self.timeline.remove_listener(topic, self)
self._publishing.remove(topic)
if topic in self._publishers:
self._publishers[topic].unregister()
def stop(self):
for topic in self._publishing:
self.stop_publishing(topic)
def message_viewed(self, bag, msg_data):
topic, msg, t = msg_data
if topic not in self._publishers:
try:
self._publishers[topic] = rospy.Publisher(topic, type(msg))
except Exception, ex:
rospy.logerr('Error creating publisher on topic %s for type %s' % (topic, str(type(msg))))
self.stop_publishing(topic)
self._publishers[topic].publish(msg)
def message_cleared(self):
pass

View File

@ -0,0 +1,158 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$
PKG = 'rxbag'
import roslib; roslib.load_manifest(PKG)
import rospy
import sys
import threading
import time
import wx
import rosbag
import util.base_frame
import timeline_panel
class RxBagApp(wx.App):
def __init__(self, options, args):
self.options = options
self.args = args
self.connected_to_ros = False
wx.App.__init__(self)
def OnInit(self):
try:
# Get filename to record to
if self.options.record:
if not self.connect_to_ros():
raise Exception('recording requires connection to master')
# Get filename to record to
if len(self.args) > 0:
record_filename = self.args[0]
else:
record_filename = 'rxbag.bag'
# Title bar
if self.options.record:
frame_title = 'rxbag - %s [recording]' % record_filename
elif len(self.args) == 1:
frame_title = 'rxbag - ' + self.args[0]
else:
frame_title = 'rxbag - [%d bags]' % len(self.args)
# Create main timeline frame
frame = util.base_frame.BaseFrame(None, 'rxbag', 'Timeline', title=frame_title)
panel = timeline_panel.TimelinePanel(frame, -1)
panel.app = self
frame.Show()
self.SetTopWindow(frame)
if self.options.record:
panel.timeline.record_bag(record_filename)
else:
RxBagInitThread(self, panel.timeline)
except Exception, ex:
print >> sys.stderr, 'Error initializing application:', ex
raise
return False
return True
def connect_to_ros(self, init_timeout=3):
if self.connected_to_ros:
return True
# Attempt to connect to master node
class InitNodeThread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.setDaemon(True)
self.inited = False
self.init_cv = threading.Condition()
def run(self):
rospy.loginfo('Master found. Connecting...')
rospy.init_node('rxbag', anonymous=True, disable_signals=True)
self.init_cv.acquire()
self.inited = True
self.init_cv.notify_all()
self.init_cv.release()
try:
# Check whether ROS master is running
master = rospy.get_master()
master.getPid()
# If so (i.e. no exception), attempt to initialize node
init_thread = InitNodeThread()
init_thread.init_cv.acquire()
init_thread.start()
init_thread.init_cv.wait(init_timeout)
if init_thread.inited:
rospy.core.register_signals()
rospy.loginfo('Connected to ROS master.')
self.connected_to_ros = True
return True
else:
rospy.logerr('Couldn\'t connect to ROS master.')
except:
rospy.loginfo('ROS master not found.')
return False
class RxBagInitThread(threading.Thread):
def __init__(self, app, timeline):
threading.Thread.__init__(self)
self.app = app
self.timeline = timeline
self.start()
def run(self):
for input_file in self.app.args:
try:
bag = rosbag.Bag(input_file)
self.timeline.add_bag(bag)
except Exception, ex:
print >> sys.stderr, 'Error loading [%s]: %s' % (input_file, str(ex))

View File

@ -42,6 +42,7 @@ import sys
import threading
import time
# Ensure wxPython version >= 2.8, and install hotfix for 64-bit Cairo support
import wxversion
WXVER = '2.8'
if wxversion.checkInstalled(WXVER):
@ -57,129 +58,24 @@ if 'wxGTK' in wx.PlatformInfo:
gdkLib = wx.lib.wxcairo._findGDKLib()
gdkLib.gdk_cairo_create.restype = ctypes.c_void_p
import rosbag
import util.base_frame
import timeline_panel
class RxBagInitThread(threading.Thread):
def __init__(self, app, timeline):
threading.Thread.__init__(self)
self.app = app
self.timeline = timeline
self.start()
def run(self):
for input_file in self.app.args:
try:
bag = rosbag.Bag(input_file)
self.timeline.add_bag(bag)
except Exception, ex:
print >> sys.stderr, 'Error loading [%s]: %s' % (input_file, str(ex))
def connect_to_ros(node_name, init_timeout):
# Attempt to connect to master node
class InitNodeThread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.setDaemon(True)
self.inited = False
self.init_cv = threading.Condition()
def run(self):
rospy.loginfo('Master found. Connecting...')
rospy.init_node(node_name, anonymous=True, disable_signals=True)
self.init_cv.acquire()
self.inited = True
self.init_cv.notify_all()
self.init_cv.release()
try:
# Check whether ROS master is running
master = rospy.get_master()
master.getPid()
# If so (i.e. no exception), attempt to initialize node
init_thread = InitNodeThread()
init_thread.init_cv.acquire()
init_thread.start()
init_thread.init_cv.wait(init_timeout)
if init_thread.inited:
rospy.core.register_signals()
rospy.loginfo('Connected to ROS master.')
return True
else:
rospy.logerr('Couldn\'t connect to ROS master.')
except:
rospy.loginfo('ROS master not found.')
return False
class RxBagApp(wx.App):
def __init__(self, options, args):
self.options = options
self.args = args
wx.App.__init__(self)
import rxbag_app
def OnInit(self):
try:
# Get filename to record to
if self.options.record:
if not connect_to_ros('rxbag', 3):
raise Exception('recording requires connection to master')
def run(options, args):
app = rxbag_app.RxBagApp(options, args)
app.MainLoop()
# Get filename to record to
if len(self.args) > 0:
record_filename = self.args[0]
else:
record_filename = 'rxbag.bag'
# Title bar
if self.options.record:
frame_title = 'rxbag - %s [recording]' % record_filename
elif len(self.args) == 1:
frame_title = 'rxbag - ' + self.args[0]
else:
frame_title = 'rxbag - [%d bags]' % len(self.args)
# Create main timeline frame
frame = util.base_frame.BaseFrame(None, 'rxbag', 'Timeline', title=frame_title)
panel = timeline_panel.TimelinePanel(frame, -1)
frame.Show()
self.SetTopWindow(frame)
if self.options.record:
panel.timeline.record_bag(record_filename)
else:
RxBagInitThread(self, panel.timeline)
except Exception, ex:
print >> sys.stderr, 'Error initializing application:', ex
raise
return False
return True
rospy.signal_shutdown('GUI shutdown')
def rxbag_main():
# Parse command line for input files and options
usage = "usage: %prog [options] BAG_FILE1 [BAG_FILE2 ...]"
parser = optparse.OptionParser(usage=usage)
parser.add_option('-r', '--record', dest='record', default=False, action='store_true', help='record to a bag file')
parser.add_option( '--profile', dest='profile', default=False, action='store_true', help='profile and write results to rxbag.prof')
parser.add_option( '--profile', dest='profile', default=False, action='store_true', help='profile and write results to rxbag.prof [advanced]')
options, args = parser.parse_args(sys.argv[1:])
if options.profile:
import cProfile
cProfile.runctx('run(options, args)', globals(), locals(), 'rxbag.prof')
else:
run(options, args)
def run(options, args):
app = RxBagApp(options, args)
app.MainLoop()
rospy.signal_shutdown('GUI shutdown')

View File

@ -55,6 +55,7 @@ import wx
import bag_helper
import plugins
from player import Player
from raw_view import RawView
from recorder import Recorder
from util.layer import Layer
@ -172,7 +173,7 @@ class PlayThread(threading.Thread):
class Timeline(Layer):
name = 'Timeline'
def __init__(self, parent, title, x, y, width, height):
Layer.__init__(self, parent, title, x, y, width, height)
@ -253,8 +254,9 @@ class Timeline(Layer):
self.index_cache_lock = threading.RLock()
self.index_cache = {}
self.invalidated_caches = set()
self.recorder = None
self.player = False
##
@ -263,6 +265,15 @@ class Timeline(Layer):
###
def on_close(self, event):
self.index_cache_thread.stop()
self.play_thread.stop()
if self.player:
self.player.stop()
if self.recorder:
self.recorder.stop()
self.recorder.join() # wait for recorder to stop until exiting app (ensures bag is closed gracefully)
def get_next_message_time(self):
if self.playhead is None:
return None
@ -273,6 +284,38 @@ class Timeline(Layer):
return entry.time
def toggle_play_all(self):
self.play_all = not self.play_all
### Publishing
def is_publishing(self, topic):
return self.player and self.player.is_publishing(topic)
def start_publishing(self, topic):
if not self.player and not self._create_player():
return False
self.player.start_publishing(topic)
return True
def stop_publishing(self, topic):
if not self.player:
return False
self.player.stop_publishing(topic)
return True
def _create_player(self):
if not self.player:
try:
self.player = Player(self)
except Exception, ex:
print >> sys.stderr, 'Error starting player; aborting publish: %s' % str(ex)
return False
return True
@property
def play_speed(self):
if self._paused:
@ -411,16 +454,6 @@ class Timeline(Layer):
with self._bag_lock:
return bag._read_message(position)
##
def on_close(self, event):
self.index_cache_thread.stop()
self.play_thread.stop()
if self.recorder:
self.recorder.stop()
self.recorder.join()
### Views / listeners
def add_view(self, topic, view):
@ -612,6 +645,8 @@ class Timeline(Layer):
self._stamp_left -= dstamp
self._stamp_right -= dstamp
self.parent.playhead.update_position()
self._playhead_positions = {}
for topic in self.topics:
bag, entry = self.get_entry(self._playhead, topic)
@ -619,9 +654,7 @@ class Timeline(Layer):
self._playhead_positions[topic] = (bag, entry.position)
self._update_message_view()
self.parent.playhead.update_position()
self.invalidate()
### Rendering
@ -1076,7 +1109,7 @@ class Timeline(Layer):
# Inform the listeners
for topic in self.topics:
topic_listeners = self.listeners.get(topic)
topic_listeners = self.listeners.get(topic)
if not topic_listeners:
continue

View File

@ -132,7 +132,7 @@ class TimelinePopupMenu(wx.Menu):
# ---
self.thumbnail_menu.AppendSeparator()
# Thumbnails... / topic/subtopic/subsubtopic
# Thumbnails... / topic
for topic, renderer in renderers:
renderer_item = self.TimelineRendererMenuItem(self.thumbnail_menu, wx.NewId(), topic.lstrip('/'), topic, renderer, self.timeline)
self.thumbnail_menu.AppendItem(renderer_item)
@ -146,13 +146,13 @@ class TimelinePopupMenu(wx.Menu):
for topic in self.timeline.topics:
datatype = self.timeline.get_datatype(topic)
# View... / topic/subtopic/subsubtopic
# View... / topic
topic_menu = wx.Menu()
self.view_topic_menu.AppendSubMenu(topic_menu, topic.lstrip('/'), topic)
viewer_types = self.timeline.get_viewer_types(datatype)
# View... / topic/subtopic/subsubtopic / Viewer
# View... / topic / Viewer
for viewer_type in viewer_types:
topic_menu.AppendItem(self.TopicViewMenuItem(topic_menu, wx.NewId(), viewer_type.name, topic, viewer_type, self.timeline))
@ -175,17 +175,28 @@ class TimelinePopupMenu(wx.Menu):
topic_menu = wx.Menu()
datatype_menu.AppendSubMenu(topic_menu, topic.lstrip('/'), topic)
# View... / datatype / topic/subtopic/subsubtopic / Viewer
# View... / datatype / topic / Viewer
for viewer_type in viewer_types:
topic_menu.AppendItem(self.TopicViewMenuItem(topic_menu, wx.NewId(), viewer_type.name, topic, viewer_type, self.timeline))
# ---
self.AppendSeparator()
# Play All Messages
self.play_all_menu = self.PlayAllMenuItem(self, wx.NewId(), 'Play all messages', self.timeline)
# Play all messages
self.play_all_menu = wx.MenuItem(self, wx.NewId(), 'Play all messages', kind=wx.ITEM_CHECK)
self.AppendItem(self.play_all_menu)
self.play_all_menu.Check(self.timeline.play_all)
self.Bind(wx.EVT_MENU, lambda e: self.timeline.toggle_play_all(), id=self.play_all_menu.GetId())
# Publish...
self.publish_menu = wx.Menu()
self.AppendSubMenu(self.publish_menu, 'Publish...', 'Publish messages')
for topic in self.timeline.topics:
# Publish... / topic
publish_topic_menu = self.PublishTopicMenuItem(self.publish_menu, wx.NewId(), topic, self.timeline)
self.publish_menu.AppendItem(publish_topic_menu)
publish_topic_menu.Check(self.timeline.is_publishing(topic))
class TimelineRendererMenuItem(wx.MenuItem):
def __init__(self, parent, id, label, topic, renderer, timeline):
@ -229,3 +240,18 @@ class TimelinePopupMenu(wx.Menu):
def on_menu(self, event):
self.timeline.play_all = not self.timeline.play_all
class PublishTopicMenuItem(wx.MenuItem):
def __init__(self, parent, id, topic, timeline):
wx.MenuItem.__init__(self, parent, id, topic.lstrip('/'), kind=wx.ITEM_CHECK)
self.topic = topic
self.timeline = timeline
parent.Bind(wx.EVT_MENU, self.on_menu, id=self.GetId())
def on_menu(self, event):
if self.timeline.is_publishing(self.topic):
self.timeline.stop_publishing(self.topic)
else:
self.timeline.start_publishing(self.topic)