motion_safety.h
Go to the documentation of this file.
00001 
00026 #include <ros/ros.h>
00027 #include "grizzly_motion/change_limiter.h"
00028 
00029 #include "diagnostic_updater/diagnostic_updater.h"
00030 #include "diagnostic_updater/publisher.h"
00031 
00032 using boost::shared_ptr;
00033 
00034 namespace grizzly_msgs {
00035 ROS_DECLARE_MESSAGE(Drive);
00036 ROS_DECLARE_MESSAGE(RawStatus);
00037 }
00038 
00039 namespace std_msgs {
00040 ROS_DECLARE_MESSAGE(Bool);
00041 }
00042 
00043 namespace diagnostic_updater {
00044 class Updater;
00045 class HeaderlessTopicDiagnostic;
00046 }
00047 
00048 class EncodersMonitor;
00049 class MotorsMonitor;
00050 
00051 typedef ChangeLimiter<grizzly_msgs::Drive> DriveChangeLimiter;
00052 
00053 namespace MotionStates
00054 {
00055   enum MotionState
00056   {
00057     // State of non-error, non-moving vehicle. As soon as the vehicle has been
00058     // stationary for a brief period, the Moving state transitions to this one.
00059     // Certain errors that will trigger an MCU estop in the Moving or Startup
00060     // states will not do so in the Stopped state.
00061     Stopped,
00062 
00063     // State when movement has been requested but is being held pending the
00064     // mandatory delay period (and accompanying ambience).
00065     Starting,
00066 
00067     // State when vehicle is moving without error.
00068     Moving,
00069 
00070     // State when an estop has been triggered (by the MCU or otherwise). Wait for
00071     // the encoders to report the vehicle as stationary, with no motion being
00072     // commanded, then transition to Stopped.
00073     PendingStopped,
00074 
00075     // State when vehicle is faulted and will remain so until ROS is restarted.
00076     // Assert and hold an MCU estop.
00077     Fault
00078   };
00079 }
00080 typedef MotionStates::MotionState MotionState;
00081 
00082 class MotionSafety
00083 {
00084 public:
00085   MotionSafety() {}
00086   MotionSafety(ros::NodeHandle* nh);
00087 
00088   bool isEstopped();
00089   void setStop(const std::string reason, bool estop, bool fault);
00090   void checkFaults();
00091 
00092 protected:
00093   ros::NodeHandle* nh_;
00094 
00095   // Main watchdog function, which takes care of calling the other components
00096   // to assess overall system health (and thus safety of driving), and manage
00097   // state transitions.
00098   void watchdogCallback(const ros::TimerEvent&);
00099   ros::Timer watchdog_timer_;
00100   MotionState state_;
00101   std::string reason_;
00102 
00103   // Keeps the time of the last received command message with a non-stationary
00104   // movement request to at least one of the wheels.
00105   ros::Time last_commanded_movement_time_;
00106   ros::Time last_non_precharge_time_;
00107 
00108   // Duration of time to spend in the Starting phase.
00109   ros::Duration starting_duration_;
00110 
00111   // Tracks the absolute time when we will transition from Starting to Moving,
00112   // as that's a timed transition.
00113   ros::Time transition_to_moving_time_;
00114 
00115   // Topics directly monitored in this class.
00116   void driveCallback(const grizzly_msgs::DriveConstPtr&);
00117   void estopCallback(const std_msgs::BoolConstPtr&);
00118   void mcuStatusCallback(const grizzly_msgs::RawStatusConstPtr&);
00119   ros::Subscriber sub_drive_, sub_mcu_status_, sub_user_estop_; 
00120   grizzly_msgs::RawStatusConstPtr last_mcu_status_; 
00121 
00122   // Publish cmd_drive through to safe_cmd_drive.
00123   ros::Publisher pub_safe_drive_;
00124     
00125   // Communication to the MCU.
00126   ros::Publisher pub_ambience_;
00127   ros::Publisher pub_estop_;
00128 
00129   // Publish diagnostics for the whole node from here.
00130   shared_ptr<diagnostic_updater::Updater> diagnostic_updater_;
00131   void diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);
00132 
00133   // Monitor the frequency of the MCU status and incoming cmd_drive messages for
00134   // acceptable range.
00135   double expected_mcu_status_frequency_;
00136   double min_cmd_drive_freq_, max_cmd_drive_freq_;
00137   shared_ptr<diagnostic_updater::HeaderlessTopicDiagnostic> diag_mcu_status_freq_, diag_cmd_drive_freq_;
00138 
00139   // Separate class for monitoring encoders for sanity.
00140   shared_ptr<EncodersMonitor> encoders_monitor_; 
00141   shared_ptr<MotorsMonitor> motor_drivers_monitor_;
00142   shared_ptr<DriveChangeLimiter> accel_limiters_[4];
00143 
00144   double width_;
00145   double radius_;
00146   double max_accel_;
00147 };
00148 
00149 


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