Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
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
00263 if (processed.size() == 0)
00264 continue;
00265
00266
00267
00268 for (unsigned int i = 0; i < processed.size(); ++i)
00269 {
00270 output.push_back(processed[i]);
00271
00272
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
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_ != "/")
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 }