example.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 """
00005 @author Brice Rebsamen <brice [dot] rebsamen [gmail]>
00006 """
00007 
00008 import roslib
00009 roslib.load_manifest('diagnostic_updater')
00010 import rospy
00011 import diagnostic_updater
00012 import diagnostic_msgs
00013 import std_msgs
00014 
00015 
00016 time_to_launch = 0
00017 
00018 '''Used as a tutorial for loading and using diagnostic updater.
00019 
00020 DummyClass and dummy_diagnostics show how to use a diagnostic_updater
00021 class.
00022 '''
00023 
00024 def dummy_diagnostic(stat):
00025     # stat is supposed to be of type diagnostic_updater.DiagnosticStatusWrapper
00026     # DiagnosticStatusWrapper is a derived class of
00027     # diagnostic_msgs.msg.DiagnosticStatus that provides a set of convenience
00028     # methods.
00029 
00030     # summary sets the level and message.
00031     # As opposed to the C++ API, there is no summaryf function: use python's
00032     # string formatting instead
00033     if time_to_launch < 10:
00034         # summary for formatted text.
00035         stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
00036             "Buckle your seat belt. Launch in %f seconds!" % time_to_launch)
00037     else:
00038         # summary for unformatted text. It's just the same ;)
00039         stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK,
00040             "Launch is in a long time. Have a soda.")
00041 
00042     # add is used to append key-value pairs.
00043     # Again, as oppose to the C++ API, there is no addf function. The second
00044     # argument is always converted to a str using the str() function.
00045     stat.add("Diagnostic Name", "dummy")
00046     # add transparently handles conversion to string (using str()).
00047     stat.add("Time to Launch", time_to_launch)
00048     # add allows arbitrary printf style formatting.
00049     stat.add("Geeky thing to say", "The square of the time to launch %f is %f" % \
00050         (time_to_launch, time_to_launch * time_to_launch) )
00051 
00052     # As opposed to the C++ diagnostic function which modifies its argument,
00053     # the python version must return the modified message.
00054     return stat
00055 
00056 
00057 class DummyClass:
00058     def produce_diagnostics(self, stat):
00059         stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "This is a silly updater.")
00060         stat.add("Stupidicity of this updater", 1000.)
00061         return stat
00062 
00063 
00064 class DummyTask(diagnostic_updater.DiagnosticTask):
00065     def __init__(self):
00066         diagnostic_updater.DiagnosticTask.__init__(self,
00067             "Updater Derived from DiagnosticTask")
00068 
00069     def run(self, stat):
00070         stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN,
00071             "This is another silly updater.")
00072         stat.add("Stupidicity of this updater", 2000.)
00073         return stat
00074 
00075 
00076 def check_lower_bound(stat):
00077     if time_to_launch > 5:
00078         stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK, "Lower-bound OK")
00079     else:
00080         stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Too low")
00081     stat.add("Low-Side Margin", time_to_launch - 5)
00082     return stat
00083 
00084 
00085 def check_upper_bound(stat):
00086     if time_to_launch < 10:
00087         stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK, "Upper-bound OK")
00088     else:
00089         stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Too high")
00090     stat.add("Top-Side Margin", 10 - time_to_launch)
00091     return stat
00092 
00093 
00094 if __name__=='__main__':
00095     rospy.init_node("diagnostic_updater_example")
00096 
00097     # The Updater class advertises to /diagnostics, and has a
00098     # ~diagnostic_period parameter that says how often the diagnostics
00099     # should be published.
00100     updater = diagnostic_updater.Updater()
00101 
00102     # The diagnostic_updater.Updater class will fill out the hardware_id
00103     # field of the diagnostic_msgs.msg.DiagnosticStatus message. You need to
00104     # use the setHardwareID() method to set the hardware ID.
00105     #
00106     # The hardware ID should be able to identify the specific device you are
00107     # working with.  If it is not appropriate to fill out a hardware ID in
00108     # your case, you should call setHardwareID("none") to avoid warnings.
00109     # (A warning will be generated as soon as your node updates with no
00110     # non-OK statuses.)
00111     updater.setHardwareID("none")
00112     # Or...
00113     updater.setHardwareID("Device-%i-%i" % (27, 46) )
00114 
00115     # Diagnostic tasks are added to the Updater. They will later be run when
00116     # the updater decides to update.
00117     # As opposed to the C++ API, there is only one add function. It can take
00118     # several types of arguments:
00119     #  - add(task): where task is a DiagnosticTask
00120     #  - add(name, fn): add a DiagnosticTask embodied by a name and function
00121     updater.add("Function updater", dummy_diagnostic)
00122     dc = DummyClass()
00123     updater.add("Method updater", dc.produce_diagnostics)
00124 
00125     # Internally, updater.add converts its arguments into a DiagnosticTask.
00126     # Sometimes it can be useful to work directly with DiagnosticTasks. Look
00127     # at FrequencyStatus and TimestampStatus in update_functions for a
00128     # real-life example of how to make a DiagnosticTask by deriving from
00129     # DiagnosticTask.
00130 
00131     # Alternatively, a FunctionDiagnosticTask is a derived class from
00132     # DiagnosticTask that can be used to create a DiagnosticTask from
00133     # a function. This will be useful when combining multiple diagnostic
00134     # tasks using a CompositeDiagnosticTask.
00135     lower = diagnostic_updater.FunctionDiagnosticTask("Lower-bound check",
00136         check_lower_bound)
00137     upper = diagnostic_updater.FunctionDiagnosticTask("Upper-bound check",
00138         check_upper_bound)
00139 
00140     # If you want to merge the outputs of two diagnostic tasks together, you
00141     # can create a CompositeDiagnosticTask, also a derived class from
00142     # DiagnosticTask. For example, we could combine the upper and lower
00143     # bounds check into a single DiagnosticTask.
00144     bounds = diagnostic_updater.CompositeDiagnosticTask("Bound check")
00145     bounds.addTask(lower)
00146     bounds.addTask(upper)
00147 
00148     # We can then add the CompositeDiagnosticTask to our Updater. When it is
00149     # run, the overall name will be the name of the composite task, i.e.,
00150     # "Bound check". The summary will be a combination of the summary of the
00151     # lower and upper tasks (see DiagnosticStatusWrapper.mergeSummary for
00152     # details on how the merging is done). The lists of key-value pairs will be
00153     # concatenated.
00154     updater.add(bounds)
00155 
00156     # You can broadcast a message in all the DiagnosticStatus if your node
00157     # is in a special state.
00158     updater.broadcast(0, "Doing important initialization stuff.")
00159 
00160     pub1 = rospy.Publisher("topic1", std_msgs.msg.Bool)
00161     pub2_temp = rospy.Publisher("topic2", std_msgs.msg.Bool)
00162     rospy.sleep(2) # It isn't important if it doesn't take time.
00163 
00164     # Some diagnostic tasks are very common, such as checking the rate
00165     # at which a topic is publishing, or checking that timestamps are
00166     # sufficiently recent. FrequencyStatus and TimestampStatus can do these
00167     # checks for you.
00168     #
00169     # Usually you would instantiate them via a HeaderlessTopicDiagnostic
00170     # (FrequencyStatus only, for topics that do not contain a header) or a
00171     # TopicDiagnostic (FrequencyStatus and TimestampStatus, for topics that
00172     # do contain a header).
00173     #
00174     # Some values are passed to the constructor as pointers. If these values
00175     # are changed, the FrequencyStatus/TimestampStatus will start operating
00176     # with the new values.
00177     #
00178     # Refer to diagnostic_updater.FrequencyStatusParam and
00179     # diagnostic_updater.TimestampStatusParam documentation for details on
00180     # what the parameters mean:
00181     freq_bounds = {'min':0.5, 'max':2} # If you update these values, the
00182     # HeaderlessTopicDiagnostic will use the new values.
00183     pub1_freq = diagnostic_updater.HeaderlessTopicDiagnostic("topic1", updater,
00184         diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10))
00185 
00186     # Note that TopicDiagnostic, HeaderlessDiagnosedPublisher,
00187     # HeaderlessDiagnosedPublisher and DiagnosedPublisher all descend from
00188     # CompositeDiagnosticTask, so you can add your own fields to them using
00189     # the addTask method.
00190     #
00191     # Each time pub1_freq is updated, lower will also get updated and its
00192     # output will be merged with the output from pub1_freq.
00193     pub1_freq.addTask(lower) # (This wouldn't work if lower was stateful).
00194 
00195     # If we know that the state of the node just changed, we can force an
00196     # immediate update.
00197     updater.force_update()
00198 
00199     # We can remove a task by refering to its name.
00200     if not updater.removeByName("Bound check"):
00201         rospy.logerr("The Bound check task was not found when trying to remove it.")
00202 
00203     while not rospy.is_shutdown():
00204         msg = std_msgs.msg.Bool()
00205         rospy.sleep(0.1)
00206 
00207         # Calls to pub1 have to be accompanied by calls to pub1_freq to keep
00208         # the statistics up to date.
00209         msg.data = False
00210         pub1.publish(msg)
00211         pub1_freq.tick()
00212 
00213         # We can call updater.update whenever is convenient. It will take care
00214         # of rate-limiting the updates.
00215         updater.update()


diagnostic_updater
Author(s): Jeremy Leibs, Blaise Gassend, Brice Rebsamen
autogenerated on Fri Jan 3 2014 11:18:57