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


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