_diagnostic_updater.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2012, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 
33 # -*- coding: utf-8 -*-
34 
35 """ diagnostic_updater for Python.
36 @author Brice Rebsamen <brice [dot] rebsamen [gmail]>
37 """
38 
39 import rospy
40 import threading
41 from diagnostic_msgs.msg import DiagnosticArray
42 
43 try:
44  import httplib
45 except ImportError:
46  import http.client as httplib
47 
48 from ._diagnostic_status_wrapper import *
49 
51  """DiagnosticTask is an abstract base class for collecting diagnostic data.
52 
53  Subclasses are provided for generating common diagnostic information.
54  A DiagnosticTask has a name, and a function that is called to cleate a
55  DiagnosticStatusWrapper.
56  """
57 
58  def __init__(self, name):
59  """Constructs a DiagnosticTask setting its name in the process."""
60  self.name = name
61 
62  def getName(self):
63  """Returns the name of the DiagnosticTask."""
64  return self.name
65 
66  def run(self, stat):
67  """Fills out this Task's DiagnosticStatusWrapper.
68  @param stat: the DiagnosticStatusWrapper to fill
69  @return the filled DiagnosticStatusWrapper
70  """
71  return stat
72 
73 
74 
76  """A DiagnosticTask based on a function.
77 
78  The FunctionDiagnosticTask calls the function when it updates. The
79  function updates the DiagnosticStatusWrapper and collects data.
80 
81  This is useful for gathering information about a device or driver, like temperature,
82  calibration, etc.
83  """
84 
85  def __init__(self, name, fn):
86  """Constructs a GenericFunctionDiagnosticTask based on the given name and function.
87  @param name Name of the function.
88  @param fn Function to be called when run is called.
89  """
90  DiagnosticTask.__init__(self, name)
91  self.fn = fn
92 
93  def run(self, stat):
94  return self.fn(stat)
95 
96 
97 
99  """Merges CompositeDiagnosticTask into a single DiagnosticTask.
100 
101  The CompositeDiagnosticTask allows multiple DiagnosticTask instances to
102  be combined into a single task that produces a single single
103  DiagnosticStatusWrapped. The output of the combination has the max of
104  the status levels, and a concatenation of the non-zero-level messages.
105 
106  For instance, this could be used to combine the calibration and offset data
107  from an IMU driver.
108  """
109 
110  def __init__(self, name):
111  """Constructs a CompositeDiagnosticTask with the given name."""
112  DiagnosticTask.__init__(self, name)
113  self.tasks = []
114 
115  def run(self, stat):
116  """Runs each child and merges their outputs."""
117  combined_summary = DiagnosticStatusWrapper()
118  original_summary = DiagnosticStatusWrapper()
119 
120  original_summary.summary(stat)
121 
122  for task in self.tasks:
123  # Put the summary that was passed in.
124  stat.summary(original_summary)
125  # Let the next task add entries and put its summary.
126  stat = task.run(stat)
127  # Merge the new summary into the combined summary.
128  combined_summary.mergeSummary(stat)
129 
130 
131  # Copy the combined summary into the output.
132  stat.summary(combined_summary)
133  return stat
134 
135  def addTask(self, t):
136  """Adds a child CompositeDiagnosticTask.
137 
138  This CompositeDiagnosticTask will be called each time this
139  CompositeDiagnosticTask is run.
140  """
141  self.tasks.append(t)
142 
143 
144 
146  """Internal use only.
147 
148  Base class for diagnostic_updater::Updater and self_test::Dispatcher.
149  The class manages a collection of diagnostic updaters. It contains the
150  common functionality used for producing diagnostic updates and for
151  self-tests.
152  """
153 
155  """Class used to represent a diagnostic task internally in
156  DiagnosticTaskVector.
157  """
158 
159  def __init__(self, name, fn):
160  self.name = name
161  self.fn = fn
162 
163  def run(self, stat):
164  stat.name = self.name
165  return self.fn(stat)
166 
167 
168  def __init__(self):
169  self.tasks = []
170  self.lock = threading.Lock()
171 
172  def addedTaskCallback(self, task):
173  """Allows an action to be taken when a task is added. The Updater class
174  uses this to immediately publish a diagnostic that says that the node
175  is loading.
176  """
177  pass
178 
179  def add(self, *args):
180  """Add a task to the DiagnosticTaskVector.
181 
182  Usage:
183  add(task): where task is a DiagnosticTask
184  add(name, fn): add a DiagnosticTask embodied by a name and function
185  """
186  if len(args)==1:
187  task = DiagnosticTaskVector.DiagnosticTaskInternal(args[0].getName(), args[0].run)
188  elif len(args)==2:
189  task = DiagnosticTaskVector.DiagnosticTaskInternal(args[0], args[1])
190 
191  with self.lock:
192  self.tasks.append(task)
193  self.addedTaskCallback(task)
194 
195  def removeByName(self, name):
196  """Removes a task based on its name.
197 
198  Removes the first task that matches the specified name. (New in
199  version 1.1.2)
200 
201  @param name Name of the task to remove.
202  @return Returns true if a task matched and was removed.
203  """
204  found = False
205  with self.lock:
206  for i in range(len(self.tasks)):
207  if self.tasks[i].name == name:
208  self.tasks.pop(i)
209  found = True
210  break
211  return found
212 
213 
214 
215 
217  """Manages a list of diagnostic tasks, and calls them in a rate-limited manner.
218 
219  This class manages a list of diagnostic tasks. Its update function
220  should be called frequently. At some predetermined rate, the update
221  function will cause all the diagnostic tasks to run, and will collate
222  and publish the resulting diagnostics. The publication rate is
223  determined by the "~diagnostic_period" ros parameter.
224 
225  The class also allows an update to be forced when something significant
226  has happened, and allows a single message to be broadcast on all the
227  diagnostics if normal operation of the node is suspended for some
228  reason.
229  """
230 
231  def __init__(self):
232  """Constructs an updater class."""
233  DiagnosticTaskVector.__init__(self)
234  self.publisher = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10)
235 
236  self.last_time = rospy.Time.now()
238  self.period = 1
239 
240  self.verbose = False
241  self.hwid = ""
242  self.warn_nohwid_done = False
243 
244  def update(self):
245  """Causes the diagnostics to update if the inter-update interval
246  has been exceeded.
247  """
249  if rospy.Time.now() >= self.last_time + rospy.Duration(self.period):
250  self.force_update()
251 
252  def force_update(self):
253  """Forces the diagnostics to update.
254 
255  Useful if the node has undergone a drastic state change that should be
256  published immediately.
257  """
258  self.last_time = rospy.Time.now()
259 
260  warn_nohwid = len(self.hwid)==0
261 
262  status_vec = []
263 
264  with self.lock: # Make sure no adds happen while we are processing here.
265  for task in self.tasks:
266  status = DiagnosticStatusWrapper()
267  status.name = task.name
268  status.level = 2
269  status.message = "No message was set"
270  status.hardware_id = self.hwid
271 
272  stat = task.run(status)
273 
274  status_vec.append(status)
275 
276  if status.level:
277  warn_nohwid = False
278 
279  if self.verbose and status.level:
280  rospy.logwarn("Non-zero diagnostic status. Name: '%s', status %i: '%s'" %
281  (status.name, status.level, status.message))
282 
283  if warn_nohwid and not self.warn_nohwid_done:
284  rospy.logwarn("diagnostic_updater: No HW_ID was set. This is probably a bug. Please report it. For devices that do not have a HW_ID, set this value to 'none'. This warning only occurs once all diagnostics are OK so it is okay to wait until the device is open before calling setHardwareID.");
285  self.warn_nohwid_done = True
286 
287  self.publish(status_vec)
288 
289  def broadcast(self, lvl, msg):
290  """Outputs a message on all the known DiagnosticStatus.
291 
292  Useful if something drastic is happening such as shutdown or a self-test.
293 
294  @param lvl Level of the diagnostic being output.
295  @param msg Status message to output.
296  """
297 
298  status_vec = []
299 
300  for task in self.tasks:
301  status = DiagnosticStatusWrapper()
302  status.name = task.name
303  status.summary(lvl, msg)
304  status_vec.append(status)
305 
306  self.publish(status_vec)
307 
308  def setHardwareID(self, hwid):
309  self.hwid = hwid
310 
312  """Recheck the diagnostic_period on the parameter server."""
313 
314  # This was getParamCached() call in the cpp code. i.e. it would throttle
315  # the actual call to the parameter server using a notification of change
316  # mechanism.
317  # This is not available in rospy. Hence I throttle the call to the
318  # parameter server using a standard timeout mechanism (4Hz)
319 
320  now = rospy.Time.now()
321  if now >= self.last_time_period_checked + rospy.Duration(0.25):
322  try:
323  self.period = rospy.get_param_cached("~diagnostic_period", 1)
324  self.last_time_period_checked = now
325  except (httplib.CannotSendRequest, httplib.ResponseNotReady):
326  pass
327 
328  def publish(self, msg):
329  """Publishes a single diagnostic status or a vector of diagnostic statuses."""
330  if not type(msg) is list:
331  msg = [msg]
332 
333  for stat in msg:
334  stat.name = rospy.get_name()[1:]+ ": " + stat.name
335 
336  da = DiagnosticArray()
337  da.status = msg
338  da.header.stamp = rospy.Time.now() # Add timestamp for ROS 0.10
339  self.publisher.publish(da)
340 
341  def addedTaskCallback(self, task):
342  stat = DiagnosticStatusWrapper()
343  stat.name = task.name
344  stat.summary(0, "Node starting up")
345  self.publish(stat)


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Thu Oct 8 2020 03:19:34