analyzer_group.cpp
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 
38 
39 using namespace std;
40 using namespace diagnostic_aggregator;
41 
44 
46  path_(""),
47  nice_name_(""),
48  analyzer_loader_("diagnostic_aggregator", "diagnostic_aggregator::Analyzer")
49 { }
50 
51 bool AnalyzerGroup::init(const string base_path, const ros::NodeHandle &n)
52 {
53  n.param("path", nice_name_, string(""));
54 
55  if (base_path.size() > 0 && base_path != "/")
56  if (nice_name_.size() > 0)
57  path_ = base_path + "/" + nice_name_;
58  else
59  path_ = base_path;
60  else
61  path_ = nice_name_;
62 
63 
64  if (path_.find("/") != 0)
65  path_ = "/" + path_;
66 
67  ros::NodeHandle analyzers_nh = ros::NodeHandle(n, "analyzers");
68 
69  XmlRpc::XmlRpcValue analyzer_params;
70  analyzers_nh.getParam("", analyzer_params);
71  ROS_DEBUG("Analyzer params: %s.", analyzer_params.toXml().c_str());
72 
73  bool init_ok = true;
74 
76  for (xml_it = analyzer_params.begin(); xml_it != analyzer_params.end(); ++xml_it)
77  {
78  XmlRpc::XmlRpcValue analyzer_name = xml_it->first;
79  ROS_DEBUG("Got analyzer name: %s", analyzer_name.toXml().c_str());
80  XmlRpc::XmlRpcValue analyzer_value = xml_it->second;
81 
82  string ns = analyzer_name;
83 
84  if (!analyzer_value.hasMember("type"))
85  {
86  ROS_ERROR("Namespace %s has no member 'type', unable to initialize analyzer for this namespace.", analyzers_nh.getNamespace().c_str());
87  boost::shared_ptr<StatusItem> item(new StatusItem(ns, "No Analyzer type given"));
88  aux_items_.push_back(item);
89  init_ok = false;
90  continue;
91  }
92 
93  XmlRpc::XmlRpcValue analyzer_type = analyzer_value["type"];
94  string an_type = analyzer_type;
95 
97  try
98  {
99  // Look for non-fully qualified class name for Analyzer type
100  if (!analyzer_loader_.isClassAvailable(an_type))
101  {
102  bool have_class = false;
103  vector<string> classes = analyzer_loader_.getDeclaredClasses();
104  for(unsigned int i = 0; i < classes.size(); ++i)
105  {
106  if(an_type == analyzer_loader_.getName(classes[i]))
107  {
108  //if we've found a match... we'll get the fully qualified name and break out of the loop
109  ROS_WARN("Analyzer specification should now include the package name. You are using a deprecated API. Please switch from %s to %s in your Analyzer specification.",
110  an_type.c_str(), classes[i].c_str());
111  an_type = classes[i];
112  have_class = true;
113  break;
114  }
115  }
116  if (!have_class)
117  {
118  ROS_ERROR("Unable to find Analyzer class %s. Check that Analyzer is fully declared.", an_type.c_str());
119  continue;
120  }
121  }
122 
123  analyzer = analyzer_loader_.createInstance(an_type);
124  }
126  {
127  ROS_ERROR("Failed to load analyzer %s, type %s. Caught exception. %s", ns.c_str(), an_type.c_str(), e.what());
128  boost::shared_ptr<StatusItem> item(new StatusItem(ns, "Pluginlib exception loading analyzer"));
129  aux_items_.push_back(item);
130  init_ok = false;
131  continue;
132  }
133 
134  if (!analyzer)
135  {
136  ROS_ERROR("Pluginlib returned a null analyzer for %s, namespace %s.", an_type.c_str(), analyzers_nh.getNamespace().c_str());
137  boost::shared_ptr<StatusItem> item(new StatusItem(ns, "Pluginlib return NULL Analyzer for " + an_type));
138  aux_items_.push_back(item);
139  init_ok = false;
140  continue;
141  }
142 
143  if (!analyzer->init(path_, ros::NodeHandle(analyzers_nh, ns)))
144  {
145  ROS_ERROR("Unable to initialize analyzer NS: %s, type: %s", analyzers_nh.getNamespace().c_str(), an_type.c_str());
146  boost::shared_ptr<StatusItem> item(new StatusItem(ns, "Analyzer init failed"));
147  aux_items_.push_back(item);
148  init_ok = false;
149  continue;
150  }
151 
152  analyzers_.push_back(analyzer);
153  }
154 
155  if (analyzers_.size() == 0)
156  {
157  init_ok = false;
158  ROS_ERROR("No analyzers initialized in AnalyzerGroup %s", analyzers_nh.getNamespace().c_str());
159  }
160 
161  return init_ok;
162 }
163 
164 AnalyzerGroup::~AnalyzerGroup()
165 {
166  analyzers_.clear();
167 }
168 
169 bool AnalyzerGroup::addAnalyzer(boost::shared_ptr<Analyzer>& analyzer)
170 {
171  analyzers_.push_back(analyzer);
172  return true;
173 }
174 
175 bool AnalyzerGroup::removeAnalyzer(boost::shared_ptr<Analyzer>& analyzer)
176 {
177  vector<boost::shared_ptr<Analyzer> >::iterator it = find(analyzers_.begin(), analyzers_.end(), analyzer);
178  if (it != analyzers_.end())
179  {
180  analyzers_.erase(it);
181  return true;
182  }
183  return false;
184 }
185 
186 bool AnalyzerGroup::match(const string name)
187 {
188  if (analyzers_.size() == 0)
189  return false;
190 
191  bool match_name = false;
192  if (matched_.count(name))
193  {
194  vector<bool> &mtch_vec = matched_[name];
195  for (unsigned int i = 0; i < mtch_vec.size(); ++i)
196  {
197  if (mtch_vec[i])
198  return true;
199  }
200  return false;
201  }
202 
203  matched_[name].resize(analyzers_.size());
204  for (unsigned int i = 0; i < analyzers_.size(); ++i)
205  {
206  bool mtch = analyzers_[i]->match(name);
207  match_name = mtch || match_name;
208  matched_[name].at(i) = mtch;
209  }
210 
211  return match_name;
212 }
213 
214 void AnalyzerGroup::resetMatches()
215 {
216  matched_.clear();
217 }
218 
219 
220 bool AnalyzerGroup::analyze(const boost::shared_ptr<StatusItem> item)
221 {
222  ROS_ASSERT_MSG(matched_.count(item->getName()), "AnalyzerGroup was asked to analyze an item it hadn't matched.");
223 
224  bool analyzed = false;
225  vector<bool> &mtch_vec = matched_[item->getName()];
226  for (unsigned int i = 0; i < mtch_vec.size(); ++i)
227  {
228  if (mtch_vec[i])
229  analyzed = analyzers_[i]->analyze(item) || analyzed;
230  }
231 
232  return analyzed;
233 }
234 
235 vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > AnalyzerGroup::report()
236 {
237  vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > output;
238 
239  boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> header_status(new diagnostic_msgs::DiagnosticStatus);
240  header_status->name = path_;
241  header_status->level = 0;
242  header_status->message = "OK";
243 
244  if (analyzers_.size() == 0)
245  {
246  header_status->level = 2;
247  header_status->message = "No analyzers";
248  output.push_back(header_status);
249 
250  if (header_status->name == "" || header_status->name == "/")
251  header_status->name = "/AnalyzerGroup";
252 
253  return output;
254  }
255 
256  bool all_stale = true;
257 
258  for (unsigned int j = 0; j < analyzers_.size(); ++j)
259  {
260  string path = analyzers_[j]->getPath();
261  string nice_name = analyzers_[j]->getName();
262 
263  vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > processed = analyzers_[j]->report();
264 
265  // Do not report anything in the header values for analyzers that don't report
266  if (processed.size() == 0)
267  continue;
268 
269  // Look through processed data for header, append it to header_status
270  // Ex: Look for /Robot/Power and append (Power, OK) to header
271  for (unsigned int i = 0; i < processed.size(); ++i)
272  {
273  output.push_back(processed[i]);
274 
275  // Add to header status
276  if (processed[i]->name == path)
277  {
278  diagnostic_msgs::KeyValue kv;
279  kv.key = nice_name;
280  kv.value = processed[i]->message;
281 
282  all_stale = all_stale && (processed[i]->level == 3);
283  header_status->level = max(header_status->level, processed[i]->level);
284  header_status->values.push_back(kv);
285  }
286  }
287  }
288 
289  // Report stale as errors unless all stale
290  if (header_status->level == 3 && !all_stale)
291  header_status->level = 2;
292 
293  header_status->message = valToMsg(header_status->level);
294 
295  if (path_ != "" && path_ != "/") // No header if we don't have a base path
296  {
297  output.push_back(header_status);
298  }
299 
300  for (unsigned int i = 0; i < aux_items_.size(); ++i)
301  {
302  output.push_back(aux_items_[i]->toStatusMsg(path_, true));
303  }
304 
305  return output;
306 }
ValueStruct::iterator iterator
std::string valToMsg(const int val)
Converts int to message {0: &#39;OK&#39;, 1: &#39;Warning&#39;, 2: &#39;Error&#39;, 3: &#39;Stale&#39; }.
Definition: status_item.h:101
#define ROS_WARN(...)
std::string toXml() const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define ROS_ASSERT_MSG(cond,...)
const std::string & getNamespace() const
Base class of all Analyzers. Loaded by aggregator.
Definition: analyzer.h:85
Allows analyzers to be grouped together, or used as sub-analyzers.
bool getParam(const std::string &key, std::string &s) const
Helper class to hold, store DiagnosticStatus messages.
Definition: status_item.h:158
PLUGINLIB_EXPORT_CLASS(diagnostic_aggregator::AnalyzerGroup, diagnostic_aggregator::Analyzer) AnalyzerGroup
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


diagnostic_aggregator
Author(s): Kevin Watts, Brice Rebsamen
autogenerated on Thu Oct 8 2020 03:17:16