encoders_monitor.cpp
Go to the documentation of this file.
00001 
00026 #include "grizzly_motion/encoders_monitor.h"
00027 #include "diagnostic_updater/publisher.h"
00028 
00032 EncodersMonitor::EncodersMonitor(ros::NodeHandle* nh)
00033   : encoders_timeout(0.11),
00034     encoder_speed_error_diff_threshold(0.8),
00035     encoder_fault_time_to_failure(1.0),
00036     failed_encoder_(-1)
00037 {
00038   sub_encoders_ = nh->subscribe("motors/encoders", 1, &EncodersMonitor::encodersCallback, this);
00039   sub_drive_ = nh->subscribe("safe_cmd_drive", 1, &EncodersMonitor::driveCallback, this); 
00040 
00041   double encoders_timeout_seconds;
00042   ros::param::param<double>("~encoders_timeout", encoders_timeout_seconds, encoders_timeout.toSec());
00043   encoders_timeout = ros::Duration(encoders_timeout_seconds);
00044 
00045   double encoder_fault_time_to_failure_seconds;
00046   ros::param::param<double>("~encoder_fault_time_to_failure",
00047       encoder_fault_time_to_failure_seconds, encoder_fault_time_to_failure.toSec());
00048   encoder_fault_time_to_failure = ros::Duration(encoder_fault_time_to_failure_seconds);
00049 }
00050 
00051 template<class M>
00052 static inline ros::Duration age(M msg) 
00053 {
00054   return ros::Time::now() - msg->header.stamp;
00055 }
00056 
00057 bool EncodersMonitor::detectFailedEncoderCandidate(VectorDrive::Index* candidate)
00058 {
00059   // Attempt to detect a failed encoder. The symptom will be that the reported velocity will
00060   // be zero or very near it despite a non-zero commanded velocity. To avoid a false positive
00061   // due to motors under heavy load/stall, only flag the error when it greatly exceeds that 
00062   // of the second-most-erroneous wheel--- this is on the theory that typical operation is
00063   // unlikely to stall a single wheel.
00064   VectorDrive wheelSpeedMeasured = grizzly_msgs::vectorFromDriveMsg(*last_received_encoders_);
00065   VectorDrive wheelSpeedCommanded = grizzly_msgs::vectorFromDriveMsg(*last_received_drive_);
00066   VectorDrive wheelSpeedError = (wheelSpeedMeasured - wheelSpeedCommanded).cwiseAbs();
00067 
00068   // Find the index with maximum error, which is our failed encoder candidate.
00069   double max_error = wheelSpeedError.maxCoeff(candidate);
00070 
00071   // Now set that one to zero and use the new max to get the difference between greatest and
00072   // second-greatest error amounts.
00073   wheelSpeedError[*candidate] = 0;
00074   double max_error_diff = max_error - wheelSpeedError.maxCoeff();
00075 
00076   // If the measured speed is not small, then it's not a failure. A failed encoder will be either
00077   // still, or buzzing back and forth.
00078   if (fabs(wheelSpeedMeasured[*candidate]) > 0.01) return false;
00079 
00080   // If the error difference does not exceed a threshold, then not an error.
00081   if (max_error_diff < encoder_speed_error_diff_threshold) return false;
00082 
00083   // Candidate failure is valid. Calling function will assert error if
00084   // this state persists for a set time period.
00085   return true;
00086 }
00087 
00088 bool EncodersMonitor::detectFailedEncoder()
00089 {
00090   if (!last_received_encoders_ || !last_received_drive_) return false;
00091 
00092   // Disable check until motor model technique implemented and more test data available.
00093   return false;
00094 
00095   VectorDrive::Index candidate_failed_encoder;
00096   if (detectFailedEncoderCandidate(&candidate_failed_encoder)) {
00097     if (last_received_encoders_->header.stamp - time_of_last_nonsuspect_encoders_ > encoder_fault_time_to_failure) {
00098       failed_encoder_ = candidate_failed_encoder;
00099       return true;
00100     } 
00101   } else {
00102     time_of_last_nonsuspect_encoders_ = last_received_encoders_->header.stamp;
00103   }
00104   return false;
00105 }
00106 
00107 bool EncodersMonitor::ok()
00108 {
00109   // If we have no encoder data, or its old, then definitely not okay.
00110   if (!last_received_encoders_ || age(last_received_encoders_) > encoders_timeout) return false;
00111 
00112   // If we have no drive data, or it's old, then we're initializing; that's fine.
00113   if (!last_received_drive_ || age(last_received_drive_) > encoders_timeout) return true;
00114 
00115   if (detectFailedEncoder()) {
00116     // Not a recoverable fault.
00117     return false;
00118   }
00119  
00120   return true;
00121 }
00122 
00126 bool EncodersMonitor::moving()
00127 {
00128   return last_received_encoders_ && !grizzly_msgs::isStationary(*last_received_encoders_.get());
00129 }
00130 
00134 void EncodersMonitor::diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat)
00135 {
00136   if (!last_received_encoders_)
00137   {
00138     stat.summary(2, "No encoders messages received.");
00139     return;
00140   } 
00141 
00142   stat.add("Age of last encoders message", age(last_received_encoders_).toSec());
00143   if (age(last_received_encoders_) > encoders_timeout)
00144   {
00145     stat.summaryf(2, "Last encoders message is stale.");
00146     return;
00147   }
00148 
00149   if (failed_encoder_ >= 0)
00150   {
00151     std::string wheel_str(grizzly_msgs::nameFromDriveIndex(failed_encoder_));
00152     stat.summaryf(2, "Encoder failure detected in %s wheel. Not a recoverable error, please service system.", wheel_str.c_str());
00153     return;
00154   }
00155 
00156   //stat.summary(1, "Encoders monitoring not implemented.");
00157   stat.summary(0, "Encoders look good.");
00158 }
00159 
00170 void EncodersMonitor::encodersCallback(const grizzly_msgs::DriveConstPtr& encoders)
00171 {
00172   last_received_encoders_ = encoders;
00173 }
00174 
00178 void EncodersMonitor::driveCallback(const grizzly_msgs::DriveConstPtr& drive)
00179 {
00180   last_received_drive_ = drive;
00181 }


grizzly_motion
Author(s):
autogenerated on Fri Aug 28 2015 10:55:30