publisher.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright (c) 2011, Dorian Scholz, TU Darmstadt
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 the TU Darmstadt 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 HOLDER 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 from __future__ import division
34 import math
35 import random
36 import time
37 
38 from python_qt_binding.QtCore import Slot, QSignalMapper, QTimer, qWarning
39 
40 import roslib
41 import rospy
42 import genpy
43 from rqt_gui_py.plugin import Plugin
44 from .publisher_widget import PublisherWidget
45 from rqt_py_common.topic_helpers import get_field_type
46 
47 
49 
50  def __init__(self, context):
51  super(Publisher, self).__init__(context)
52  self.setObjectName('Publisher')
53 
54  # create widget
55  self._widget = PublisherWidget()
56  self._widget.add_publisher.connect(self.add_publisher)
57  self._widget.change_publisher.connect(self.change_publisher)
58  self._widget.publish_once.connect(self.publish_once)
59  self._widget.remove_publisher.connect(self.remove_publisher)
60  self._widget.clean_up_publishers.connect(self.clean_up_publishers)
61  if context.serial_number() > 1:
62  self._widget.setWindowTitle(
63  self._widget.windowTitle() + (' (%d)' % context.serial_number()))
64 
65  # create context for the expression eval statement
66  self._eval_locals = {'i': 0}
67  for module in (math, random, time):
68  self._eval_locals.update(module.__dict__)
69  self._eval_locals['genpy'] = genpy
70  del self._eval_locals['__name__']
71  del self._eval_locals['__doc__']
72 
73  self._publishers = {}
74  self._id_counter = 0
75 
76  self._timeout_mapper = QSignalMapper(self)
77  self._timeout_mapper.mapped[int].connect(self.publish_once)
78 
79  # add our self to the main window
80  context.add_widget(self._widget)
81 
82  @Slot(str, str, float, bool)
83  def add_publisher(self, topic_name, type_name, rate, enabled):
84  publisher_info = {
85  'topic_name': str(topic_name),
86  'type_name': str(type_name),
87  'rate': float(rate),
88  'enabled': bool(enabled),
89  }
90  self._add_publisher(publisher_info)
91 
92  def _add_publisher(self, publisher_info):
93  publisher_info['publisher_id'] = self._id_counter
94  self._id_counter += 1
95  publisher_info['counter'] = 0
96  publisher_info['enabled'] = publisher_info.get('enabled', False)
97  publisher_info['expressions'] = publisher_info.get('expressions', {})
98 
99  publisher_info['message_instance'] = self._create_message_instance(
100  publisher_info['type_name'])
101  if publisher_info['message_instance'] is None:
102  return
103 
104  # create publisher and timer
105  try:
106  publisher_info['publisher'] = rospy.Publisher(
107  publisher_info['topic_name'], type(publisher_info['message_instance']), queue_size=100)
108  except TypeError:
109  publisher_info['publisher'] = rospy.Publisher(
110  publisher_info['topic_name'], type(publisher_info['message_instance']))
111  publisher_info['timer'] = QTimer(self)
112 
113  # add publisher info to _publishers dict and create signal mapping
114  self._publishers[publisher_info['publisher_id']] = publisher_info
115  self._timeout_mapper.setMapping(publisher_info['timer'], publisher_info['publisher_id'])
116  publisher_info['timer'].timeout.connect(self._timeout_mapper.map)
117  if publisher_info['enabled'] and publisher_info['rate'] > 0:
118  publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
119 
120  self._widget.publisher_tree_widget.model().add_publisher(publisher_info)
121 
122  @Slot(int, str, str, str, object)
123  def change_publisher(self, publisher_id, topic_name, column_name, new_value, setter_callback):
124  handler = getattr(self, '_change_publisher_%s' % column_name, None)
125  if handler is not None:
126  new_text = handler(self._publishers[publisher_id], topic_name, new_value)
127  if new_text is not None:
128  setter_callback(new_text)
129 
130  def _change_publisher_topic(self, publisher_info, topic_name, new_value):
131  publisher_info['enabled'] = (new_value and new_value.lower() in ['1', 'true', 'yes'])
132  # qDebug(
133  # 'Publisher._change_publisher_enabled(): %s enabled: %s' %
134  # (publisher_info['topic_name'], publisher_info['enabled']))
135  if publisher_info['enabled'] and publisher_info['rate'] > 0:
136  publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
137  else:
138  publisher_info['timer'].stop()
139  return None
140 
141  def _change_publisher_type(self, publisher_info, topic_name, new_value):
142  type_name = new_value
143  # create new slot
144  slot_value = self._create_message_instance(type_name)
145 
146  # find parent slot
147  slot_path = topic_name[len(publisher_info['topic_name']):].strip('/').split('/')
148  parent_slot = eval('.'.join(["publisher_info['message_instance']"] + slot_path[:-1]))
149 
150  # find old slot
151  slot_name = slot_path[-1]
152  slot_index = parent_slot.__slots__.index(slot_name)
153 
154  # restore type if user value was invalid
155  if slot_value is None:
156  qWarning('Publisher._change_publisher_type(): could not find type: %s' % (type_name))
157  return parent_slot._slot_types[slot_index]
158 
159  else:
160  # replace old slot
161  parent_slot._slot_types[slot_index] = type_name
162  setattr(parent_slot, slot_name, slot_value)
163 
164  self._widget.publisher_tree_widget.model().update_publisher(publisher_info)
165 
166  def _change_publisher_rate(self, publisher_info, topic_name, new_value):
167  try:
168  rate = float(new_value)
169  except Exception:
170  qWarning('Publisher._change_publisher_rate(): could not parse rate value: %s' %
171  (new_value))
172  else:
173  publisher_info['rate'] = rate
174  # qDebug(
175  # 'Publisher._change_publisher_rate(): %s rate changed: %fHz' %
176  # (publisher_info['topic_name'], publisher_info['rate']))
177  publisher_info['timer'].stop()
178  if publisher_info['enabled'] and publisher_info['rate'] > 0:
179  publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
180  # make sure the column value reflects the actual rate
181  return '%.2f' % publisher_info['rate']
182 
183  def _change_publisher_expression(self, publisher_info, topic_name, new_value):
184  expression = str(new_value)
185  if len(expression) == 0:
186  if topic_name in publisher_info['expressions']:
187  del publisher_info['expressions'][topic_name]
188  # qDebug(
189  # 'Publisher._change_publisher_expression(): removed expression'
190  # 'for: %s' % (topic_name))
191  else:
192  slot_type, is_array = get_field_type(topic_name)
193  if is_array:
194  slot_type = list
195  # strip possible trailing error message from expression
196  error_prefix = '# error'
197  error_prefix_pos = expression.find(error_prefix)
198  if error_prefix_pos >= 0:
199  expression = expression[:error_prefix_pos]
200  success, _ = self._evaluate_expression(expression, slot_type)
201  if success:
202  old_expression = publisher_info['expressions'].get(topic_name, None)
203  publisher_info['expressions'][topic_name] = expression
204  # print('Publisher._change_publisher_expression(): topic: %s, type: %s,'
205  # 'expression: %s') % (topic_name, slot_type, new_value)
206  self._fill_message_slots(
207  publisher_info['message_instance'], publisher_info['topic_name'],
208  publisher_info['expressions'], publisher_info['counter'])
209  try:
210  publisher_info['message_instance']._check_types()
211  except Exception as e:
212  print('serialization error: %s' % e)
213  if old_expression is not None:
214  publisher_info['expressions'][topic_name] = old_expression
215  else:
216  del publisher_info['expressions'][topic_name]
217  return '%s %s: %s' % (expression, error_prefix, e)
218  return expression
219  else:
220  return '%s %s evaluating as "%s"' % (expression, error_prefix, slot_type.__name__)
221 
222  def _extract_array_info(self, type_str):
223  array_size = None
224  if '[' in type_str and type_str[-1] == ']':
225  type_str, array_size_str = type_str.split('[', 1)
226  array_size_str = array_size_str[:-1]
227  if len(array_size_str) > 0:
228  array_size = int(array_size_str)
229  else:
230  array_size = 0
231 
232  return type_str, array_size
233 
234  def _create_message_instance(self, type_str):
235  base_type_str, array_size = self._extract_array_info(type_str)
236 
237  base_message_type = roslib.message.get_message_class(base_type_str)
238  if base_message_type is None:
239  print('Could not create message of type "%s".' % base_type_str)
240  return None
241 
242  if array_size is not None:
243  message = []
244  for _ in range(array_size):
245  message.append(base_message_type())
246  else:
247  message = base_message_type()
248  return message
249 
250  def _evaluate_expression(self, expression, slot_type):
251  successful_eval = True
252 
253  try:
254  # try to evaluate expression
255  value = eval(expression, {}, self._eval_locals)
256  except Exception:
257  successful_eval = False
258 
259  if slot_type is str:
260  if successful_eval:
261  value = str(value)
262  else:
263  # for string slots just convert the expression to str, if it did not
264  # evaluate successfully
265  value = str(expression)
266  successful_eval = True
267 
268  elif successful_eval:
269  type_set = set((slot_type, type(value)))
270  # check if value's type and slot_type belong to the same type group, i.e. array types,
271  # numeric types and if they do, make sure values's type is converted to the exact
272  # slot_type
273  if type_set <= set((list, tuple)) or type_set <= set((int, float)):
274  # convert to the right type
275  value = slot_type(value)
276 
277  if successful_eval and isinstance(value, slot_type):
278  return True, value
279  else:
280  qWarning(
281  'Publisher._evaluate_expression(): failed to evaluate expression: "%s" as '
282  'Python type "%s"' % (expression, slot_type.__name__))
283 
284  return False, None
285 
286  def _fill_message_slots(self, message, topic_name, expressions, counter):
287  if topic_name in expressions and len(expressions[topic_name]) > 0:
288 
289  # get type
290  if hasattr(message, '_type'):
291  message_type = message._type
292  else:
293  message_type = type(message)
294 
295  self._eval_locals['i'] = counter
296  success, value = self._evaluate_expression(expressions[topic_name], message_type)
297  if not success:
298  value = message_type()
299  return value
300 
301  # if no expression exists for this topic_name, continue with it's child slots
302  elif hasattr(message, '__slots__'):
303  for slot_name in message.__slots__:
304  value = self._fill_message_slots(
305  getattr(message, slot_name), topic_name + '/' + slot_name, expressions, counter)
306  if value is not None:
307  setattr(message, slot_name, value)
308 
309  elif type(message) in (list, tuple) and (len(message) > 0):
310  for index, slot in enumerate(message):
311  value = self._fill_message_slots(
312  slot, topic_name + '[%d]' % index, expressions, counter)
313  # this deals with primitive-type arrays
314  if not hasattr(message[0], '__slots__') and value is not None:
315  message[index] = value
316 
317  return None
318 
319  @Slot(int)
320  def publish_once(self, publisher_id):
321  publisher_info = self._publishers.get(publisher_id, None)
322  if publisher_info is not None:
323  publisher_info['counter'] += 1
324  self._fill_message_slots(
325  publisher_info['message_instance'],
326  publisher_info['topic_name'],
327  publisher_info['expressions'],
328  publisher_info['counter'])
329  publisher_info['publisher'].publish(publisher_info['message_instance'])
330 
331  @Slot(int)
332  def remove_publisher(self, publisher_id):
333  publisher_info = self._publishers.get(publisher_id, None)
334  if publisher_info is not None:
335  publisher_info['timer'].stop()
336  publisher_info['publisher'].unregister()
337  del self._publishers[publisher_id]
338 
339  def save_settings(self, plugin_settings, instance_settings):
340  publisher_copies = []
341  for publisher in self._publishers.values():
342  publisher_copy = {}
343  publisher_copy.update(publisher)
344  publisher_copy['enabled'] = False
345  del publisher_copy['timer']
346  del publisher_copy['message_instance']
347  del publisher_copy['publisher']
348  publisher_copies.append(publisher_copy)
349  instance_settings.set_value('publishers', repr(publisher_copies))
350 
351  def restore_settings(self, plugin_settings, instance_settings):
352  publishers = eval(instance_settings.value('publishers', '[]'))
353  for publisher in publishers:
354  self._add_publisher(publisher)
355 
357  self._widget.publisher_tree_widget.model().clear()
358  for publisher_info in self._publishers.values():
359  publisher_info['timer'].stop()
360  publisher_info['publisher'].unregister()
361  self._publishers = {}
362 
363  def shutdown_plugin(self):
364  self._widget.shutdown_plugin()
365  self.clean_up_publishers()
def publish_once(self, publisher_id)
Definition: publisher.py:320
def _create_message_instance(self, type_str)
Definition: publisher.py:234
def _change_publisher_topic(self, publisher_info, topic_name, new_value)
Definition: publisher.py:130
def restore_settings(self, plugin_settings, instance_settings)
Definition: publisher.py:351
def _change_publisher_type(self, publisher_info, topic_name, new_value)
Definition: publisher.py:141
def save_settings(self, plugin_settings, instance_settings)
Definition: publisher.py:339
def __init__(self, context)
Definition: publisher.py:50
def _evaluate_expression(self, expression, slot_type)
Definition: publisher.py:250
def add_publisher(self, topic_name, type_name, rate, enabled)
Definition: publisher.py:83
def _change_publisher_expression(self, publisher_info, topic_name, new_value)
Definition: publisher.py:183
def change_publisher(self, publisher_id, topic_name, column_name, new_value, setter_callback)
Definition: publisher.py:123
def _change_publisher_rate(self, publisher_info, topic_name, new_value)
Definition: publisher.py:166
def _extract_array_info(self, type_str)
Definition: publisher.py:222
def _fill_message_slots(self, message, topic_name, expressions, counter)
Definition: publisher.py:286
def remove_publisher(self, publisher_id)
Definition: publisher.py:332
def _add_publisher(self, publisher_info)
Definition: publisher.py:92


rqt_publisher
Author(s): Dorian Scholz
autogenerated on Thu Jun 6 2019 19:28:21