38 #ifndef _PR2_MECHANISM_DIAGNOSTICS_H_MECH_DIAG_ 39 #define _PR2_MECHANISM_DIAGNOSTICS_H_MECH_DIAG_ 42 #include <pr2_mechanism_msgs/MechanismStatistics.h> 43 #include <diagnostic_msgs/DiagnosticArray.h> 47 #include <boost/shared_ptr.hpp> 48 #include <boost/math/special_functions/fpclassify.hpp> 52 #include "std_srvs/Empty.h" 53 #include "std_msgs/Bool.h" 65 std::map<std::string, boost::shared_ptr<JointStats> >
joint_stats;
75 void mechCallback(
const pr2_mechanism_msgs::MechanismStatistics::ConstPtr& mechMsg);
84 bool ok()
const {
return n_.
ok(); }
89 #endif // _PR2_MECHANISM_DIAGNOSTICS_H_MECH_DIAG_
~CtrlJntDiagnosticPublisher()
void mechCallback(const pr2_mechanism_msgs::MechanismStatistics::ConstPtr &mechMsg)
Publishes diagnostics for controllers, joints from pr2_mechanism_msgs/MechanismStatistics message...
bool disable_controller_warnings_
ros::Subscriber mech_st_sub_
std::map< std::string, boost::shared_ptr< JointStats > > joint_stats
std::map< std::string, boost::shared_ptr< ControllerStats > > controller_stats
CtrlJntDiagnosticPublisher()