diagnostic_aggregator: diagnostic_aggregator: analyzer

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     Analyzer* analyzer = NULL;
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_.createClassInstance(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 == NULL)
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   for (unsigned int i = 0; i < analyzers_.size(); ++i)
00166     delete analyzers_[i];
00167   analyzers_.clear();
00168 }
00169 
00170 bool AnalyzerGroup::match(const string name)
00171 {
00172   if (analyzers_.size() == 0)
00173     return false;
00174 
00175   bool match_name = false;
00176   if (matched_.count(name))
00177   {
00178     vector<bool> &mtch_vec = matched_[name];
00179     for (unsigned int i = 0; i < mtch_vec.size(); ++i)
00180     {
00181       if (mtch_vec[i])
00182         return true;
00183     }
00184     return false;
00185   }
00186   
00187   matched_[name].resize(analyzers_.size());
00188   for (unsigned int i = 0; i < analyzers_.size(); ++i)
00189   {
00190     bool mtch = analyzers_[i]->match(name);
00191     match_name = mtch || match_name;
00192     matched_[name].at(i) = mtch;
00193   }
00194 
00195   return match_name;
00196 }
00197 
00198 bool AnalyzerGroup::analyze(const boost::shared_ptr<StatusItem> item)
00199 {
00200   ROS_ASSERT_MSG(matched_.count(item->getName()), "AnalyzerGroup was asked to analyze an item it hadn't matched.");
00201 
00202   bool analyzed = false;
00203   vector<bool> &mtch_vec = matched_[item->getName()];
00204   for (unsigned int i = 0; i < mtch_vec.size(); ++i)
00205   {
00206     if (mtch_vec[i])
00207       analyzed = analyzers_[i]->analyze(item) || analyzed;
00208   }
00209   
00210   return analyzed;
00211 }
00212 
00213 vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > AnalyzerGroup::report()
00214 {
00215   vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > output;
00216 
00217   boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> header_status(new diagnostic_msgs::DiagnosticStatus);
00218   header_status->name = path_;
00219   header_status->level = 0;
00220   header_status->message = "OK";
00221 
00222   if (analyzers_.size() == 0)
00223   {
00224     header_status->level = 2;
00225     header_status->message = "No analyzers";
00226     output.push_back(header_status);
00227     
00228     if (header_status->name == "" || header_status->name == "/")
00229       header_status->name = "/AnalyzerGroup";
00230 
00231     return output;
00232   }
00233 
00234   bool all_stale = true;
00235 
00236   for (unsigned int j = 0; j < analyzers_.size(); ++j)
00237   {
00238     string path = analyzers_[j]->getPath();
00239     string nice_name = analyzers_[j]->getName();
00240 
00241     vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > processed = analyzers_[j]->report();
00242 
00243     // Do not report anything in the header values for analyzers that don't report
00244     if (processed.size() == 0)
00245       continue;
00246 
00247     // Look through processed data for header, append it to header_status
00248     // Ex: Look for /Robot/Power and append (Power, OK) to header
00249     for (unsigned int i = 0; i < processed.size(); ++i)
00250     {
00251       output.push_back(processed[i]);
00252 
00253       // Add to header status
00254       if (processed[i]->name == path)
00255       {
00256         diagnostic_msgs::KeyValue kv;
00257         kv.key = nice_name;
00258         kv.value = processed[i]->message;
00259         
00260         all_stale = all_stale && (processed[i]->level == 3);
00261         header_status->level = max(header_status->level, processed[i]->level);
00262         header_status->values.push_back(kv);
00263       }
00264     }
00265   }
00266 
00267   // Report stale as errors unless all stale
00268   if (header_status->level == 3 && !all_stale)
00269     header_status->level = 2;
00270 
00271   header_status->message = valToMsg(header_status->level);
00272 
00273   if (path_ != "" && path_ != "/") // No header if we don't have a base path
00274   {
00275     output.push_back(header_status);
00276   }
00277 
00278   for (unsigned int i = 0; i < aux_items_.size(); ++i)
00279   {
00280     output.push_back(aux_items_[i]->toStatusMsg(path_, true));
00281   }
00282 
00283   return output;
00284 }
analyzer_group.cpp
