rosplot.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2009, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of Willow Garage, Inc. nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 #
00035 
00036 import string
00037 import sys
00038 import threading
00039 import time
00040 
00041 import rosgraph
00042 import roslib.message
00043 import roslib.names
00044 import rospy
00045 
00046 
00047 class RosPlotException(Exception):
00048     pass
00049 
00050 
00051 def _get_topic_type(topic):
00052     """
00053     subroutine for getting the topic type
00054     (nearly identical to rostopic._get_topic_type, except it returns rest of name instead of fn)
00055 
00056     :returns: topic type, real topic name, and rest of name referenced
00057       if the topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str``
00058     """
00059     try:
00060         master = rosgraph.Master('/rosplot')
00061         val = master.getTopicTypes()
00062     except:
00063         raise RosPlotException("unable to get list of topics from master")
00064     matches = [(t, t_type) for t, t_type in val if t == topic or topic.startswith(t + '/')]
00065     if matches:
00066         t, t_type = matches[0]
00067         if t_type == roslib.names.ANYTYPE:
00068             return None, None, None
00069         if t_type == topic:
00070             return t_type, None
00071         return t_type, t, topic[len(t):]
00072     else:
00073         return None, None, None
00074 
00075 
00076 def get_topic_type(topic):
00077     """
00078     Get the topic type (nearly identical to rostopic.get_topic_type, except it doesn't return a fn)
00079 
00080     :returns: topic type, real topic name, and rest of name referenced
00081       if the \a topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str``
00082     """
00083     topic_type, real_topic, rest = _get_topic_type(topic)
00084     if topic_type:
00085         return topic_type, real_topic, rest
00086     else:
00087         return None, None, None
00088 
00089 
00090 class ROSData(object):
00091     """
00092     Subscriber to ROS topic that buffers incoming data
00093     """
00094 
00095     def __init__(self, topic, start_time):
00096         self.name = topic
00097         self.start_time = start_time
00098         self.error = None
00099 
00100         self.lock = threading.Lock()
00101         self.buff_x = []
00102         self.buff_y = []
00103 
00104         topic_type, real_topic, fields = get_topic_type(topic)
00105         if topic_type is not None:
00106             self.field_evals = generate_field_evals(fields)
00107             data_class = roslib.message.get_message_class(topic_type)
00108             self.sub = rospy.Subscriber(real_topic, data_class, self._ros_cb)
00109         else:
00110             self.error = RosPlotException("Can not resolve topic type of %s" % topic)
00111 
00112     def close(self):
00113         self.sub.unregister()
00114 
00115     def _ros_cb(self, msg):
00116         """
00117         ROS subscriber callback
00118         :param msg: ROS message data
00119         """
00120         try:
00121             self.lock.acquire()
00122             try:
00123                 self.buff_y.append(self._get_data(msg))
00124                 # #944: use message header time if present
00125                 if msg.__class__._has_header:
00126                     self.buff_x.append(msg.header.stamp.to_sec() - self.start_time)
00127                 else:
00128                     self.buff_x.append(rospy.get_time() - self.start_time)
00129                 #self.axes[index].plot(datax, buff_y)
00130             except AttributeError, e:
00131                 self.error = RosPlotException("Invalid topic spec [%s]: %s" % (self.name, str(e)))
00132         finally:
00133             self.lock.release()
00134 
00135     def next(self):
00136         """
00137         Get the next data in the series
00138 
00139         :returns: [xdata], [ydata]
00140         """
00141         if self.error:
00142             raise self.error
00143         try:
00144             self.lock.acquire()
00145             buff_x = self.buff_x
00146             buff_y = self.buff_y
00147             self.buff_x = []
00148             self.buff_y = []
00149         finally:
00150             self.lock.release()
00151         return buff_x, buff_y
00152 
00153     def _get_data(self, msg):
00154         val = msg
00155         try:
00156             if not self.field_evals:
00157                 return float(val)
00158             for f in self.field_evals:
00159                 val = f(val)
00160             return float(val)
00161         except IndexError:
00162             self.error = RosPlotException("[%s] index error for: %s" % (self.name, str(val).replace('\n', ', ')))
00163         except TypeError:
00164             self.error = RosPlotException("[%s] value was not numeric: %s" % (self.name, val))
00165 
00166 
00167 def _array_eval(field_name, slot_num):
00168     """
00169     :param field_name: name of field to index into, ``str``
00170     :param slot_num: index of slot to return, ``str``
00171     :returns: fn(msg_field)->msg_field[slot_num]
00172     """
00173     def fn(f):
00174         return getattr(f, field_name).__getitem__(slot_num)
00175     return fn
00176 
00177 
00178 def _field_eval(field_name):
00179     """
00180     :param field_name: name of field to return, ``str``
00181     :returns: fn(msg_field)->msg_field.field_name
00182     """
00183     def fn(f):
00184         return getattr(f, field_name)
00185     return fn
00186 
00187 
00188 def generate_field_evals(fields):
00189     try:
00190         evals = []
00191         fields = [f for f in fields.split('/') if f]
00192         for f in fields:
00193             if '[' in f:
00194                 field_name, rest = f.split('[')
00195                 slot_num = string.atoi(rest[:rest.find(']')])
00196                 evals.append(_array_eval(field_name, slot_num))
00197             else:
00198                 evals.append(_field_eval(f))
00199         return evals
00200     except Exception, e:
00201         raise RosPlotException("cannot parse field reference [%s]: %s" % (fields, str(e)))


rqt_plot
Author(s): Dorian Scholz
autogenerated on Mon Oct 6 2014 07:15:32