00001 00025 #ifndef _CONTROL_LAYER_BASE_H_ 00026 #define _CONTROL_LAYER_BASE_H_ 00027 00028 00029 00030 #ifdef HAVE_MADP 00031 00032 #include <madp/Globals.h> 00033 00034 #define MDM_MAXHORIZON MAXHORIZON 00035 00036 #else 00037 00038 #define MDM_MAXHORIZON 999999 00039 00040 #endif 00041 00042 #include <boost/shared_ptr.hpp> 00043 00044 #include <ros/ros.h> 00045 #include <std_srvs/Empty.h> 00046 00047 00048 00049 namespace mdm_library 00050 { 00056 class ControlLayerBase 00057 { 00058 public: 00063 enum CONTROLLER_STATUS {STOPPED, STARTED}; 00064 00071 ControlLayerBase ( const CONTROLLER_STATUS initial_status = STARTED ); 00072 00074 uint32_t getDecisionEpisode (); 00075 00077 CONTROLLER_STATUS getStatus (); 00078 00080 uint32_t getDecisionHorizon (); 00082 void resetDecisionEpisode (); 00083 00090 virtual void stopController(); 00095 virtual void startController(); 00100 virtual void resetController(); 00101 00102 protected: 00107 uint32_t incrementDecisionEpisode(); 00112 void setStatus ( const CONTROLLER_STATUS status ); 00113 00114 ros::NodeHandle nh_; 00115 00116 private: 00121 bool stopCallback ( std_srvs::Empty::Request& request, std_srvs::Empty::Response& response ); 00126 bool startCallback ( std_srvs::Empty::Request& request, std_srvs::Empty::Response& response ); 00131 bool resetCallback ( std_srvs::Empty::Request& request, std_srvs::Empty::Response& response ); 00132 00134 CONTROLLER_STATUS status_; 00136 uint32_t decision_episode_; 00140 uint32_t decision_horizon_; 00141 00146 ros::ServiceServer stop_srv_; 00151 ros::ServiceServer start_srv_; 00156 ros::ServiceServer reset_srv_; 00157 }; 00158 } 00159 00160 #endif