motion_safety_node.cpp
Go to the documentation of this file.
00001 
00026 #include "grizzly_motion/motion_safety.h"
00027 #include "grizzly_motion/motors_monitor.h"
00028 #include "grizzly_motion/encoders_monitor.h"
00029 #include "grizzly_motion/change_limiter.h"
00030 
00031 #include "grizzly_msgs/Ambience.h"
00032 #include "grizzly_msgs/Drive.h"
00033 #include "grizzly_msgs/RawStatus.h"
00034 #include "std_msgs/Bool.h"
00035 
00036 using diagnostic_updater::DiagnosticStatusWrapper;
00037 using diagnostic_updater::FrequencyStatusParam;
00038 using diagnostic_updater::HeaderlessTopicDiagnostic;
00039 using diagnostic_updater::Updater;
00040 
00041 MotionSafety::MotionSafety(ros::NodeHandle* nh)
00042   : nh_(nh), state_(MotionStates::Stopped)
00043 {
00044   ros::param::get("vehicle_width", width_);
00045   ros::param::get("wheel_radius", radius_);
00046   ros::param::get("max_acceleration", max_accel_);  // m/s^2
00047   starting_duration_ = ros::Duration(2.0);
00048 
00049   // Drive pass-through
00050   sub_drive_ = nh_->subscribe("cmd_drive", 1, &MotionSafety::driveCallback, this);
00051   pub_safe_drive_ = nh_->advertise<grizzly_msgs::Drive>("safe_cmd_drive", 1);
00052 
00053   // MCU interface
00054   pub_ambience_ = nh_->advertise<grizzly_msgs::Ambience>("mcu/ambience", 1);
00055   pub_estop_ = nh_->advertise<std_msgs::Bool>("mcu/estop", 1);
00056   sub_mcu_status_ = nh_->subscribe("mcu/status", 1, &MotionSafety::mcuStatusCallback, this);
00057   watchdog_timer_ = nh_->createTimer(ros::Duration(0.05), &MotionSafety::watchdogCallback, this);
00058 
00059   // Drive pass-through
00060   sub_user_estop_ = nh_->subscribe("estop", 1, &MotionSafety::estopCallback, this);
00061 
00062   // Set up the diagnostic updater
00063   diagnostic_updater_.reset(new Updater());
00064   diagnostic_updater_->setHardwareID("grizzly");
00065 
00066   // Frequency report on statuses coming from the MCU.
00067   expected_mcu_status_frequency_ = 50;
00068   diag_mcu_status_freq_.reset(new HeaderlessTopicDiagnostic("MCU status", *diagnostic_updater_,
00069       FrequencyStatusParam(&expected_mcu_status_frequency_, &expected_mcu_status_frequency_, 0.01, 5.0)));
00070 
00071   // Frequency report on inbound drive messages.
00072   min_cmd_drive_freq_ = 10;
00073   max_cmd_drive_freq_ = 50;
00074   diag_cmd_drive_freq_.reset(new HeaderlessTopicDiagnostic("Drive command", *diagnostic_updater_, 
00075       FrequencyStatusParam(&min_cmd_drive_freq_, &max_cmd_drive_freq_, 0.01, 5.0)));
00076 
00077   diagnostic_updater_->add("Motion Safety", this, &MotionSafety::diagnostic);
00078 
00079   // More specialized monitoring for encoders. 
00080   encoders_monitor_.reset(new EncodersMonitor(nh_));
00081   motor_drivers_monitor_.reset(new MotorsMonitor(nh_));
00082   diagnostic_updater_->add("Encoders", encoders_monitor_.get(), &EncodersMonitor::diagnostic);
00083 
00084   // Rate-of-change limiter for wheel speed commands.
00085   accel_limiters_[0].reset(new DriveChangeLimiter(max_accel_ / radius_, &grizzly_msgs::Drive::front_left));
00086   accel_limiters_[1].reset(new DriveChangeLimiter(max_accel_ / radius_, &grizzly_msgs::Drive::front_right));
00087   accel_limiters_[2].reset(new DriveChangeLimiter(max_accel_ / radius_, &grizzly_msgs::Drive::rear_left));
00088   accel_limiters_[3].reset(new DriveChangeLimiter(max_accel_ / radius_, &grizzly_msgs::Drive::rear_right));
00089 }
00090 
00091 void MotionSafety::setStop(const std::string reason, bool estop=false, bool fault=false)
00092 {
00093   ROS_ERROR_STREAM("Stop transition occurring for reason: " << reason);
00094   if (estop || fault)
00095   {
00096     std_msgs::Bool msg;
00097     msg.data = true;
00098     pub_estop_.publish(msg);
00099     state_ = fault ? MotionStates::Fault : MotionStates::PendingStopped;
00100   }
00101   else
00102   {
00103     state_ = MotionStates::Stopped;
00104   }
00105   reason_ = reason;
00106 }
00107 
00108 void MotionSafety::checkFaults()
00109 {
00110   // Precharge fault
00111   if (last_mcu_status_)
00112   {
00113     if (!(last_mcu_status_->error & grizzly_msgs::RawStatus::ERROR_BRK_DET))
00114       last_non_precharge_time_ = last_mcu_status_->header.stamp;
00115     if (last_mcu_status_->header.stamp - last_non_precharge_time_ > ros::Duration(4.0))
00116     {
00117       // Pre-charging in excess of 4 seconds signals a serious electrical failure.
00118       setStop("Precharge persisted for more than four seconds.", true, true);
00119     }
00120   }
00121 
00122   // Encoders fault
00123   if (encoders_monitor_->detectFailedEncoder())
00124   {
00125     setStop("Encoder failure detected.", true, true);
00126   }
00127 
00128   // Motor driver fault
00129 
00130 }
00131 
00132 void MotionSafety::watchdogCallback(const ros::TimerEvent&)
00133 {
00134   // Messages to be published to the MCU at each run of this function.
00135   grizzly_msgs::Ambience ambience;
00136   std_msgs::Bool estop;
00137   estop.data = false;
00138 
00139   checkFaults();
00140  
00141   bool encoders_ok = encoders_monitor_->ok();
00142   bool motor_controllers_ok = motor_drivers_monitor_->ok();
00143 
00144   if (state_ == MotionStates::Stopped)
00145   {
00146     if (ros::Time::now() - last_commanded_movement_time_ < ros::Duration(0.1) &&
00147         !isEstopped())
00148     {
00149       state_ = MotionStates::Starting;
00150       transition_to_moving_time_ = ros::Time::now() + starting_duration_;
00151     }
00152   }
00153 
00154   if (state_ == MotionStates::Starting)
00155   {
00156     ambience.beacon = ambience.headlight = ambience.taillight = ambience.beep =
00157       grizzly_msgs::Ambience::PATTERN_DFLASH;
00158     if (ros::Time::now() > transition_to_moving_time_)
00159       state_ = MotionStates::Moving;
00160     if (ros::Time::now() - last_commanded_movement_time_ > ros::Duration(0.1))
00161       setStop("Command messages stale.");
00162     if (!encoders_ok)
00163       setStop("Encoders not okay.");
00164     //if(!motor_controllers_ok)
00165     //  setStop("Motor controllers not okay.");
00166     if (isEstopped())
00167       setStop("External estop.");
00168   }
00169 
00170   if (state_ == MotionStates::Moving)
00171   {
00172     ambience.beacon = ambience.beep =
00173       grizzly_msgs::Ambience::PATTERN_FLASH;
00174     if (!encoders_ok)
00175       setStop("Encoders not okay.", true, true);
00176     if (isEstopped())
00177       setStop("External estop.");
00178     //if (!motor_controllers_ok)
00179     //  setStop("Motor controllers not okay.", true);
00180 
00181     // There are two levels of timeout at work when Grizzly is in motion. The immediate
00182     // timeout is 110ms, enforced in the Roboteq firmware:
00183     //   https://github.com/g/roboteq/blob/master/roboteq_driver/mbs/script.mbs#L10
00184     //
00185     // This is the secondary timeout, which switches the statemachine back to the Stopped
00186     // state, which requires the starting_duration_ wait before moving again. This delay
00187     // is only necessary when the vehicle has been stationary/uncommanded for a short period.
00188     if (ros::Time::now() - last_commanded_movement_time_ > ros::Duration(3.0))
00189       setStop("Command messages stale.");
00190   }
00191 
00192   if (state_ == MotionStates::PendingStopped)
00193   {
00194     estop.data = true;
00195     // Three conditions to exit this state:
00196     //   - Vehicle must be nonmoving, according to the encoders
00197     //   - Vehicle must not be receiving motion commands (none in the last second)
00198     //   - Vehicle must be in the estop state, requiring a reset
00199     if (!encoders_monitor_->moving() &&
00200         ros::Time::now() - last_commanded_movement_time_ > ros::Duration(1.0) &&
00201         isEstopped())
00202     {
00203       state_ = MotionStates::Stopped;
00204     }
00205   }
00206   
00207   if (state_ == MotionStates::Fault)
00208   {
00209     estop.data = true;
00210   }
00211 
00212   diagnostic_updater_->update(); 
00213   pub_ambience_.publish(ambience);
00214   pub_estop_.publish(estop);
00215 }
00216 
00217 void MotionSafety::estopCallback(const std_msgs::BoolConstPtr& msg)
00218 {
00219   if (msg->data == true) {
00220     // Publish an immediate message, then also signal the state transition.
00221     pub_estop_.publish(msg);
00222     if (state_ != MotionStates::Fault) state_ = MotionStates::PendingStopped;
00223   }
00224 }
00225 
00231 void MotionSafety::driveCallback(const grizzly_msgs::DriveConstPtr& drive_commanded)
00232 {
00233   diag_cmd_drive_freq_->tick();
00234 
00235   // This signals the main loop function to begin a transition to the Moving state.
00236   if (!grizzly_msgs::isStationary(*drive_commanded.get()))
00237     last_commanded_movement_time_ = drive_commanded->header.stamp;
00238  
00239   grizzly_msgs::Drive drive_safe;
00240   drive_safe.header = drive_commanded->header;
00241 
00242   if (state_ == MotionStates::Moving)
00243   {
00244     for (int wheel = 0; wheel < 4; wheel++) 
00245       accel_limiters_[wheel]->apply(drive_commanded.get(), &drive_safe);
00246   }
00247 
00248   pub_safe_drive_.publish(drive_safe);
00249 }
00250 
00251 bool MotionSafety::isEstopped()
00252 {
00253   return last_mcu_status_ && 
00254     (last_mcu_status_->error & grizzly_msgs::RawStatus::ERROR_ESTOP_RESET);
00255 }
00256 
00257 void MotionSafety::mcuStatusCallback(const grizzly_msgs::RawStatusConstPtr& status)
00258 {
00259   diag_mcu_status_freq_->tick();
00260   last_mcu_status_ = status;
00261 }
00262 
00263 void MotionSafety::diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat)
00264 {
00265   std::string state_str;
00266   int level = 0;
00267   switch(state_) 
00268   {
00269     case MotionStates::Stopped:
00270       state_str = "Stopped";
00271       break;
00272     case MotionStates::Starting:
00273       state_str = "Starting";
00274       reason_.clear();
00275       break;
00276     case MotionStates::Moving:
00277       state_str = "Moving";
00278       reason_.clear();
00279       break;
00280     case MotionStates::PendingStopped:
00281       state_str = "PendingStopped";
00282       level = 1;
00283       break;
00284     case MotionStates::Fault:
00285       state_str = "Faulted";
00286       level = 2;
00287       break;
00288     default:
00289       state_str = "Unknown";
00290       level = 2;
00291       break;
00292   }
00293   stat.summaryf(level, "Vehicle State %s", state_str.c_str());
00294   if (!reason_.empty()) {
00295     stat.summaryf(level, "Vehicle State %s (Reason: %s)", state_str.c_str(), reason_.c_str());
00296   }
00297   stat.add("reason", reason_);
00298   stat.add("state", state_); 
00299   stat.add("last move command (seconds)", (ros::Time::now() - last_commanded_movement_time_).toSec()); 
00300   stat.add("vehicle in motion", encoders_monitor_->moving());
00301 }
00302 
00306 int main(int argc, char ** argv)
00307 {
00308   ros::init(argc, argv, "grizzly_motion_safety"); 
00309   ros::NodeHandle nh("");
00310   MotionSafety ms(&nh);
00311   ros::spin();
00312 }
00313 


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