motors_monitor.cpp
Go to the documentation of this file.
00001 
00026 #include "grizzly_motion/motors_monitor.h"
00027 #include <roboteq_msgs/Feedback.h>
00028 #include <roboteq_msgs/Status.h>
00029 #include <grizzly_msgs/eigen.h>
00030 #include <boost/bind.hpp>
00031 
00032 
00033 MotorsMonitor::MotorsMonitor(ros::NodeHandle* nh) : nh_("")
00034 {
00035 
00036   double motors_timeout_seconds;
00037   ros::param::param<double>("~motors_timeout", motors_timeout_seconds, 0.31);
00038   motors_timeout_ = ros::Duration(motors_timeout_seconds);
00039 
00040   fb_sub_[0] = nh_.subscribe<roboteq_msgs::Feedback>("motors/front_right/feedback",1,boost::bind(&MotorsMonitor::motor_feedback,this,_1,(int)grizzly_msgs::Drives::FrontLeft));
00041   fb_sub_[1] = nh_.subscribe<roboteq_msgs::Feedback>("motors/front_left/feedback",1,boost::bind(&MotorsMonitor::motor_feedback,this,_1,(int)grizzly_msgs::Drives::FrontRight));
00042   fb_sub_[2] = nh_.subscribe<roboteq_msgs::Feedback>("motors/rear_right/feedback",1,boost::bind(&MotorsMonitor::motor_feedback,this,_1,(int)grizzly_msgs::Drives::RearLeft));
00043   fb_sub_[3] = nh_.subscribe<roboteq_msgs::Feedback>("motors/rear_left/feedback",1,boost::bind(&MotorsMonitor::motor_feedback,this,_1,(int)grizzly_msgs::Drives::RearRight));
00044 
00045   stat_sub_[0] = nh_.subscribe<roboteq_msgs::Status>("motors/front_right/status",1,boost::bind(&MotorsMonitor::motor_status,this,_1,(int)grizzly_msgs::Drives::FrontLeft));
00046   stat_sub_[1] = nh_.subscribe<roboteq_msgs::Status>("motors/front_left/status",1,boost::bind(&MotorsMonitor::motor_status,this,_1,(int)grizzly_msgs::Drives::FrontRight));
00047   stat_sub_[2] = nh_.subscribe<roboteq_msgs::Status>("motors/rear_right/status",1,boost::bind(&MotorsMonitor::motor_status,this,_1,(int)grizzly_msgs::Drives::RearLeft));
00048   stat_sub_[3] = nh_.subscribe<roboteq_msgs::Status>("motors/rear_left/status",1,boost::bind(&MotorsMonitor::motor_status,this,_1,(int)grizzly_msgs::Drives::RearRight));
00049 
00050   for (int i=grizzly_msgs::Drives::FrontLeft;i<=grizzly_msgs::Drives::RearRight;i++) {
00051     fault_level_[i] = 0;
00052   } 
00053 }
00054 
00055 template<class M>
00056 static inline ros::Duration age(M msg) 
00057 {
00058   return ros::Time::now() - msg->header.stamp;
00059 }
00060 
00064 bool MotorsMonitor::ok()
00065 {
00066   // If we have no data, or its old, then definitely not okay.
00067   for (int i=grizzly_msgs::Drives::FrontLeft;i<=grizzly_msgs::Drives::RearRight;i++) {
00068     if (!last_received_status_[i] || !last_received_feedback_)
00069     {
00070       ROS_DEBUG_THROTTLE(1.0, "Motors not ok due to missing feedback or status.");
00071       return false;
00072     }
00073     if (age(last_received_status_[i]) > motors_timeout_)
00074     {
00075       ROS_DEBUG_THROTTLE(1.0, "Motors not ok due to timed-out status message.");
00076       return false;
00077     }
00078     fault_level_[i] = lookForSeriousFault (last_received_status_[i]->fault, i);
00079     if (fault_level_[i] > 0)
00080     {
00081       ROS_DEBUG_STREAM_THROTTLE(1.0, "Motors not ok from " << grizzly_msgs::nameFromDriveIndex(i) << 
00082                                      " driver due to fault code " << (int)last_received_status_[i]->fault);
00083       return false;
00084     }
00085   }
00086 
00087   return true;
00088 }
00089 
00090 int MotorsMonitor::lookForSeriousFault(uint8_t fault_code, const int motor_num)
00091 {
00092   int error_level = 0;
00093   using roboteq_msgs::Status;
00094  
00095   //Warning level faults 
00096   if (fault_code & Status::FAULT_UNDERVOLTAGE) {
00097     error_level = 1;
00098   }
00099 
00100   if (fault_code & Status::FAULT_EMERGENCY_STOP) {
00101     error_level = 1;
00102   }
00103 
00104   if (fault_code & Status::FAULT_SEPEX_EXCITATION_FAULT) {
00105     error_level = 1;
00106   }
00107 
00108   if (fault_code & Status::FAULT_STARTUP_CONFIG_FAULT) {
00109     error_level = 1;
00110   }
00111 
00112   //More serious faults 
00113   if (fault_code & Status::FAULT_OVERHEAT) {
00114     error_level = 2;
00115   }
00116 
00117   if (fault_code & Status::FAULT_OVERVOLTAGE) {
00118     error_level = 2;
00119   }
00120 
00121   if (fault_code & Status::FAULT_SHORT_CIRCUIT) {
00122     error_level = 2;
00123   } 
00124 
00125   if (fault_code & Status::FAULT_MOSFET_FAILURE) {
00126     error_level = 2;
00127   }
00128   return error_level;
00129 }
00130 
00131 void MotorsMonitor::motor_feedback(const roboteq_msgs::FeedbackConstPtr msg, const int motor_num) {
00132   last_received_feedback_[motor_num] = msg;
00133 }
00134 
00135 void MotorsMonitor::motor_status(const roboteq_msgs::StatusConstPtr& msg, const int motor_num) {
00136   last_received_status_[motor_num] = msg;
00137 }


grizzly_motion
Author(s):
autogenerated on Thu Jun 6 2019 21:44:02