generic_analyzer_base.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, 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 
37 #ifndef GENERIC_ANALYZER_BASE_H
38 #define GENERIC_ANALYZER_BASE_H
39 
40 #include <map>
41 #include <ros/ros.h>
42 #include <vector>
43 #include <string>
44 #include <sstream>
45 #include <boost/shared_ptr.hpp>
46 #include <boost/regex.hpp>
48 #include "diagnostic_msgs/DiagnosticStatus.h"
49 #include "diagnostic_msgs/KeyValue.h"
52 
53 namespace diagnostic_aggregator {
54 
65 class GenericAnalyzerBase : public Analyzer
66 {
67 public:
69  nice_name_(""), path_(""), timeout_(-1.0), num_items_expected_(-1),
70  discard_stale_(false), has_initialized_(false), has_warned_(false)
71  { }
72 
73  virtual ~GenericAnalyzerBase() { items_.clear(); }
74 
75  /*
76  *\brief Cannot be initialized from (string, NodeHandle) like defined Analyzers
77  */
78  bool init(const std::string path, const ros::NodeHandle &n) = 0;
79 
80  /*
81  *\brief Must be initialized with path, and a "nice name"
82  *
83  * Must be initialized in order to prepend the path to all outgoing status messages.
84  */
85  bool init(const std::string path, const std::string nice_name,
86  double timeout = -1.0, int num_items_expected = -1, bool discard_stale = false)
87  {
88  num_items_expected_ = num_items_expected;
89  timeout_ = timeout;
90  nice_name_ = nice_name;
91  path_ = path;
92  discard_stale_ = discard_stale;
95 
96  if (discard_stale_ && timeout <= 0)
97  {
98  ROS_WARN("Cannot discard stale items if no timeout specified. No items will be discarded");
99  discard_stale_ = false;
100  }
101 
102  has_initialized_ = true;
103 
104  return true;
105  }
106 
110  virtual bool analyze(const boost::shared_ptr<StatusItem> item)
111  {
112  if (!has_initialized_ && !has_warned_)
113  {
114  has_warned_ = true;
115  ROS_ERROR("GenericAnalyzerBase is asked to analyze diagnostics without being initialized. init() must be called in order to correctly use this class.");
116  }
117 
118  if (!has_initialized_)
119  return false;
120 
121  items_[item->getName()] = item;
122 
123  return has_initialized_;
124  }
125 
131  virtual std::vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > report()
132  {
133  if (!has_initialized_ && !has_warned_)
134  {
135  has_warned_ = true;
136  ROS_ERROR("GenericAnalyzerBase is asked to report diagnostics without being initialized. init() must be called in order to correctly use this class.");
137  }
138  if (!has_initialized_)
139  {
140  std::vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > vec;
141  return vec;
142  }
143 
144  boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> header_status(new diagnostic_msgs::DiagnosticStatus());
145  header_status->name = path_;
146  header_status->level = DiagnosticLevel::Level_OK;
147  header_status->message = "OK";
148 
149  std::vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > processed;
150  processed.push_back(header_status);
151 
152  bool all_stale = true;
153 
154  std::map<std::string, boost::shared_ptr<StatusItem> >::iterator it = items_.begin();
155  while(it != items_.end())
156  {
157  std::string name = it->first;
158  boost::shared_ptr<StatusItem> item = it->second;
159 
160  bool stale = false;
161  if (timeout_ > 0)
162  stale = (ros::Time::now() - item->getLastUpdateTime()).toSec() > timeout_;
163 
164  // Erase item if its stale and we're discarding items
165  if (discard_stale_ && stale)
166  {
167  items_.erase(it++);
168  continue;
169  }
170 
171  const int8_t level = item->getLevel();
172  header_status->level = std::max(header_status->level, level);
173 
174  diagnostic_msgs::KeyValue kv;
175  kv.key = name;
176  kv.value = item->getMessage();
177 
178  header_status->values.push_back(kv);
179 
180  all_stale = all_stale && ((level == int(DiagnosticLevel::Level_Stale)) || stale);
181 
182  //boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> stat = item->toStatusMsg(path_, stale);
183 
184  processed.push_back(item->toStatusMsg(path_, stale));
185 
186  if (stale)
187  header_status->level = DiagnosticLevel::Level_Stale;
188 
189  ++it;
190  }
191 
192  // Header is not stale unless all subs are
193  if (all_stale)
194  header_status->level = DiagnosticLevel::Level_Stale;
195  else if (header_status->level == int(DiagnosticLevel::Level_Stale))
196  header_status->level = DiagnosticLevel::Level_Error;
197 
198  header_status->message = valToMsg(header_status->level);
199 
200  // If we expect a given number of items, check that we have this number
201  if (num_items_expected_ == 0 && items_.size() == 0)
202  {
203  header_status->level = 0;
204  header_status->message = "OK";
205  }
206  else if (num_items_expected_ > 0 && int(items_.size()) != num_items_expected_)
207  {
208  int8_t lvl = DiagnosticLevel::Level_Error;
209  header_status->level = std::max(lvl, header_status->level);
210 
211  std::stringstream expec, item;
212  expec << num_items_expected_;
213  item << items_.size();
214 
215  if (items_.size() > 0)
216  header_status->message = "Expected " + expec.str() + ", found " + item.str();
217  else
218  header_status->message = "No items found, expected " + expec.str();
219  }
220 
221  bool header_stale_timed_out = false;
223  header_stale_timed_out = (ros::Time::now() - last_header_status_change_).toSec() > timeout_;
224 
225  if (last_header_level_ != header_status->level)
226  {
228  last_header_level_ = header_status->level; // update the last header level
229  }
230 
231  if (discard_stale_ && processed.size() == 1 && header_stale_timed_out)
232  {
233  std::vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > vec;
234  return vec;
235  }
236 
237  return processed;
238  }
239 
243  virtual bool match(const std::string name) = 0;
244 
248  virtual std::string getPath() const { return path_; }
249 
253  virtual std::string getName() const { return nice_name_; }
254 
255 protected:
258 
259  double timeout_;
261 
265  void addItem(std::string name, boost::shared_ptr<StatusItem> item) { items_[name] = item; }
266 
267 private:
271  std::map<std::string, boost::shared_ptr<StatusItem> > items_;
272 
274  ros::Time last_header_status_change_; // last timestamp at which the header of the returned diagnostic msg array changed
275  int last_header_level_; // last level reported by the header of the returned diagnostic msg array
276 
277 };
278 
279 }
280 #endif //GENERIC_ANALYZER_BASE_H
diagnostic_aggregator
Definition: aggregator.h:61
diagnostic_aggregator::GenericAnalyzerBase::init
bool init(const std::string path, const ros::NodeHandle &n)=0
Analyzer is initialized with base path and namespace.
diagnostic_aggregator::GenericAnalyzerBase::has_initialized_
bool has_initialized_
Definition: generic_analyzer_base.h:337
boost::shared_ptr
analyzer.h
diagnostic_aggregator::GenericAnalyzerBase::nice_name_
std::string nice_name_
Definition: generic_analyzer_base.h:320
diagnostic_aggregator::GenericAnalyzerBase::last_header_level_
int last_header_level_
Definition: generic_analyzer_base.h:339
ros.h
diagnostic_aggregator::Level_Error
@ Level_Error
Definition: status_item.h:108
status_item.h
diagnostic_aggregator::GenericAnalyzerBase::analyze
virtual bool analyze(const boost::shared_ptr< StatusItem > item)
Update state with new StatusItem.
Definition: generic_analyzer_base.h:174
diagnostic_aggregator::GenericAnalyzerBase::path_
std::string path_
Definition: generic_analyzer_base.h:321
testing::internal::string
::std::string string
Definition: gtest.h:1979
diagnostic_aggregator::GenericAnalyzerBase::items_
std::map< std::string, boost::shared_ptr< StatusItem > > items_
Stores items by name. State of analyzer.
Definition: generic_analyzer_base.h:335
diagnostic_aggregator::GenericAnalyzerBase::timeout_
double timeout_
Definition: generic_analyzer_base.h:323
diagnostic_aggregator::GenericAnalyzerBase::GenericAnalyzerBase
GenericAnalyzerBase()
Definition: generic_analyzer_base.h:132
diagnostic_aggregator::Level_OK
@ Level_OK
Definition: status_item.h:106
diagnostic_aggregator::GenericAnalyzerBase::discard_stale_
bool discard_stale_
Definition: generic_analyzer_base.h:337
diagnostic_aggregator::GenericAnalyzerBase::getName
virtual std::string getName() const
Returns nice name (ex: "Power System")
Definition: generic_analyzer_base.h:317
diagnostic_aggregator::GenericAnalyzerBase::getPath
virtual std::string getPath() const
Returns full prefix (ex: "/Robot/Power System")
Definition: generic_analyzer_base.h:312
ROS_WARN
#define ROS_WARN(...)
diagnostic_aggregator::valToMsg
std::string valToMsg(const int val)
Converts int to message {0: 'OK', 1: 'Warning', 2: 'Error', 3: 'Stale' }.
Definition: status_item.h:133
diagnostic_aggregator::GenericAnalyzerBase::~GenericAnalyzerBase
virtual ~GenericAnalyzerBase()
Definition: generic_analyzer_base.h:137
diagnostic_aggregator::GenericAnalyzerBase::report
virtual std::vector< boost::shared_ptr< diagnostic_msgs::DiagnosticStatus > > report()
Reports current state, returns vector of formatted status messages.
Definition: generic_analyzer_base.h:195
ros::Time
diagnostic_aggregator::GenericAnalyzerBase::num_items_expected_
int num_items_expected_
Definition: generic_analyzer_base.h:324
ROS_ERROR
#define ROS_ERROR(...)
class_list_macros.hpp
diagnostic_aggregator::GenericAnalyzerBase::addItem
void addItem(std::string name, boost::shared_ptr< StatusItem > item)
Subclasses can add items to analyze.
Definition: generic_analyzer_base.h:329
diagnostic_aggregator::Level_Stale
@ Level_Stale
Definition: status_item.h:109
diagnostic_aggregator::GenericAnalyzerBase::match
virtual bool match(const std::string name)=0
Match function isn't implemented by GenericAnalyzerBase.
diagnostic_aggregator::GenericAnalyzerBase::has_warned_
bool has_warned_
Definition: generic_analyzer_base.h:337
diagnostic_aggregator::GenericAnalyzerBase::last_header_status_change_
ros::Time last_header_status_change_
Definition: generic_analyzer_base.h:338
ros::NodeHandle
ros::Time::now
static Time now()


diagnostic_aggregator
Author(s): Kevin Watts, Brice Rebsamen
autogenerated on Tue Nov 15 2022 03:17:12