rqt_publisher: publisher.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2011, Dorian Scholz, TU Darmstadt
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 the TU Darmstadt 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 HOLDER 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 from __future__ import division
00034 import math
00035 import random
00036 import time
00037 
00038 from python_qt_binding.QtCore import Slot, QSignalMapper, QTimer, qWarning
00039 
00040 import roslib
00041 import rospy
00042 import genpy
00043 from rqt_gui_py.plugin import Plugin
00044 from .publisher_widget import PublisherWidget
00045 from rqt_py_common.topic_helpers import get_field_type
00046 
00047 
00048 class Publisher(Plugin):
00049 
00050     def __init__(self, context):
00051         super(Publisher, self).__init__(context)
00052         self.setObjectName('Publisher')
00053 
00054         # create widget
00055         self._widget = PublisherWidget()
00056         self._widget.add_publisher.connect(self.add_publisher)
00057         self._widget.change_publisher.connect(self.change_publisher)
00058         self._widget.publish_once.connect(self.publish_once)
00059         self._widget.remove_publisher.connect(self.remove_publisher)
00060         self._widget.clean_up_publishers.connect(self.clean_up_publishers)
00061         if context.serial_number() > 1:
00062             self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
00063 
00064         # create context for the expression eval statement
00065         self._eval_locals = {'i': 0}
00066         for module in (math, random, time):
00067             self._eval_locals.update(module.__dict__)
00068         self._eval_locals['genpy'] = genpy
00069         del self._eval_locals['__name__']
00070         del self._eval_locals['__doc__']
00071 
00072         self._publishers = {}
00073         self._id_counter = 0
00074 
00075         self._timeout_mapper = QSignalMapper(self)
00076         self._timeout_mapper.mapped[int].connect(self.publish_once)
00077 
00078         # add our self to the main window
00079         context.add_widget(self._widget)
00080 
00081     @Slot(str, str, float, bool)
00082     def add_publisher(self, topic_name, type_name, rate, enabled):
00083         publisher_info = {
00084             'topic_name': str(topic_name),
00085             'type_name': str(type_name),
00086             'rate': float(rate),
00087             'enabled': bool(enabled),
00088         }
00089         self._add_publisher(publisher_info)
00090 
00091     def _add_publisher(self, publisher_info):
00092         publisher_info['publisher_id'] = self._id_counter
00093         self._id_counter += 1
00094         publisher_info['counter'] = 0
00095         publisher_info['enabled'] = publisher_info.get('enabled', False)
00096         publisher_info['expressions'] = publisher_info.get('expressions', {})
00097 
00098         publisher_info['message_instance'] = self._create_message_instance(publisher_info['type_name'])
00099         if publisher_info['message_instance'] is None:
00100             return
00101 
00102         # create publisher and timer
00103         try:
00104             publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance']), queue_size=100)
00105         except TypeError:
00106             publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance']))
00107         publisher_info['timer'] = QTimer(self)
00108 
00109         # add publisher info to _publishers dict and create signal mapping
00110         self._publishers[publisher_info['publisher_id']] = publisher_info
00111         self._timeout_mapper.setMapping(publisher_info['timer'], publisher_info['publisher_id'])
00112         publisher_info['timer'].timeout.connect(self._timeout_mapper.map)
00113         if publisher_info['enabled'] and publisher_info['rate'] > 0:
00114             publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
00115 
00116         self._widget.publisher_tree_widget.model().add_publisher(publisher_info)
00117 
00118     @Slot(int, str, str, str, object)
00119     def change_publisher(self, publisher_id, topic_name, column_name, new_value, setter_callback):
00120         handler = getattr(self, '_change_publisher_%s' % column_name, None)
00121         if handler is not None:
00122             new_text = handler(self._publishers[publisher_id], topic_name, new_value)
00123             if new_text is not None:
00124                 setter_callback(new_text)
00125 
00126     def _change_publisher_topic(self, publisher_info, topic_name, new_value):
00127         publisher_info['enabled'] = (new_value and new_value.lower() in ['1', 'true', 'yes'])
00128         #qDebug('Publisher._change_publisher_enabled(): %s enabled: %s' % (publisher_info['topic_name'], publisher_info['enabled']))
00129         if publisher_info['enabled'] and publisher_info['rate'] > 0:
00130             publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
00131         else:
00132             publisher_info['timer'].stop()
00133         return None
00134 
00135     def _change_publisher_type(self, publisher_info, topic_name, new_value):
00136         type_name = new_value
00137         # create new slot
00138         slot_value = self._create_message_instance(type_name)
00139 
00140         # find parent slot
00141         slot_path = topic_name[len(publisher_info['topic_name']):].strip('/').split('/')
00142         parent_slot = eval('.'.join(["publisher_info['message_instance']"] + slot_path[:-1]))
00143 
00144         # find old slot
00145         slot_name = slot_path[-1]
00146         slot_index = parent_slot.__slots__.index(slot_name)
00147 
00148         # restore type if user value was invalid
00149         if slot_value is None:
00150             qWarning('Publisher._change_publisher_type(): could not find type: %s' % (type_name))
00151             return parent_slot._slot_types[slot_index]
00152 
00153         else:
00154             # replace old slot
00155             parent_slot._slot_types[slot_index] = type_name
00156             setattr(parent_slot, slot_name, slot_value)
00157 
00158             self._widget.publisher_tree_widget.model().update_publisher(publisher_info)
00159 
00160     def _change_publisher_rate(self, publisher_info, topic_name, new_value):
00161         try:
00162             rate = float(new_value)
00163         except Exception:
00164             qWarning('Publisher._change_publisher_rate(): could not parse rate value: %s' % (new_value))
00165         else:
00166             publisher_info['rate'] = rate
00167             #qDebug('Publisher._change_publisher_rate(): %s rate changed: %fHz' % (publisher_info['topic_name'], publisher_info['rate']))
00168             publisher_info['timer'].stop()
00169             if publisher_info['enabled'] and publisher_info['rate'] > 0:
00170                 publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
00171         # make sure the column value reflects the actual rate
00172         return '%.2f' % publisher_info['rate']
00173 
00174     def _change_publisher_expression(self, publisher_info, topic_name, new_value):
00175         expression = str(new_value)
00176         if len(expression) == 0:
00177             if topic_name in publisher_info['expressions']:
00178                 del publisher_info['expressions'][topic_name]
00179                 #qDebug('Publisher._change_publisher_expression(): removed expression for: %s' % (topic_name))
00180         else:
00181             slot_type, is_array = get_field_type(topic_name)
00182             if is_array:
00183                 slot_type = list
00184             # strip possible trailing error message from expression
00185             error_prefix = '# error'
00186             error_prefix_pos = expression.find(error_prefix)
00187             if error_prefix_pos >= 0:
00188                 expression = expression[:error_prefix_pos]
00189             success, _ = self._evaluate_expression(expression, slot_type)
00190             if success:
00191                 old_expression = publisher_info['expressions'].get(topic_name, None)
00192                 publisher_info['expressions'][topic_name] = expression
00193                 #print 'Publisher._change_publisher_expression(): topic: %s, type: %s, expression: %s' % (topic_name, slot_type, new_value)
00194                 self._fill_message_slots(publisher_info['message_instance'], publisher_info['topic_name'], publisher_info['expressions'], publisher_info['counter'])
00195                 try:
00196                     publisher_info['message_instance']._check_types()
00197                 except Exception, e:
00198                     error_str = str(e)
00199                     print 'serialization error:', error_str
00200                     if old_expression is not None:
00201                         publisher_info['expressions'][topic_name] = old_expression
00202                     else:
00203                         del publisher_info['expressions'][topic_name]
00204                     return '%s %s: %s' % (expression, error_prefix, error_str)
00205                 return expression
00206             else:
00207                 return '%s %s evaluating as "%s"' % (expression, error_prefix, slot_type.__name__)
00208 
00209     def _extract_array_info(self, type_str):
00210         array_size = None
00211         if '[' in type_str and type_str[-1] == ']':
00212             type_str, array_size_str = type_str.split('[', 1)
00213             array_size_str = array_size_str[:-1]
00214             if len(array_size_str) > 0:
00215                 array_size = int(array_size_str)
00216             else:
00217                 array_size = 0
00218 
00219         return type_str, array_size
00220 
00221     def _create_message_instance(self, type_str):
00222         base_type_str, array_size = self._extract_array_info(type_str)
00223 
00224         base_message_type = roslib.message.get_message_class(base_type_str)
00225         if base_message_type is None:
00226             print 'Could not create message of type "%s".' % base_type_str
00227             return None
00228 
00229         if array_size is not None:
00230             message = []
00231             for _ in range(array_size):
00232                 message.append(base_message_type())
00233         else:
00234             message = base_message_type()
00235         return message
00236 
00237     def _evaluate_expression(self, expression, slot_type):
00238         successful_eval = True
00239 
00240         try:
00241             # try to evaluate expression
00242             value = eval(expression, {}, self._eval_locals)
00243         except Exception:
00244             successful_eval = False
00245 
00246         if slot_type is str:
00247             if successful_eval:
00248                 value = str(value)
00249             else:
00250                 # for string slots just convert the expression to str, if it did not evaluate successfully
00251                 value = str(expression)
00252             successful_eval = True
00253 
00254         elif successful_eval:
00255             type_set = set((slot_type, type(value)))
00256             # check if value's type and slot_type belong to the same type group, i.e. array types, numeric types
00257             # and if they do, make sure values's type is converted to the exact slot_type
00258             if type_set <= set((list, tuple)) or type_set <= set((int, float)):
00259                 # convert to the right type
00260                 value = slot_type(value)
00261 
00262         if successful_eval and isinstance(value, slot_type):
00263             return True, value
00264         else:
00265             qWarning('Publisher._evaluate_expression(): failed to evaluate expression: "%s" as Python type "%s"' % (expression, slot_type.__name__))
00266 
00267         return False, None
00268 
00269     def _fill_message_slots(self, message, topic_name, expressions, counter):
00270         if topic_name in expressions and len(expressions[topic_name]) > 0:
00271 
00272             # get type
00273             if hasattr(message, '_type'):
00274                 message_type = message._type
00275             else:
00276                 message_type = type(message)
00277 
00278             self._eval_locals['i'] = counter
00279             success, value = self._evaluate_expression(expressions[topic_name], message_type)
00280             if not success:
00281                 value = message_type()
00282             return value
00283 
00284         # if no expression exists for this topic_name, continue with it's child slots
00285         elif hasattr(message, '__slots__'):
00286             for slot_name in message.__slots__:
00287                 value = self._fill_message_slots(getattr(message, slot_name), topic_name + '/' + slot_name, expressions, counter)
00288                 if value is not None:
00289                     setattr(message, slot_name, value)
00290 
00291         elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(message[0], '__slots__'):
00292             for index, slot in enumerate(message):
00293                 self._fill_message_slots(slot, topic_name + '[%d]' % index, expressions, counter)
00294 
00295         return None
00296 
00297     @Slot(int)
00298     def publish_once(self, publisher_id):
00299         publisher_info = self._publishers.get(publisher_id, None)
00300         if publisher_info is not None:
00301             publisher_info['counter'] += 1
00302             self._fill_message_slots(publisher_info['message_instance'], publisher_info['topic_name'], publisher_info['expressions'], publisher_info['counter'])
00303             publisher_info['publisher'].publish(publisher_info['message_instance'])
00304 
00305     @Slot(int)
00306     def remove_publisher(self, publisher_id):
00307         publisher_info = self._publishers.get(publisher_id, None)
00308         if publisher_info is not None:
00309             publisher_info['timer'].stop()
00310             publisher_info['publisher'].unregister()
00311             del self._publishers[publisher_id]
00312 
00313     def save_settings(self, plugin_settings, instance_settings):
00314         publisher_copies = []
00315         for publisher in self._publishers.values():
00316             publisher_copy = {}
00317             publisher_copy.update(publisher)
00318             publisher_copy['enabled'] = False
00319             del publisher_copy['timer']
00320             del publisher_copy['message_instance']
00321             del publisher_copy['publisher']
00322             publisher_copies.append(publisher_copy)
00323         instance_settings.set_value('publishers', repr(publisher_copies))
00324 
00325     def restore_settings(self, plugin_settings, instance_settings):
00326         publishers = eval(instance_settings.value('publishers', '[]'))
00327         for publisher in publishers:
00328             self._add_publisher(publisher)
00329 
00330     def clean_up_publishers(self):
00331         self._widget.publisher_tree_widget.model().clear()
00332         for publisher_info in self._publishers.values():
00333             publisher_info['timer'].stop()
00334             publisher_info['publisher'].unregister()
00335         self._publishers = {}
00336 
00337     def shutdown_plugin(self):
00338         self._widget.shutdown_plugin()
00339         self.clean_up_publishers()