rosplot.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2009, Willow Garage, Inc.
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without
9 # modification, are permitted provided that the following conditions
10 # are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above
15 # copyright notice, this list of conditions and the following
16 # disclaimer in the documentation and/or other materials provided
17 # with the distribution.
18 # * Neither the name of Willow Garage, Inc. nor the names of its
19 # contributors may be used to endorse or promote products derived
20 # from this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34 #
35 
36 import sys
37 import threading
38 import time
39 
40 import rosgraph
41 import roslib.message
42 import roslib.names
43 import rospy
44 from std_msgs.msg import Bool
45 
46 
47 class RosPlotException(Exception):
48  pass
49 
50 
51 def _get_topic_type(topic):
52  """
53  subroutine for getting the topic type
54  (nearly identical to rostopic._get_topic_type, except it returns rest of name instead of fn)
55 
56  :returns: topic type, real topic name, and rest of name referenced
57  if the topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str``
58  """
59  try:
60  master = rosgraph.Master('/rosplot')
61  val = master.getTopicTypes()
62  except:
63  raise RosPlotException("unable to get list of topics from master")
64  matches = [(t, t_type) for t, t_type in val if t == topic or topic.startswith(t + '/')]
65  if matches:
66  t, t_type = matches[0]
67  if t_type == roslib.names.ANYTYPE:
68  return None, None, None
69  if t_type == topic:
70  return t_type, None
71  return t_type, t, topic[len(t):]
72  else:
73  return None, None, None
74 
75 
76 def get_topic_type(topic):
77  """
78  Get the topic type (nearly identical to rostopic.get_topic_type, except it doesn't return a fn)
79 
80  :returns: topic type, real topic name, and rest of name referenced
81  if the \a topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str``
82  """
83  topic_type, real_topic, rest = _get_topic_type(topic)
84  if topic_type:
85  return topic_type, real_topic, rest
86  else:
87  return None, None, None
88 
89 
90 class ROSData(object):
91 
92  """
93  Subscriber to ROS topic that buffers incoming data
94  """
95 
96  def __init__(self, topic, start_time):
97  self.name = topic
98  self.start_time = start_time
99  self.error = None
100 
101  self.lock = threading.Lock()
102  self.buff_x = []
103  self.buff_y = []
104 
105  topic_type, real_topic, fields = get_topic_type(topic)
106  if topic_type is not None:
108  data_class = roslib.message.get_message_class(topic_type)
109  self.sub = rospy.Subscriber(real_topic, data_class, self._ros_cb)
110  else:
111  self.error = RosPlotException("Can not resolve topic type of %s" % topic)
112 
113  def close(self):
114  self.sub.unregister()
115 
116  def _ros_cb(self, msg):
117  """
118  ROS subscriber callback
119  :param msg: ROS message data
120  """
121  try:
122  self.lock.acquire()
123  try:
124  self.buff_y.append(self._get_data(msg))
125  # 944: use message header time if present
126  if msg.__class__._has_header:
127  self.buff_x.append(msg.header.stamp.to_sec() - self.start_time)
128  else:
129  self.buff_x.append(rospy.get_time() - self.start_time)
130  # self.axes[index].plot(datax, buff_y)
131  except AttributeError as e:
132  self.error = RosPlotException("Invalid topic spec [%s]: %s" % (self.name, str(e)))
133  finally:
134  self.lock.release()
135 
136  def next(self):
137  """
138  Get the next data in the series
139 
140  :returns: [xdata], [ydata]
141  """
142  if self.error:
143  raise self.error
144  try:
145  self.lock.acquire()
146  buff_x = self.buff_x
147  buff_y = self.buff_y
148  self.buff_x = []
149  self.buff_y = []
150  finally:
151  self.lock.release()
152  return buff_x, buff_y
153 
154  def _get_data(self, msg):
155  val = msg
156  try:
157  if not self.field_evals:
158  if isinstance(val, Bool):
159  # extract boolean field from bool messages
160  val = val.data
161  return float(val)
162  for f in self.field_evals:
163  val = f(val)
164  return float(val)
165  except IndexError:
166  self.error = RosPlotException(
167  "[%s] index error for: %s" % (self.name, str(val).replace('\n', ', ')))
168  except TypeError:
169  self.error = RosPlotException("[%s] value was not numeric: %s" % (self.name, val))
170 
171 
172 def _array_eval(field_name, slot_num):
173  """
174  :param field_name: name of field to index into, ``str``
175  :param slot_num: index of slot to return, ``str``
176  :returns: fn(msg_field)->msg_field[slot_num]
177  """
178  def fn(f):
179  return getattr(f, field_name).__getitem__(slot_num)
180  return fn
181 
182 
183 def _field_eval(field_name):
184  """
185  :param field_name: name of field to return, ``str``
186  :returns: fn(msg_field)->msg_field.field_name
187  """
188  def fn(f):
189  return getattr(f, field_name)
190  return fn
191 
192 
194  try:
195  evals = []
196  fields = [f for f in fields.split('/') if f]
197  for f in fields:
198  if '[' in f:
199  field_name, rest = f.split('[')
200  slot_num = int(rest[:rest.find(']')])
201  evals.append(_array_eval(field_name, slot_num))
202  else:
203  evals.append(_field_eval(f))
204  return evals
205  except Exception as e:
206  raise RosPlotException("cannot parse field reference [%s]: %s" % (fields, str(e)))
rqt_plot.rosplot.RosPlotException
Definition: rosplot.py:47
rqt_plot.rosplot.ROSData.lock
lock
Definition: rosplot.py:101
rqt_plot.rosplot._field_eval
def _field_eval(field_name)
Definition: rosplot.py:183
rqt_plot.rosplot.ROSData.__init__
def __init__(self, topic, start_time)
Definition: rosplot.py:96
rqt_plot.rosplot.ROSData.next
def next(self)
Definition: rosplot.py:136
rqt_plot.rosplot.ROSData
Definition: rosplot.py:90
rqt_plot.rosplot.ROSData.error
error
Definition: rosplot.py:99
rqt_plot.rosplot.ROSData.field_evals
field_evals
Definition: rosplot.py:107
rqt_plot.rosplot.ROSData._ros_cb
def _ros_cb(self, msg)
Definition: rosplot.py:116
rqt_plot.rosplot.ROSData.close
def close(self)
Definition: rosplot.py:113
rqt_plot.rosplot._array_eval
def _array_eval(field_name, slot_num)
Definition: rosplot.py:172
rqt_plot.rosplot.ROSData.sub
sub
Definition: rosplot.py:109
rqt_plot.rosplot.ROSData.start_time
start_time
Definition: rosplot.py:98
rqt_plot.rosplot._get_topic_type
def _get_topic_type(topic)
Definition: rosplot.py:51
rqt_plot.rosplot.ROSData.buff_y
buff_y
Definition: rosplot.py:103
rqt_plot.rosplot.ROSData.buff_x
buff_x
Definition: rosplot.py:102
rqt_plot.rosplot.generate_field_evals
def generate_field_evals(fields)
Definition: rosplot.py:193
rqt_plot.rosplot.get_topic_type
def get_topic_type(topic)
Definition: rosplot.py:76
rqt_plot.rosplot.ROSData._get_data
def _get_data(self, msg)
Definition: rosplot.py:154
rqt_plot.rosplot.ROSData.name
name
Definition: rosplot.py:97


rqt_plot
Author(s): Dorian Scholz, Dirk Thomas
autogenerated on Wed Mar 2 2022 00:58:35