analyzer_group.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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 
00037 #include <diagnostic_aggregator/analyzer_group.h>
00038 
00039 using namespace std;
00040 using namespace diagnostic_aggregator;
00041 
00042 PLUGINLIB_EXPORT_CLASS(diagnostic_aggregator::AnalyzerGroup, 
00043                         diagnostic_aggregator::Analyzer)
00044 
00045 AnalyzerGroup::AnalyzerGroup() :
00046   path_(""),
00047   nice_name_(""),
00048   analyzer_loader_("diagnostic_aggregator", "diagnostic_aggregator::Analyzer")
00049 { }
00050 
00051 bool AnalyzerGroup::init(const string base_path, const ros::NodeHandle &n)
00052 {
00053   n.param("path", nice_name_, string(""));
00054   
00055   if (base_path.size() > 0 && base_path != "/")
00056     path_ = base_path + "/" + nice_name_;
00057   else
00058     path_ = nice_name_;
00059 
00060 
00061   if (path_.find("/") != 0)
00062     path_ = "/" + path_;
00063 
00064   ros::NodeHandle analyzers_nh = ros::NodeHandle(n, "analyzers");
00065     
00066   XmlRpc::XmlRpcValue analyzer_params;
00067   analyzers_nh.getParam("", analyzer_params);
00068   ROS_DEBUG("Analyzer params: %s.", analyzer_params.toXml().c_str());
00069 
00070   bool init_ok = true;
00071 
00072   XmlRpc::XmlRpcValue::iterator xml_it;
00073   for (xml_it = analyzer_params.begin(); xml_it != analyzer_params.end(); ++xml_it)
00074   {
00075     XmlRpc::XmlRpcValue analyzer_name = xml_it->first;
00076     ROS_DEBUG("Got analyzer name: %s", analyzer_name.toXml().c_str());
00077     XmlRpc::XmlRpcValue analyzer_value = xml_it->second;
00078 
00079     string ns = analyzer_name;
00080     
00081     if (!analyzer_value.hasMember("type"))
00082     {
00083       ROS_ERROR("Namespace %s has no member 'type', unable to initialize analyzer for this namespace.", analyzers_nh.getNamespace().c_str());
00084       boost::shared_ptr<StatusItem> item(new StatusItem(ns, "No Analyzer type given"));
00085       aux_items_.push_back(item);
00086       init_ok = false;
00087       continue;
00088     }
00089 
00090     XmlRpc::XmlRpcValue analyzer_type = analyzer_value["type"];
00091     string an_type = analyzer_type;
00092     
00093     boost::shared_ptr<Analyzer> analyzer;
00094     try
00095     {
00096       // Look for non-fully qualified class name for Analyzer type
00097       if (!analyzer_loader_.isClassAvailable(an_type))
00098       {
00099         bool have_class = false;
00100         vector<string> classes = analyzer_loader_.getDeclaredClasses();
00101         for(unsigned int i = 0; i < classes.size(); ++i)
00102         {
00103           if(an_type == analyzer_loader_.getName(classes[i]))
00104           {
00105             //if we've found a match... we'll get the fully qualified name and break out of the loop
00106             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.",
00107                      an_type.c_str(), classes[i].c_str());
00108             an_type = classes[i];
00109             have_class = true;
00110             break;
00111           }
00112         }
00113         if (!have_class)
00114         {
00115           ROS_ERROR("Unable to find Analyzer class %s. Check that Analyzer is fully declared.", an_type.c_str());
00116           continue;
00117         }
00118       }
00119 
00120       analyzer = analyzer_loader_.createInstance(an_type);
00121     }
00122     catch (pluginlib::LibraryLoadException& e)
00123     {
00124       ROS_ERROR("Failed to load analyzer %s, type %s. Caught exception. %s", ns.c_str(), an_type.c_str(), e.what());
00125       boost::shared_ptr<StatusItem> item(new StatusItem(ns, "Pluginlib exception loading analyzer"));
00126       aux_items_.push_back(item);
00127       init_ok = false;
00128       continue;
00129     }
00130 
00131     if (!analyzer)
00132     {
00133       ROS_ERROR("Pluginlib returned a null analyzer for %s, namespace %s.", an_type.c_str(), analyzers_nh.getNamespace().c_str());
00134       boost::shared_ptr<StatusItem> item(new StatusItem(ns, "Pluginlib return NULL Analyzer for " + an_type));
00135       aux_items_.push_back(item);
00136       init_ok = false;
00137       continue;
00138     }
00139     
00140     if (!analyzer->init(path_, ros::NodeHandle(analyzers_nh, ns)))
00141     {
00142       ROS_ERROR("Unable to initialize analyzer NS: %s, type: %s", analyzers_nh.getNamespace().c_str(), an_type.c_str());
00143       boost::shared_ptr<StatusItem> item(new StatusItem(ns, "Analyzer init failed"));
00144       aux_items_.push_back(item);
00145       init_ok = false;
00146       continue;
00147     }
00148     
00149     analyzers_.push_back(analyzer);
00150   }
00151 
00152   if (analyzers_.size() == 0)
00153   {
00154     init_ok = false;
00155     ROS_ERROR("No analyzers initialized in AnalyzerGroup %s", analyzers_nh.getNamespace().c_str());
00156   }
00157 
00158   return init_ok;
00159 }
00160 
00161 AnalyzerGroup::~AnalyzerGroup()
00162 {
00163   analyzers_.clear();
00164 }
00165 
00166 bool AnalyzerGroup::addAnalyzer(boost::shared_ptr<Analyzer>& analyzer)
00167 {
00168   analyzers_.push_back(analyzer);
00169   return true;
00170 }
00171 
00172 bool AnalyzerGroup::removeAnalyzer(boost::shared_ptr<Analyzer>& analyzer)
00173 {
00174   vector<boost::shared_ptr<Analyzer> >::iterator it = find(analyzers_.begin(), analyzers_.end(), analyzer);
00175   if (it != analyzers_.end())
00176   {
00177     analyzers_.erase(it);
00178     return true;
00179   }
00180   return false;
00181 }
00182 
00183 bool AnalyzerGroup::match(const string name)
00184 {
00185   if (analyzers_.size() == 0)
00186     return false;
00187 
00188   bool match_name = false;
00189   if (matched_.count(name))
00190   {
00191     vector<bool> &mtch_vec = matched_[name];
00192     for (unsigned int i = 0; i < mtch_vec.size(); ++i)
00193     {
00194       if (mtch_vec[i])
00195         return true;
00196     }
00197     return false;
00198   }
00199   
00200   matched_[name].resize(analyzers_.size());
00201   for (unsigned int i = 0; i < analyzers_.size(); ++i)
00202   {
00203     bool mtch = analyzers_[i]->match(name);
00204     match_name = mtch || match_name;
00205     matched_[name].at(i) = mtch;
00206   }
00207 
00208   return match_name;
00209 }
00210 
00211 void AnalyzerGroup::resetMatches()
00212 {
00213   matched_.clear();
00214 }
00215 
00216 
00217 bool AnalyzerGroup::analyze(const boost::shared_ptr<StatusItem> item)
00218 {
00219   ROS_ASSERT_MSG(matched_.count(item->getName()), "AnalyzerGroup was asked to analyze an item it hadn't matched.");
00220 
00221   bool analyzed = false;
00222   vector<bool> &mtch_vec = matched_[item->getName()];
00223   for (unsigned int i = 0; i < mtch_vec.size(); ++i)
00224   {
00225     if (mtch_vec[i])
00226       analyzed = analyzers_[i]->analyze(item) || analyzed;
00227   }
00228   
00229   return analyzed;
00230 }
00231 
00232 vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > AnalyzerGroup::report()
00233 {
00234   vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > output;
00235 
00236   boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> header_status(new diagnostic_msgs::DiagnosticStatus);
00237   header_status->name = path_;
00238   header_status->level = 0;
00239   header_status->message = "OK";
00240 
00241   if (analyzers_.size() == 0)
00242   {
00243     header_status->level = 2;
00244     header_status->message = "No analyzers";
00245     output.push_back(header_status);
00246     
00247     if (header_status->name == "" || header_status->name == "/")
00248       header_status->name = "/AnalyzerGroup";
00249 
00250     return output;
00251   }
00252 
00253   bool all_stale = true;
00254 
00255   for (unsigned int j = 0; j < analyzers_.size(); ++j)
00256   {
00257     string path = analyzers_[j]->getPath();
00258     string nice_name = analyzers_[j]->getName();
00259 
00260     vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > processed = analyzers_[j]->report();
00261 
00262     // Do not report anything in the header values for analyzers that don't report
00263     if (processed.size() == 0)
00264       continue;
00265 
00266     // Look through processed data for header, append it to header_status
00267     // Ex: Look for /Robot/Power and append (Power, OK) to header
00268     for (unsigned int i = 0; i < processed.size(); ++i)
00269     {
00270       output.push_back(processed[i]);
00271 
00272       // Add to header status
00273       if (processed[i]->name == path)
00274       {
00275         diagnostic_msgs::KeyValue kv;
00276         kv.key = nice_name;
00277         kv.value = processed[i]->message;
00278         
00279         all_stale = all_stale && (processed[i]->level == 3);
00280         header_status->level = max(header_status->level, processed[i]->level);
00281         header_status->values.push_back(kv);
00282       }
00283     }
00284   }
00285 
00286   // Report stale as errors unless all stale
00287   if (header_status->level == 3 && !all_stale)
00288     header_status->level = 2;
00289 
00290   header_status->message = valToMsg(header_status->level);
00291 
00292   if (path_ != "" && path_ != "/") // No header if we don't have a base path
00293   {
00294     output.push_back(header_status);
00295   }
00296 
00297   for (unsigned int i = 0; i < aux_items_.size(); ++i)
00298   {
00299     output.push_back(aux_items_[i]->toStatusMsg(path_, true));
00300   }
00301 
00302   return output;
00303 }


diagnostic_aggregator
Author(s): Kevin Watts, Brice Rebsamen
autogenerated on Mon Aug 14 2017 02:51:51