player.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2012, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 
33 """
34 Player listens to messages from the timeline and publishes them to ROS.
35 """
36 
37 import rospy
38 import rosgraph_msgs
39 
40 from python_qt_binding.QtCore import QObject
41 
42 CLOCK_TOPIC = "/clock"
43 
44 
45 class Player(QObject):
46 
47  """
48  This object handles publishing messages as the playhead passes over their position
49  """
50 
51  def __init__(self, timeline):
52  super(Player, self).__init__()
53  self.timeline = timeline
54 
55  self._publishing = set()
56  self._publishers = {}
57 
58  self._publish_clock = False
59  self._last_clock = rosgraph_msgs.msg.Clock()
60  self._resume = False
61 
62  def resume(self):
63  self._resume = True
64 
65  def is_publishing(self, topic):
66  return topic in self._publishing
67 
68  def start_publishing(self, topic):
69  if topic in self._publishing:
70  return
71  self._publishing.add(topic)
72  self.timeline.add_listener(topic, self)
73 
74  def stop_publishing(self, topic):
75  if topic not in self._publishing:
76  return
77  self.timeline.remove_listener(topic, self)
78 
79  if topic in self._publishers:
80  self._publishers[topic].unregister()
81  del self._publishers[topic]
82 
83  self._publishing.remove(topic)
84 
86  if CLOCK_TOPIC not in self._publishers:
87  # Activate clock publishing only if the publisher was created successful
88  self._publish_clock = self.create_publisher(CLOCK_TOPIC, rosgraph_msgs.msg.Clock())
89 
91  self._publish_clock = False
92  if CLOCK_TOPIC in self._publishers:
93  self._publishers[CLOCK_TOPIC].unregister()
94  del self._publishers[CLOCK_TOPIC]
95 
96  def stop(self):
97  for topic in list(self._publishing):
98  self.stop_publishing(topic)
100 
101  def create_publisher(self, topic, msg):
102  try:
103  try:
104  self._publishers[topic] = rospy.Publisher(topic, type(msg), queue_size=100)
105  except TypeError:
106  self._publishers[topic] = rospy.Publisher(topic, type(msg))
107  return True
108  except Exception as ex:
109  # Any errors, stop listening/publishing to this topic
110  rospy.logerr('Error creating publisher on topic %s for type %s. \nError text: %s' %
111  (topic, str(type(msg)), str(ex)))
112  if topic != CLOCK_TOPIC:
113  self.stop_publishing(topic)
114  return False
115 
116  def message_viewed(self, bag, msg_data):
117  """
118  When a message is viewed publish it
119  :param bag: the bag the message is in, ''rosbag.bag''
120  :param msg_data: tuple of the message data and topic info, ''(str, msg)''
121  """
122  # Don't publish unless the playhead is moving.
123  if self.timeline.play_speed <= 0.0:
124  return
125 
126  topic, msg, clock = msg_data
127 
128  # Create publisher if this is the first message on the topic
129  if topic not in self._publishers:
130  self.create_publisher(topic, msg)
131 
132  if self._publish_clock:
133  time_msg = rosgraph_msgs.msg.Clock()
134  time_msg.clock = clock
135  if self._resume or self._last_clock.clock < time_msg.clock:
136  self._resume = False
137  self._last_clock = time_msg
138  self._publishers[CLOCK_TOPIC].publish(time_msg)
139  self._publishers[topic].publish(msg)
140 
141  def message_cleared(self):
142  pass
143 
144  def event(self, event):
145  """
146  This function will be called to process events posted by post_event
147  it will call message_cleared or message_viewed with the relevant data
148  """
149  bag, msg_data = event.data
150  if msg_data:
151  self.message_viewed(bag, msg_data)
152  else:
153  self.message_cleared()
154  return True
def start_clock_publishing(self)
Definition: player.py:85
def start_publishing(self, topic)
Definition: player.py:68
def is_publishing(self, topic)
Definition: player.py:65
def create_publisher(self, topic, msg)
Definition: player.py:101
def stop(self)
Definition: player.py:96
def message_cleared(self)
Definition: player.py:141
def message_viewed(self, bag, msg_data)
Definition: player.py:116
def event(self, event)
Definition: player.py:144
def __init__(self, timeline)
Definition: player.py:51
def stop_publishing(self, topic)
Definition: player.py:74
def resume(self)
Definition: player.py:62
def stop_clock_publishing(self)
Definition: player.py:90


rqt_bag
Author(s): Aaron Blasdel, Tim Field
autogenerated on Fri Jun 7 2019 22:05:54