example.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 #include <std_msgs/Bool.h>
38 
39 // ERROR defined in windows.h causes name collision, undefine the macro to fix the issue
40 #ifdef ERROR
41 #undef ERROR
42 #endif
43 
45 
46 /*
47  *\brief Used as a tutorial for loading and using diagnostic updater
48  *
49  * DummyClass and dummy_diagnostics show how to use a diagnostic_updater
50  * class.
51  */
52 
54 {
55  // DiagnosticStatusWrapper are a derived class of
56  // diagnostic_msgs::DiagnosticStatus provides a set of convenience
57  // methods.
58 
59  // summary and summaryf set the level and message.
60  if (time_to_launch < 10)
61  // summaryf for formatted text.
62  stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Buckle your seat belt. Launch in %f seconds!", time_to_launch);
63  else
64  // summary for unformatted text.
65  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Launch is in a long time. Have a soda.");
66 
67  // add and addf are used to append key-value pairs.
68  stat.add("Diagnostic Name", "dummy");
69  // add transparently handles conversion to string (using a string_stream).
70  stat.add("Time to Launch", time_to_launch);
71  // addf allows arbitrary printf style formatting.
72  stat.addf("Geeky thing to say", "The square of the time to launch %f is %f",
74 }
75 
77 {
78 public:
80  {
81  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "This is a silly updater.");
82 
83  stat.add("Stupidicity of this updater", 1000.);
84  }
85 };
86 
88 {
89 public:
90  DummyTask() : DiagnosticTask("Updater Derived from DiagnosticTask")
91  {}
92 
94  {
95  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "This is another silly updater.");
96  stat.add("Stupidicity of this updater", 2000.);
97  }
98 };
99 
101 {
102  if (time_to_launch > 5)
103  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Lower-bound OK");
104  else
106 
107  stat.add("Low-Side Margin", time_to_launch - 5);
108 }
109 
111 {
112  if (time_to_launch < 10)
113  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Upper-bound OK");
114  else
116 
117  stat.add("Top-Side Margin", 10 - time_to_launch);
118 }
119 
120 int main(int argc, char **argv)
121 {
122  ros::init(argc, argv, "diagnostic_updater_example");
123 
124  ros::NodeHandle nh;
125 
126  // The Updater class advertises to /diagnostics, and has a
127  // ~diagnostic_period parameter that says how often the diagnostics
128  // should be published.
130 
131  // The diagnostic_updater::Updater class will fill out the hardware_id
132  // field of the diagnostic_msgs::DiagnosticStatus message. You need to
133  // use the setHardwareID() or setHardwareIDf() methods to set the
134  // hardware ID.
135  //
136  // The hardware ID should be able to identify the specific device you are
137  // working with. If it is not appropriate to fill out a hardware ID in
138  // your case, you should call setHardwareIDf("none") to avoid warnings.
139  // (A warning will be generated as soon as your node updates with no
140  // non-OK statuses.)
141  updater.setHardwareID("none");
142  // Or...
143  updater.setHardwareIDf("Device-%i-%i", 27, 46);
144 
145  // Diagnostic tasks are added to the Updater. They will later be run when
146  // the updater decides to update. The add method is heavily overloaded
147  // for convenienc. Check doxygen for the full list of add methods.
148  updater.add("Function updater", dummy_diagnostic);
149  DummyClass dc;
150  updater.add("Method updater", &dc, &DummyClass::produce_diagnostics);
151 
152  // Internally, updater.add converts its arguments into a DiagnosticTask.
153  // Sometimes it can be useful to work directly with DiagnosticTasks. Look
154  // at FrequencyStatus and TimestampStatus in update_functions.h for a
155  // real-life example of how to make a DiagnosticTask by deriving from
156  // DiagnosticTask.
157 
158  // Alternatively, a FunctionDiagnosticTask is a derived class from
159  // DiagnosticTask that can be used to create a DiagnosticTask from
160  // a function. This will be useful when combining multiple diagnostic
161  // tasks using a CompositeDiagnosticTask.
163  boost::bind(&check_lower_bound, _1));
165  boost::bind(&check_upper_bound, _1));
166 
167  // If you want to merge the outputs of two diagnostic tasks together, you
168  // can create a CompositeDiagnosticTask, also a derived class from
169  // DiagnosticTask. For example, we could combine the upper and lower
170  // bounds check into a single DiagnosticTask.
172  bounds.addTask(&lower);
173  bounds.addTask(&upper);
174 
175  // We can then add the CompositeDiagnosticTask to our Updater. When it is
176  // run, the overall name will be the name of the composite task, i.e.,
177  // "Bound check". The summary will be a combination of the summary of the
178  // lower and upper tasks (see \ref
179  // DiagnosticStatusWrapper::mergeSummarSummary for details on how the
180  // merging is done). The lists of key-value pairs will be concatenated.
181  updater.add(bounds);
182 
183  // You can broadcast a message in all the DiagnosticStatus if your node
184  // is in a special state.
185  updater.broadcast(0, "Doing important initialization stuff.");
186 
187  ros::Publisher pub1 = nh.advertise<std_msgs::Bool>("topic1", 1);
188  ros::Publisher pub2_temp = nh.advertise<std_msgs::Bool>("topic2", 1);
189  ros::Duration(2).sleep(); // It isn't important if it doesn't take time.
190 
191  // Some diagnostic tasks are very common, such as checking the rate
192  // at which a topic is publishing, or checking that timestamps are
193  // sufficiently recent. FrequencyStatus and TimestampStatus can do these
194  // checks for you.
195  //
196  // Usually you would instantiate them via a HeaderlessTopicDiagnostic
197  // (FrequencyStatus only, for topics that do not contain a header) or a
198  // TopicDiagnostic (FrequencyStatus and TimestampStatus, for topics that
199  // do contain a header).
200  //
201  // Some values are passed to the constructor as pointers. If these values
202  // are changed, the FrequencyStatus/TimestampStatus will start operating
203  // with the new values.
204  //
205  // Refer to diagnostic_updater::FrequencyStatusParam and
206  // diagnostic_updater::TimestampStatusParam documentation for details on
207  // what the parameters mean:
208  double min_freq = 0.5; // If you update these values, the
209  double max_freq = 2; // HeaderlessTopicDiagnostic will use the new values.
211  diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10));
212 
213  // Note that TopicDiagnostic, HeaderlessDiagnosedPublisher,
214  // HeaderlessDiagnosedPublisher and DiagnosedPublisher all descend from
215  // CompositeDiagnosticTask, so you can add your own fields to them using
216  // the addTask method.
217  //
218  // Each time pub1_freq is updated, lower will also get updated and its
219  // output will be merged with the output from pub1_freq.
220  pub1_freq.addTask(&lower); // (This wouldn't work if lower was stateful).
221 
222  // If we know that the state of the node just changed, we can force an
223  // immediate update.
224  updater.force_update();
225 
226  // We can remove a task by refering to its name.
227  if (!updater.removeByName("Bound check"))
228  ROS_ERROR("The Bound check task was not found when trying to remove it.");
229 
230  while (nh.ok())
231  {
232  std_msgs::Bool msg;
233  ros::Duration(0.1).sleep();
234 
235  // Calls to pub1 have to be accompanied by calls to pub1_freq to keep
236  // the statistics up to date.
237  msg.data = false;
238  pub1.publish(msg);
239  pub1_freq.tick();
240 
241  // We can call updater.update whenever is convenient. It will take care
242  // of rate-limiting the updates.
243  updater.update();
244  }
245 
246  return 0;
247 }
pub2_temp
Definition: example.py:194
double time_to_launch
Definition: example.cpp:44
void summary(unsigned char lvl, const std::string s)
Fills out the level and message fields of the DiagnosticStatus.
A structure that holds the constructor parameters for the FrequencyStatus class.
void setHardwareID(const std::string &hwid)
void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
Fills out this Task&#39;s DiagnosticStatusWrapper.
Definition: example.cpp:93
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void add(const std::string &name, TaskFunction f)
Add a DiagnosticTask embodied by a name and function to the DiagnosticTaskVector. ...
void addf(const std::string &key, const char *format,...)
Add a key-value pair using a format string.
void check_upper_bound(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: example.cpp:110
Manages a list of diagnostic tasks, and calls them in a rate-limited manner.
void summaryf(unsigned char lvl, const char *format,...)
Formatted version of summary.
pub1_freq
Definition: example.py:216
void publish(const boost::shared_ptr< M > &message) const
A class to facilitate making diagnostics for a topic using a FrequencyStatus.
Definition: publisher.h:55
int main(int argc, char **argv)
Definition: example.cpp:120
void update()
Causes the diagnostics to update if the inter-update interval has been exceeded.
void dummy_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: example.cpp:53
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool removeByName(const std::string name)
Remove a task based on its name.
void produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: example.cpp:79
void force_update()
Forces the diagnostics to update.
DummyTask()
Definition: example.cpp:90
DiagnosticTask is an abstract base class for collecting diagnostic data.
void check_lower_bound(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: example.cpp:100
void add(const std::string &key, const T &val)
Add a key-value pair.
void broadcast(int lvl, const std::string msg)
Output a message on all the known DiagnosticStatus.
bool sleep() const
void setHardwareIDf(const char *format,...)
bool ok() const
virtual void tick()
Signals that a publication has occurred.
Definition: publisher.h:88
#define ROS_ERROR(...)
Merges CompositeDiagnosticTask into a single DiagnosticTask.
Wrapper for the diagnostic_msgs::DiagnosticStatus message that makes it easier to update...
void addTask(DiagnosticTask *t)
Adds a child CompositeDiagnosticTask.


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Mon Feb 28 2022 22:18:16