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


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Tue Mar 26 2019 03:09:44