$search
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 # Revision $Id$ 00036 00037 # rosplot current exists as a support library for rxplot. rosplot is 00038 # incomplete as a general library for getting plottable ROS Topic 00039 # data. The rxplot module still contains much of the logic necessary 00040 # for this. 00041 00042 import string 00043 import sys 00044 import threading 00045 import time 00046 00047 import roslib.message 00048 import roslib.scriptutil 00049 00050 import rospy 00051 00052 class RosPlotException(Exception): pass 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 ## @return str, str, str: topic type, real topic name, and rest of name referenced 00057 ## if the \a topic points to a field within a topic, e.g. /rosout/msg 00058 def _get_topic_type(topic): 00059 code, msg, val = roslib.scriptutil.get_master().getPublishedTopics('/', '/') 00060 if code != 1: 00061 raise RosPlotException("unable to get list of topics from master") 00062 matches = [(t, t_type) for t, t_type in val if t == topic or topic.startswith(t+'/')] 00063 if matches: 00064 t, t_type = matches[0] 00065 if t_type == roslib.names.ANYTYPE: 00066 return None, None, None 00067 if t_type == topic: 00068 return t_type, None 00069 return t_type, t, topic[len(t):] 00070 else: 00071 return None, None, None 00072 00073 ## get the topic type (nearly identical to rostopic.get_topic_type, except it doesn't return a fn) 00074 ## @return str, str, str: topic type, real topic name, and rest of name referenced 00075 ## if the \a topic points to a field within a topic, e.g. /rosout/msg 00076 def get_topic_type(topic): 00077 topic_type, real_topic, rest = _get_topic_type(topic) 00078 if topic_type: 00079 return topic_type, real_topic, rest 00080 else: 00081 print >> sys.stderr, "WARNING: topic [%s] does not appear to be published yet"%topic 00082 while not rospy.is_shutdown(): 00083 topic_type, real_topic, rest = _get_topic_type(topic) 00084 if topic_type: 00085 return topic_type, real_topic, rest 00086 else: 00087 time.sleep(0.1) 00088 return None, None, None 00089 00090 00091 ## Subscriber to ROS topic that buffers incoming data 00092 class ROSData(object): 00093 00094 def __init__(self, topic, start_time): 00095 self.name = topic 00096 self.start_time = start_time 00097 self.error = None 00098 00099 self.lock = threading.Lock() 00100 self.buff_x = [] 00101 self.buff_y = [] 00102 00103 topic_type, real_topic, fields = get_topic_type(topic) 00104 self.field_evals = generate_field_evals(fields) 00105 data_class = roslib.message.get_message_class(topic_type) 00106 self.sub = rospy.Subscriber(real_topic, data_class, self._ros_cb) 00107 00108 def close(self): 00109 self.sub.unregister() 00110 00111 ## ROS subscriber callback 00112 ## @param self 00113 ## @param msg 00114 def _ros_cb(self, msg): 00115 try: 00116 self.lock.acquire() 00117 try: 00118 self.buff_y.append(self._get_data(msg)) 00119 # #944: use message header time if present 00120 if msg.__class__._has_header: 00121 self.buff_x.append(msg.header.stamp.to_sec() - self.start_time) 00122 else: 00123 self.buff_x.append(rospy.get_time() - self.start_time) 00124 #self.axes[index].plot(datax, buff_y) 00125 except AttributeError, e: 00126 self.error = RosPlotException("Invalid topic spec [%s]: %s"%(self.name, str(e))) 00127 finally: 00128 self.lock.release() 00129 00130 ## Get the next data in the series 00131 ## @param self 00132 ## @return [xdata], [ydata] 00133 def next(self): 00134 if self.error: 00135 raise self.error 00136 try: 00137 self.lock.acquire() 00138 buff_x = self.buff_x 00139 buff_y = self.buff_y 00140 self.buff_x = [] 00141 self.buff_y = [] 00142 finally: 00143 self.lock.release() 00144 return buff_x, buff_y 00145 00146 def _get_data(self, msg): 00147 val = msg 00148 try: 00149 if not self.field_evals: 00150 return float(val) 00151 for f in self.field_evals: 00152 val = f(val) 00153 return float(val) 00154 except TypeError: 00155 print "[%s] value was not numeric: %s"%(self.name, val) 00156 #TODO: really, really shouldn't be doing this here 00157 sys.exit(1) 00158 00159 _paused = False 00160 def toggle_ros_pause(): 00161 global _paused 00162 _paused = not _paused 00163 00164 # TODO: this should affect data collection, but it doesn't right now 00165 _stopped = False 00166 def set_ros_stop(): 00167 global _stopped, _paused 00168 _stopped = True 00169 _paused = True 00170 00171 def is_ros_pause(): 00172 return _paused 00173 def is_ros_stop(): 00174 return _stopped 00175 00176 ## @param str field_name: name of field to index into 00177 ## @param str slot_num: index of slot to return 00178 ## @return fn(msg_field)->msg_field[slot_num] 00179 def _array_eval(field_name, slot_num): 00180 def fn(f): 00181 return getattr(f, field_name).__getitem__(slot_num) 00182 return fn 00183 00184 ## @param str field_name: name of field to return 00185 ## @return fn(msg_field)->msg_field.field_name 00186 def _field_eval(field_name): 00187 def fn(f): 00188 return getattr(f, field_name) 00189 return fn 00190 00191 def generate_field_evals(fields): 00192 try: 00193 evals = [] 00194 fields = [f for f in fields.split('/') if f] 00195 for f in fields: 00196 if '[' in f: 00197 field_name, rest = f.split('[') 00198 slot_num = string.atoi(rest[:rest.find(']')]) 00199 evals.append(_array_eval(field_name, slot_num)) 00200 else: 00201 evals.append(_field_eval(f)) 00202 return evals 00203 except Exception, e: 00204 raise RosPlotException("cannot parse field reference [%s]: %s"%(fields, str(e))) 00205