player.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2012, 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 import rospy
00038 import rosgraph_msgs
00039 
00040 from python_qt_binding.QtCore import QObject
00041 
00042 CLOCK_TOPIC = "/clock"
00043 
00044 class Player(QObject):
00045     """
00046     This object handles publishing messages as the playhead passes over their position
00047     """
00048     def __init__(self, timeline):
00049         super(Player, self).__init__()
00050         self.timeline = timeline
00051 
00052         self._publishing = set()
00053         self._publishers = {}
00054 
00055         self._publish_clock = False
00056         self._last_clock = rosgraph_msgs.msg.Clock()
00057         self._resume = False
00058 
00059     def resume(self):
00060         self._resume = True
00061 
00062     def is_publishing(self, topic):
00063         return topic in self._publishing
00064 
00065     def start_publishing(self, topic):
00066         if topic in self._publishing:
00067             return
00068         self._publishing.add(topic)
00069         self.timeline.add_listener(topic, self)
00070 
00071     def stop_publishing(self, topic):
00072         if topic not in self._publishing:
00073             return
00074         self.timeline.remove_listener(topic, self)
00075 
00076         if topic in self._publishers:
00077             self._publishers[topic].unregister()
00078             del self._publishers[topic]
00079 
00080         self._publishing.remove(topic)
00081 
00082     def start_clock_publishing(self):
00083         if CLOCK_TOPIC not in self._publishers:
00084             # Activate clock publishing only if the publisher was created successful
00085             self._publish_clock = self.create_publisher(CLOCK_TOPIC, rosgraph_msgs.msg.Clock())
00086 
00087     def stop_clock_publishing(self):
00088         self._publish_clock = False
00089         if CLOCK_TOPIC in self._publishers:
00090             self._publishers[CLOCK_TOPIC].unregister()
00091             del self._publishers[CLOCK_TOPIC]
00092 
00093     def stop(self):
00094         for topic in list(self._publishing):
00095             self.stop_publishing(topic)
00096         self.stop_clock_publishing()
00097 
00098     def create_publisher(self, topic, msg):
00099         try:
00100             try:
00101                 self._publishers[topic] = rospy.Publisher(topic, type(msg), queue_size=100)
00102             except TypeError:
00103                 self._publishers[topic] = rospy.Publisher(topic, type(msg))
00104             return True
00105         except Exception as ex:
00106             # Any errors, stop listening/publishing to this topic
00107             rospy.logerr('Error creating publisher on topic %s for type %s. \nError text: %s' % (topic, str(type(msg)), str(ex)))
00108             if topic != CLOCK_TOPIC:
00109                 self.stop_publishing(topic)
00110             return False
00111 
00112     def message_viewed(self, bag, msg_data):
00113         """
00114         When a message is viewed publish it
00115         :param bag: the bag the message is in, ''rosbag.bag''
00116         :param msg_data: tuple of the message data and topic info, ''(str, msg)''
00117         """
00118         # Don't publish unless the playhead is moving.
00119         if self.timeline.play_speed <= 0.0:
00120             return
00121 
00122         topic, msg, clock = msg_data
00123 
00124         # Create publisher if this is the first message on the topic
00125         if topic not in self._publishers:
00126             self.create_publisher(topic, msg)
00127 
00128         if self._publish_clock:
00129             time_msg = rosgraph_msgs.msg.Clock()
00130             time_msg.clock = clock
00131             if self._resume or self._last_clock.clock < time_msg.clock:
00132                 self._resume = False
00133                 self._last_clock = time_msg
00134                 self._publishers[CLOCK_TOPIC].publish(time_msg)
00135         self._publishers[topic].publish(msg)
00136 
00137     def message_cleared(self):
00138         pass
00139 
00140     def event(self, event):
00141         """
00142         This function will be called to process events posted by post_event
00143         it will call message_cleared or message_viewed with the relevant data
00144         """
00145         bag, msg_data = event.data
00146         if msg_data:
00147             self.message_viewed(bag, msg_data)
00148         else:
00149             self.message_cleared()
00150         return True


rqt_bag
Author(s): Aaron Blasdel, Tim Field
autogenerated on Thu Aug 17 2017 02:19:27