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
00038 #ifndef _PR2_MECHANISM_DIAGNOSTICS_H_JOINT_DIAG_
00039 #define _PR2_MECHANISM_DIAGNOSTICS_H_JOINT_DIAG_
00040
00041 #include <vector>
00042 #include <float.h>
00043
00044 #include <boost/shared_ptr.hpp>
00045 #include <boost/math/special_functions/fpclassify.hpp>
00046 #include <boost/accumulators/accumulators.hpp>
00047 #include <boost/accumulators/statistics/max.hpp>
00048 #include <boost/accumulators/statistics/min.hpp>
00049
00050 #include <ros/ros.h>
00051 #include <pr2_mechanism_msgs/MechanismStatistics.h>
00052 #include <pr2_mechanism_msgs/JointStatistics.h>
00053 #include <pr2_mechanism_msgs/ActuatorStatistics.h>
00054 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00055 #include <urdf_interface/joint.h>
00056
00057 namespace pr2_mechanism_diagnostics
00058 {
00059
00061 template<typename T>
00062 inline bool is_valid(T t)
00063 {
00064 if (t == 0)
00065 return true;
00066
00067 return boost::math::isnormal(t);
00068 }
00069
00077 class JointStats
00078 {
00079 private:
00080 ros::Time updateTime;
00081
00082 mutable bool needs_reset;
00083
00084 std::string name;
00085 double position;
00086 double velocity;
00087 double measured_effort;
00088 double commanded_effort;
00089 bool is_calibrated;
00090 bool violated_limits;
00091 double odometer;
00092
00093
00094 double max_pos_val, min_pos_val, max_abs_vel_val, max_abs_eff_val;
00095
00096 void reset_vals();
00098 public:
00099 JointStats(std::string nam);
00100
00101 ~JointStats() { }
00102
00103 bool update(const pr2_mechanism_msgs::JointStatistics &js);
00104
00105 boost::shared_ptr<diagnostic_updater::DiagnosticStatusWrapper> toDiagStat() const;
00106 };
00107
00108 }
00109
00110 #endif // _PR2_MECHANISM_DIAGNOSTICS_H_JOINT_DIAG_