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 from std_msgs.msg import Bool
00046 
00047 
00048 class RosPlotException(Exception):
00049     pass
00050 
00051 
00052 def _get_topic_type(topic):
00053     """
00054     subroutine for getting the topic type
00055     (nearly identical to rostopic._get_topic_type, except it returns rest of name instead of fn)
00056 
00057     :returns: topic type, real topic name, and rest of name referenced
00058       if the topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str``
00059     """
00060     try:
00061         master = rosgraph.Master('/rosplot')
00062         val = master.getTopicTypes()
00063     except:
00064         raise RosPlotException("unable to get list of topics from master")
00065     matches = [(t, t_type) for t, t_type in val if t == topic or topic.startswith(t + '/')]
00066     if matches:
00067         t, t_type = matches[0]
00068         if t_type == roslib.names.ANYTYPE:
00069             return None, None, None
00070         if t_type == topic:
00071             return t_type, None
00072         return t_type, t, topic[len(t):]
00073     else:
00074         return None, None, None
00075 
00076 
00077 def get_topic_type(topic):
00078     """
00079     Get the topic type (nearly identical to rostopic.get_topic_type, except it doesn't return a fn)
00080 
00081     :returns: topic type, real topic name, and rest of name referenced
00082       if the \a topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str``
00083     """
00084     topic_type, real_topic, rest = _get_topic_type(topic)
00085     if topic_type:
00086         return topic_type, real_topic, rest
00087     else:
00088         return None, None, None
00089 
00090 
00091 class ROSData(object):
00092 
00093     """
00094     Subscriber to ROS topic that buffers incoming data
00095     """
00096 
00097     def __init__(self, topic, start_time):
00098         self.name = topic
00099         self.start_time = start_time
00100         self.error = None
00101 
00102         self.lock = threading.Lock()
00103         self.buff_x = []
00104         self.buff_y = []
00105 
00106         topic_type, real_topic, fields = get_topic_type(topic)
00107         if topic_type is not None:
00108             self.field_evals = generate_field_evals(fields)
00109             data_class = roslib.message.get_message_class(topic_type)
00110             self.sub = rospy.Subscriber(real_topic, data_class, self._ros_cb)
00111         else:
00112             self.error = RosPlotException("Can not resolve topic type of %s" % topic)
00113 
00114     def close(self):
00115         self.sub.unregister()
00116 
00117     def _ros_cb(self, msg):
00118         """
00119         ROS subscriber callback
00120         :param msg: ROS message data
00121         """
00122         try:
00123             self.lock.acquire()
00124             try:
00125                 self.buff_y.append(self._get_data(msg))
00126                 # 944: use message header time if present
00127                 if msg.__class__._has_header:
00128                     self.buff_x.append(msg.header.stamp.to_sec() - self.start_time)
00129                 else:
00130                     self.buff_x.append(rospy.get_time() - self.start_time)
00131                 # self.axes[index].plot(datax, buff_y)
00132             except AttributeError as e:
00133                 self.error = RosPlotException("Invalid topic spec [%s]: %s" % (self.name, str(e)))
00134         finally:
00135             self.lock.release()
00136 
00137     def next(self):
00138         """
00139         Get the next data in the series
00140 
00141         :returns: [xdata], [ydata]
00142         """
00143         if self.error:
00144             raise self.error
00145         try:
00146             self.lock.acquire()
00147             buff_x = self.buff_x
00148             buff_y = self.buff_y
00149             self.buff_x = []
00150             self.buff_y = []
00151         finally:
00152             self.lock.release()
00153         return buff_x, buff_y
00154 
00155     def _get_data(self, msg):
00156         val = msg
00157         try:
00158             if not self.field_evals:
00159                 if isinstance(val, Bool):
00160                     # extract boolean field from bool messages
00161                     val = val.data
00162                 return float(val)
00163             for f in self.field_evals:
00164                 val = f(val)
00165             return float(val)
00166         except IndexError:
00167             self.error = RosPlotException(
00168                 "[%s] index error for: %s" % (self.name, str(val).replace('\n', ', ')))
00169         except TypeError:
00170             self.error = RosPlotException("[%s] value was not numeric: %s" % (self.name, val))
00171 
00172 
00173 def _array_eval(field_name, slot_num):
00174     """
00175     :param field_name: name of field to index into, ``str``
00176     :param slot_num: index of slot to return, ``str``
00177     :returns: fn(msg_field)->msg_field[slot_num]
00178     """
00179     def fn(f):
00180         return getattr(f, field_name).__getitem__(slot_num)
00181     return fn
00182 
00183 
00184 def _field_eval(field_name):
00185     """
00186     :param field_name: name of field to return, ``str``
00187     :returns: fn(msg_field)->msg_field.field_name
00188     """
00189     def fn(f):
00190         return getattr(f, field_name)
00191     return fn
00192 
00193 
00194 def generate_field_evals(fields):
00195     try:
00196         evals = []
00197         fields = [f for f in fields.split('/') if f]
00198         for f in fields:
00199             if '[' in f:
00200                 field_name, rest = f.split('[')
00201                 slot_num = string.atoi(rest[:rest.find(']')])
00202                 evals.append(_array_eval(field_name, slot_num))
00203             else:
00204                 evals.append(_field_eval(f))
00205         return evals
00206     except Exception as e:
00207         raise RosPlotException("cannot parse field reference [%s]: %s" % (fields, str(e)))


rqt_plot
Author(s): Dorian Scholz
autogenerated on Sun Mar 17 2019 02:29:34