diagnostic_aggregator: diagnostic_aggregator: generic

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/generic_analyzer.h"
00038 
00039 using namespace diagnostic_aggregator;
00040 using namespace std;
00041 
00042 PLUGINLIB_DECLARE_CLASS(diagnostic_aggregator,
00043                         GenericAnalyzer, 
00044                         diagnostic_aggregator::GenericAnalyzer, 
00045                         diagnostic_aggregator::Analyzer)
00046 
00047 
00048 GenericAnalyzer::GenericAnalyzer() { }
00049 
00050 bool GenericAnalyzer::init(const string base_path, const ros::NodeHandle &n)
00051 { 
00052   string nice_name;
00053   if (!n.getParam("path", nice_name))
00054   {
00055     ROS_ERROR("GenericAnalyzer was not given parameter \"path\". Namepspace: %s",
00056               n.getNamespace().c_str());
00057     return false;
00058   }
00059 
00060   XmlRpc::XmlRpcValue findRemove;
00061   if (n.getParam("find_and_remove_prefix", findRemove))
00062   {
00063     vector<string> output;
00064     getParamVals(findRemove, output);
00065     chaff_ = output;
00066     startswith_ = output;
00067   }
00068   
00069   XmlRpc::XmlRpcValue removes;
00070   if (n.getParam("remove_prefix", removes))
00071     getParamVals(removes, chaff_);
00072     
00073   XmlRpc::XmlRpcValue startswith;
00074   if (n.getParam("startswith", startswith))
00075     getParamVals(startswith, startswith_);
00076 
00077   XmlRpc::XmlRpcValue name_val;
00078   if (n.getParam("name", name_val))
00079     getParamVals(name_val, name_);
00080 
00081   XmlRpc::XmlRpcValue contains;
00082   if (n.getParam("contains", contains))
00083     getParamVals(contains, contains_);
00084 
00085   XmlRpc::XmlRpcValue expected;
00086   if (n.getParam("expected", expected))
00087   {
00088     getParamVals(expected, expected_);
00089     for (unsigned int i = 0; i < expected_.size(); ++i)
00090     {
00091       boost::shared_ptr<StatusItem> item(new StatusItem(expected_[i]));
00092       addItem(expected_[i], item);
00093     }
00094  }
00095  
00096   XmlRpc::XmlRpcValue regexes;
00097   if (n.getParam("regex", regexes))
00098   {
00099     vector<string> regex_strs;
00100     getParamVals(regexes, regex_strs);
00101   
00102     for (unsigned int i = 0; i < regex_strs.size(); ++i)
00103     {
00104       try
00105       {
00106         boost::regex re(regex_strs[i]);
00107         regex_.push_back(re);
00108       }
00109       catch (boost::regex_error& e)
00110       {
00111         ROS_ERROR("Attempted to make regex from %s. Caught exception, ignoring value. Exception: %s", 
00112                  regex_strs[i].c_str(), e.what());
00113       }
00114     }
00115   }
00116 
00117   if (startswith_.size() == 0 && name_.size() == 0 && 
00118       contains_.size() == 0 && expected_.size() == 0 && regex_.size() == 0)
00119   {
00120     ROS_ERROR("GenericAnalyzer was not initialized with any way of checking diagnostics. Name: %s, namespace: %s", nice_name.c_str(), n.getNamespace().c_str());
00121     return false;
00122   }
00123   
00124   double timeout;
00125   int num_items_expected;
00126   bool discard_stale;
00127   n.param("timeout", timeout, 5.0);   // Timeout for stale
00128   n.param("num_items", num_items_expected, -1); // Number of items must match this
00129   n.param("discard_stale", discard_stale, false);
00130 
00131   string my_path;
00132   if (base_path == "/")
00133     my_path = nice_name;
00134   else
00135     my_path = base_path + "/" + nice_name;
00136 
00137   if (my_path.find("/") != 0)
00138     my_path = "/" + my_path;
00139 
00140   return GenericAnalyzerBase::init(my_path, nice_name, 
00141                                    timeout, num_items_expected, discard_stale);
00142 }
00143 
00144 GenericAnalyzer::~GenericAnalyzer() { }
00145 
00146 
00147 bool GenericAnalyzer::match(const string name)
00148 {
00149   boost::cmatch what;
00150   for (unsigned int i = 0; i < regex_.size(); ++i)
00151   {
00152     if (boost::regex_match(name.c_str(), what, regex_[i]))
00153       return true;
00154   }
00155   
00156   for (unsigned int i = 0; i < expected_.size(); ++i)
00157   {
00158     if (name == expected_[i])
00159       return true;
00160   }
00161   
00162   for (unsigned int i = 0; i < name_.size(); ++i)
00163   {
00164     if (name == name_[i])
00165       return true;
00166   }
00167   
00168   for (unsigned int i = 0; i < startswith_.size(); ++i)
00169   {
00170     if (name.find(startswith_[i]) == 0)
00171       return true;
00172   }
00173   
00174   for (unsigned int i = 0; i < contains_.size(); ++i)
00175   {
00176     if (name.find(contains_[i]) != string::npos)
00177       return true;
00178   }
00179   
00180   return false;
00181 }
00182 
00183 vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > GenericAnalyzer::report()
00184 {
00185   vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > processed = GenericAnalyzerBase::report();
00186 
00187   // Check and make sure our expected names haven't been removed ...
00188   vector<string> expected_names_missing;
00189   bool has_name = false;
00190   bool all_stale = true;
00191   
00192   for (unsigned int i = 0; i < expected_.size(); ++i)
00193   {
00194     has_name = false;
00195     for (unsigned int j = 0; j < processed.size(); ++j)
00196     {
00197       if (!processed[j]->level == 3)
00198         all_stale = false;
00199 
00200       size_t last_slash = processed[j]->name.rfind("/");
00201       string nice_name = processed[j]->name.substr(last_slash + 1);
00202       if (nice_name == expected_[i] || nice_name == getOutputName(expected_[i]))
00203       {
00204         has_name = true;
00205         break;
00206       }
00207 
00208       // Remove chaff, check names
00209       for (unsigned int k = 0; k < chaff_.size(); ++k)
00210       {     
00211         if (nice_name == removeLeadingNameChaff(expected_[i], chaff_[k]))
00212         {
00213           has_name = true;
00214           break;
00215         }
00216       }
00217 
00218     }
00219     if (!has_name)
00220       expected_names_missing.push_back(expected_[i]);
00221   }  
00222   
00223   // Add missing names to header ...
00224   for (unsigned int i = 0; i < expected_names_missing.size(); ++i)
00225   {
00226     boost::shared_ptr<StatusItem> item(new StatusItem(expected_names_missing[i]));
00227     processed.push_back(item->toStatusMsg(path_, true));
00228   } 
00229 
00230   // Check that all processed items aren't stale
00231   for (unsigned int j = 0; j < processed.size(); ++j)
00232   {
00233     if (processed[j]->level != 3)
00234       all_stale = false;
00235   }
00236 
00237   for (unsigned int j = 0; j < processed.size(); ++j)
00238   {
00239     // Remove all leading name chaff
00240     for (unsigned int i = 0; i < chaff_.size(); ++i)
00241       processed[j]->name = removeLeadingNameChaff(processed[j]->name, chaff_[i]);
00242 
00243     // If we're missing any items, set the header status to error or stale
00244     if (expected_names_missing.size() > 0 && processed[j]->name == path_)
00245     {
00246       if (!all_stale)
00247       {
00248         processed[j]->level = 2;
00249         processed[j]->message = "Error";
00250       }
00251       else
00252       {
00253         processed[j]->level = 3;
00254         processed[j]->message = "All Stale";
00255       }
00256 
00257       // Add all missing items to header item
00258       for (unsigned int k = 0; k < expected_names_missing.size(); ++k)
00259       {
00260         diagnostic_msgs::KeyValue kv;
00261         kv.key = expected_names_missing[k];
00262         kv.value = "Missing";
00263         processed[j]->values.push_back(kv);
00264       }
00265     }
00266   }
00267   
00268   return processed;
00269 }
generic_analyzer.cpp
