$search
00001 # Software License Agreement (BSD License) 00002 # 00003 # Copyright (c) 2009, Willow Garage, Inc. 00004 # All rights reserved. 00005 # 00006 # Redistribution and use in source and binary forms, with or without 00007 # modification, are permitted provided that the following conditions 00008 # are met: 00009 # 00010 # * Redistributions of source code must retain the above copyright 00011 # notice, this list of conditions and the following disclaimer. 00012 # * Redistributions in binary form must reproduce the above 00013 # copyright notice, this list of conditions and the following 00014 # disclaimer in the documentation and/or other materials provided 00015 # with the distribution. 00016 # * Neither the name of Willow Garage, Inc. nor the names of its 00017 # contributors may be used to endorse or promote products derived 00018 # from this software without specific prior written permission. 00019 # 00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 # POSSIBILITY OF SUCH DAMAGE. 00032 00033 """ 00034 Player listens to messages from the timeline and publishes them to ROS. 00035 """ 00036 00037 PKG = 'rxbag' 00038 import roslib; roslib.load_manifest(PKG) 00039 00040 import wx 00041 00042 import rosbag 00043 import rosgraph.masterapi 00044 import rospy 00045 00046 class Player(object): 00047 def __init__(self, timeline): 00048 self.timeline = timeline 00049 00050 self._publishing = set() 00051 self._publishers = {} 00052 00053 # Attempt to connect to the ROS master 00054 if not wx.GetApp().connect_to_ros(): 00055 raise Exception('Error connecting to ROS') 00056 00057 def is_publishing(self, topic): 00058 return topic in self._publishing 00059 00060 def start_publishing(self, topic): 00061 if topic in self._publishing: 00062 return 00063 00064 self._publishing.add(topic) 00065 00066 self.timeline.add_listener(topic, self) 00067 00068 def stop_publishing(self, topic): 00069 if topic not in self._publishing: 00070 return 00071 00072 self.timeline.remove_listener(topic, self) 00073 00074 if topic in self._publishers: 00075 self._publishers[topic].unregister() 00076 del self._publishers[topic] 00077 00078 self._publishing.remove(topic) 00079 00080 def stop(self): 00081 for topic in list(self._publishing): 00082 self.stop_publishing(topic) 00083 00084 ## 00085 00086 def message_viewed(self, bag, msg_data): 00087 # Don't publish unless the playhead is moving. 00088 if self.timeline.play_speed <= 0.0: 00089 return 00090 00091 topic, msg, t = msg_data 00092 00093 # Create publisher if this is the first message on the topic 00094 if topic not in self._publishers: 00095 try: 00096 self._publishers[topic] = rospy.Publisher(topic, type(msg)) 00097 except Exception, ex: 00098 # Any errors, stop listening/publishing to this topic 00099 rospy.logerr('Error creating publisher on topic %s for type %s' % (topic, str(type(msg)))) 00100 self.stop_publishing(topic) 00101 00102 self._publishers[topic].publish(msg) 00103 00104 def message_cleared(self): 00105 pass