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/aggregator.h>
00038
00039 using namespace std;
00040 using namespace diagnostic_aggregator;
00041
00042 Aggregator::Aggregator() :
00043 pub_rate_(1.0),
00044 analyzer_group_(NULL),
00045 other_analyzer_(NULL),
00046 base_path_("")
00047 {
00048 ros::NodeHandle nh = ros::NodeHandle("~");
00049 nh.param(string("base_path"), base_path_, string(""));
00050 if (base_path_.size() > 0 && base_path_.find("/") != 0)
00051 base_path_ = "/" + base_path_;
00052
00053 nh.param("pub_rate", pub_rate_, pub_rate_);
00054
00055 analyzer_group_ = new AnalyzerGroup();
00056
00057 if (!analyzer_group_->init(base_path_, nh))
00058 {
00059 ROS_ERROR("Analyzer group for diagnostic aggregator failed to initialize!");
00060 }
00061
00062
00063
00064 other_analyzer_ = new OtherAnalyzer();
00065 other_analyzer_->init(base_path_);
00066
00067 diag_sub_ = n_.subscribe("/diagnostics", 1000, &Aggregator::diagCallback, this);
00068 agg_pub_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics_agg", 1);
00069 toplevel_state_pub_ = n_.advertise<diagnostic_msgs::DiagnosticStatus>("/diagnostics_toplevel_state", 1);
00070 }
00071
00072 void Aggregator::checkTimestamp(const diagnostic_msgs::DiagnosticArray::ConstPtr& diag_msg)
00073 {
00074 if (diag_msg->header.stamp.toSec() != 0)
00075 return;
00076
00077 string stamp_warn = "No timestamp set for diagnostic message. Message names: ";
00078 vector<diagnostic_msgs::DiagnosticStatus>::const_iterator it;
00079 for (it = diag_msg->status.begin(); it != diag_msg->status.end(); ++it)
00080 {
00081 if (it != diag_msg->status.begin())
00082 stamp_warn += ", ";
00083 stamp_warn += it->name;
00084 }
00085
00086 if (!ros_warnings_.count(stamp_warn))
00087 {
00088 ROS_WARN("%s", stamp_warn.c_str());
00089 ros_warnings_.insert(stamp_warn);
00090 }
00091 }
00092
00093 void Aggregator::diagCallback(const diagnostic_msgs::DiagnosticArray::ConstPtr& diag_msg)
00094 {
00095 checkTimestamp(diag_msg);
00096
00097 bool analyzed = false;
00098 for (unsigned int j = 0; j < diag_msg->status.size(); ++j)
00099 {
00100 analyzed = false;
00101 boost::shared_ptr<StatusItem> item(new StatusItem(&diag_msg->status[j]));
00102 if (analyzer_group_->match(item->getName()))
00103 analyzed = analyzer_group_->analyze(item);
00104
00105 if (!analyzed)
00106 other_analyzer_->analyze(item);
00107 }
00108 }
00109
00110 Aggregator::~Aggregator()
00111 {
00112 if (analyzer_group_) delete analyzer_group_;
00113
00114 if (other_analyzer_) delete other_analyzer_;
00115 }
00116
00117 void Aggregator::publishData()
00118 {
00119 diagnostic_msgs::DiagnosticArray diag_array;
00120
00121 diagnostic_msgs::DiagnosticStatus diag_toplevel_state;
00122 diag_toplevel_state.name = "toplevel_state";
00123 diag_toplevel_state.level = -1;
00124 int min_level = 255;
00125
00126 vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > processed = analyzer_group_->report();
00127 for (unsigned int i = 0; i < processed.size(); ++i)
00128 {
00129 diag_array.status.push_back(*processed[i]);
00130
00131 if (processed[i]->level > diag_toplevel_state.level)
00132 diag_toplevel_state.level = processed[i]->level;
00133 if (processed[i]->level < min_level)
00134 min_level = processed[i]->level;
00135 }
00136
00137 vector<boost::shared_ptr<diagnostic_msgs::DiagnosticStatus> > processed_other = other_analyzer_->report();
00138 for (unsigned int i = 0; i < processed_other.size(); ++i)
00139 {
00140 diag_array.status.push_back(*processed_other[i]);
00141
00142 if (processed_other[i]->level > diag_toplevel_state.level)
00143 diag_toplevel_state.level = processed_other[i]->level;
00144 if (processed_other[i]->level < min_level)
00145 min_level = processed_other[i]->level;
00146 }
00147
00148 diag_array.header.stamp = ros::Time::now();
00149
00150 agg_pub_.publish(diag_array);
00151
00152
00153 if (diag_toplevel_state.level > 2 and min_level <= 2)
00154 diag_toplevel_state.level = 2;
00155
00156 toplevel_state_pub_.publish(diag_toplevel_state);
00157 }
00158
00159
00160
00161
00162
00163
00164
00165