example.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  * 
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  * 
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  * 
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <diagnostic_updater/diagnostic_updater.h>
00036 #include <std_msgs/Bool.h>
00037 #include <diagnostic_updater/publisher.h>
00038 
00039 double time_to_launch;
00040 
00041 /*
00042  *\brief Used as a tutorial for loading and using diagnostic updater
00043  *
00044  * DummyClass and dummy_diagnostics show how to use a diagnostic_updater
00045  * class.
00046  */
00047 
00048 void dummy_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
00049 {
00050   // DiagnosticStatusWrapper are a derived class of 
00051   // diagnostic_msgs::DiagnosticStatus provides a set of convenience
00052   // methods.
00053   
00054   // summary and summaryf set the level and message.
00055   if (time_to_launch < 10)
00056     // summaryf for formatted text.
00057     stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Buckle your seat belt. Launch in %f seconds!", time_to_launch);
00058   else
00059     // summary for unformatted text.
00060     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Launch is in a long time. Have a soda.");
00061 
00062   // add and addf are used to append key-value pairs.
00063   stat.add("Diagnostic Name", "dummy");
00064   // add transparently handles conversion to string (using a string_stream).
00065   stat.add("Time to Launch", time_to_launch);
00066   // addf allows arbitrary printf style formatting.
00067   stat.addf("Geeky thing to say", "The square of the time to launch %f is %f", 
00068       time_to_launch, time_to_launch * time_to_launch);
00069 }
00070 
00071 class DummyClass
00072 {
00073 public:
00074   void produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
00075   {
00076     stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "This is a silly updater.");
00077 
00078     stat.add("Stupidicity of this updater", 1000.);
00079   }
00080 };
00081 
00082 class DummyTask : public diagnostic_updater::DiagnosticTask
00083 {
00084 public:
00085   DummyTask() : DiagnosticTask("Updater Derived from DiagnosticTask")
00086   {}
00087 
00088   void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
00089   {
00090     stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "This is another silly updater.");
00091     stat.add("Stupidicity of this updater", 2000.);
00092   }
00093 };
00094 
00095 void check_lower_bound(diagnostic_updater::DiagnosticStatusWrapper &stat)
00096 {
00097   if (time_to_launch > 5)
00098     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Lower-bound OK");
00099   else
00100     stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Too low");
00101 
00102   stat.add("Low-Side Margin", time_to_launch - 5);
00103 }
00104 
00105 void check_upper_bound(diagnostic_updater::DiagnosticStatusWrapper &stat)
00106 {
00107   if (time_to_launch < 10)
00108     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Upper-bound OK");
00109   else
00110     stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Too high");
00111 
00112   stat.add("Top-Side Margin", 10 - time_to_launch);
00113 }
00114 
00115 int main(int argc, char **argv)
00116 {
00117   ros::init(argc, argv, "diagnostic_updater_example");
00118   
00119   ros::NodeHandle nh;
00120   
00121   // The Updater class advertises to /diagnostics, and has a
00122   // ~diagnostic_period parameter that says how often the diagnostics
00123   // should be published.
00124   diagnostic_updater::Updater updater;
00125 
00126   // The diagnostic_updater::Updater class will fill out the hardware_id
00127   // field of the diagnostic_msgs::DiagnosticStatus message. You need to
00128   // use the setHardwareID() or setHardwareIDf() methods to set the
00129   // hardware ID. 
00130   //
00131   // The hardware ID should be able to identify the specific device you are
00132   // working with.  If it is not appropriate to fill out a hardware ID in
00133   // your case, you should call setHardwareIDf("none") to avoid warnings.
00134   // (A warning will be generated as soon as your node updates with no
00135   // non-OK statuses.)
00136   updater.setHardwareID("none"); 
00137   // Or...
00138   updater.setHardwareIDf("Device-%i-%i", 27, 46);
00139 
00140   // Diagnostic tasks are added to the Updater. They will later be run when
00141   // the updater decides to update. The add method is heavily overloaded
00142   // for convenienc. Check doxygen for the full list of add methods.
00143   updater.add("Function updater", dummy_diagnostic);
00144   DummyClass dc;
00145   updater.add("Method updater", &dc, &DummyClass::produce_diagnostics);
00146   
00147   // Internally, updater.add converts its arguments into a DiagnosticTask.
00148   // Sometimes it can be useful to work directly with DiagnosticTasks. Look
00149   // at FrequencyStatus and TimestampStatus in update_functions.h for a
00150   // real-life example of how to make a DiagnosticTask by deriving from
00151   // DiagnosticTask.
00152   
00153   // Alternatively, a FunctionDiagnosticTask is a derived class from
00154   // DiagnosticTask that can be used to create a DiagnosticTask from
00155   // a function. This will be useful when combining multiple diagnostic
00156   // tasks using a CompositeDiagnosticTask.
00157   diagnostic_updater::FunctionDiagnosticTask lower("Lower-bound check",
00158       boost::bind(&check_lower_bound, _1));
00159   diagnostic_updater::FunctionDiagnosticTask upper("Upper-bound check",
00160       boost::bind(&check_upper_bound, _1));
00161 
00162   // If you want to merge the outputs of two diagnostic tasks together, you
00163   // can create a CompositeDiagnosticTask, also a derived class from
00164   // DiagnosticTask. For example, we could combine the upper and lower
00165   // bounds check into a single DiagnosticTask.
00166   diagnostic_updater::CompositeDiagnosticTask bounds("Bound check");
00167   bounds.addTask(&lower);
00168   bounds.addTask(&upper);
00169 
00170   // We can then add the CompositeDiagnosticTask to our Updater. When it is
00171   // run, the overall name will be the name of the composite task, i.e., 
00172   // "Bound check". The summary will be a combination of the summary of the
00173   // lower and upper tasks (see \ref
00174   // DiagnosticStatusWrapper::mergeSummarSummary for details on how the
00175   // merging is done). The lists of key-value pairs will be concatenated.
00176   updater.add(bounds);
00177 
00178   // You can broadcast a message in all the DiagnosticStatus if your node
00179   // is in a special state.
00180   updater.broadcast(0, "Doing important initialization stuff.");
00181 
00182   ros::Publisher pub1 = nh.advertise<std_msgs::Bool>("topic1", 1);
00183   ros::Publisher pub2_temp = nh.advertise<std_msgs::Bool>("topic2", 1);
00184   ros::Duration(2).sleep(); // It isn't important if it doesn't take time.
00185 
00186   // Some diagnostic tasks are very common, such as checking the rate
00187   // at which a topic is publishing, or checking that timestamps are
00188   // sufficiently recent. FrequencyStatus and TimestampStatus can do these
00189   // checks for you. 
00190   //
00191   // Usually you would instantiate them via a HeaderlessTopicDiagnostic
00192   // (FrequencyStatus only, for topics that do not contain a header) or a
00193   // TopicDiagnostic (FrequencyStatus and TimestampStatus, for topics that
00194   // do contain a header). 
00195   //
00196   // Some values are passed to the constructor as pointers. If these values
00197   // are changed, the FrequencyStatus/TimestampStatus will start operating
00198   // with the new values. 
00199   //
00200   // Refer to diagnostic_updater::FrequencyStatusParam and
00201   // diagnostic_updater::TimestampStatusParam documentation for details on
00202   // what the parameters mean:
00203   double min_freq = 0.5; // If you update these values, the
00204   double max_freq = 2; // HeaderlessTopicDiagnostic will use the new values.
00205   diagnostic_updater::HeaderlessTopicDiagnostic pub1_freq("topic1", updater,
00206       diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10));
00207 
00208   // Note that TopicDiagnostic, HeaderlessDiagnosedPublisher,
00209   // HeaderlessDiagnosedPublisher and DiagnosedPublisher all descend from
00210   // CompositeDiagnosticTask, so you can add your own fields to them using
00211   // the addTask method.
00212   //
00213   // Each time pub1_freq is updated, lower will also get updated and its
00214   // output will be merged with the output from pub1_freq.
00215   pub1_freq.addTask(&lower); // (This wouldn't work if lower was stateful).
00216 
00217   // If we know that the state of the node just changed, we can force an
00218   // immediate update.
00219   updater.force_update();
00220 
00221   // We can remove a task by refering to its name.
00222   if (!updater.removeByName("Bound check"))
00223     ROS_ERROR("The Bound check task was not found when trying to remove it.");
00224 
00225   while (nh.ok())
00226   {
00227     std_msgs::Bool msg;
00228     ros::Duration(0.1).sleep();
00229     
00230     // Calls to pub1 have to be accompanied by calls to pub1_freq to keep
00231     // the statistics up to date.
00232     msg.data = false;
00233     pub1.publish(msg);
00234     pub1_freq.tick();
00235 
00236     // We can call updater.update whenever is convenient. It will take care
00237     // of rate-limiting the updates.
00238     updater.update();
00239   }
00240 
00241   return 0; 
00242 }


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Mon Aug 14 2017 02:52:20