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_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
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
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
00242 if (processed.size() == 0)
00243 continue;
00244
00245
00246
00247 for (unsigned int i = 0; i < processed.size(); ++i)
00248 {
00249 output.push_back(processed[i]);
00250
00251
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
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_ != "/")
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 }