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


diagnostic_aggregator
Author(s): Kevin Watts, Brice Rebsamen
autogenerated on Fri Aug 28 2015 10:30:07