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
00035
00036 #include "pr2_mechanism_diagnostics/pr2_mechanism_diagnostics.h"
00037 #include <exception>
00038 #include <limits>
00039 #include "angles/angles.h"
00040
00041 using namespace pr2_mechanism_diagnostics;
00042 using namespace std;
00043
00044
00045 CtrlJntDiagnosticPublisher::CtrlJntDiagnosticPublisher() :
00046 pnh_("~"),
00047 use_sim_time_(false),
00048 disable_controller_warnings_(false)
00049 {
00050 mech_st_sub_ = n_.subscribe("mechanism_statistics", 1000, &CtrlJntDiagnosticPublisher::mechCallback, this);
00051 diag_pub_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00052
00053 n_.param("/use_sim_time", use_sim_time_, false);
00054
00055 pnh_.param("disable_controller_warnings", disable_controller_warnings_, false);
00056 }
00057
00058
00059
00060
00061
00062 void CtrlJntDiagnosticPublisher::mechCallback(const pr2_mechanism_msgs::MechanismStatistics::ConstPtr& mechMsg)
00063 {
00064
00065 vector<pr2_mechanism_msgs::JointStatistics>::const_iterator it;
00066 for (it = mechMsg->joint_statistics.begin(); it != mechMsg->joint_statistics.end(); ++it)
00067 {
00068 if (joint_stats.count(it->name) == 0)
00069 joint_stats[it->name] = boost::shared_ptr<JointStats>(new JointStats(it->name));
00070
00071 joint_stats[it->name]->update(*it);
00072 }
00073
00074
00075 vector<pr2_mechanism_msgs::ControllerStatistics>::const_iterator c_it;
00076 for (c_it = mechMsg->controller_statistics.begin(); c_it != mechMsg->controller_statistics.end(); ++c_it)
00077 {
00078 if (controller_stats.count(c_it->name) == 0)
00079 controller_stats[c_it->name] = boost::shared_ptr<ControllerStats>(new ControllerStats(c_it->name, use_sim_time_ || disable_controller_warnings_));
00080
00081 controller_stats[c_it->name]->update(*c_it);
00082 }
00083 }
00084
00085 void CtrlJntDiagnosticPublisher::publishDiag()
00086 {
00087 diagnostic_msgs::DiagnosticArray array;
00088
00089
00090 map<string, boost::shared_ptr<JointStats> >::iterator it;
00091 for (it = joint_stats.begin(); it != joint_stats.end(); ++it)
00092 array.status.push_back(*(it->second->toDiagStat()));
00093
00094
00095 map<string, boost::shared_ptr<ControllerStats> >::iterator c_it;
00096 unsigned int num_controllers = 0;
00097 vector<string> erase_controllers;
00098 for (c_it = controller_stats.begin(); c_it != controller_stats.end(); ++c_it)
00099 {
00100 if (c_it->second->shouldDiscard())
00101 erase_controllers.push_back(c_it->first);
00102
00103 array.status.push_back(*(c_it->second->toDiagStat()));
00104 num_controllers++;
00105 }
00106 for (unsigned int i=0; i<erase_controllers.size(); i++)
00107 controller_stats.erase(erase_controllers[i]);
00108
00109
00110 if (num_controllers == 0)
00111 {
00112 diagnostic_updater::DiagnosticStatusWrapper stat;
00113 stat.name = "Controller: No controllers loaded in controller manager";
00114 array.status.push_back(stat);
00115 }
00116
00117 array.header.stamp = ros::Time::now();
00118 diag_pub_.publish(array);
00119 }
00120
00121
00122 int main(int argc, char **argv)
00123 {
00124 ros::init(argc, argv, "pr2_mechanism_diagnostics");
00125
00126 try
00127 {
00128 CtrlJntDiagnosticPublisher ctrlJntPub;
00129
00130 ros::Rate pub_rate(1.0);
00131 while (ctrlJntPub.ok())
00132 {
00133 pub_rate.sleep();
00134 ros::spinOnce();
00135 ctrlJntPub.publishDiag();
00136 }
00137 }
00138 catch (exception& e)
00139 {
00140 ROS_FATAL("Controllers/Joint to Diagnostics node caught exception. Aborting. %s", e.what());
00141 ROS_BREAK();
00142 }
00143
00144 exit(0);
00145 return 0;
00146 }