Go to the documentation of this file.00001
00025 #include <mdm_library/control_layer_base.h>
00026
00027
00028
00029 using namespace ros;
00030 using namespace std;
00031 using namespace mdm_library;
00032
00033
00034
00035 ControlLayerBase::
00036 ControlLayerBase ( const CONTROLLER_STATUS initial_status ) :
00037 status_ ( initial_status ),
00038 decision_episode_ ( 0 ),
00039 stop_srv_ ( nh_.advertiseService ( "stop", &ControlLayerBase::stopCallback, this ) ),
00040 start_srv_ ( nh_.advertiseService ( "start", &ControlLayerBase::startCallback, this ) ),
00041 reset_srv_ ( nh_.advertiseService ( "reset", &ControlLayerBase::resetCallback, this ) )
00042 {
00043 NodeHandle private_nh ( "~" );
00044 int dh;
00045 if ( private_nh.getParam ( "decision_horizon", dh ) )
00046 {
00047 decision_horizon_ = ( uint32_t ) dh;
00048 }
00049 else
00050 {
00051 decision_horizon_ = MDM_MAXHORIZON;
00052 }
00053 }
00054
00055
00056
00057 bool
00058 ControlLayerBase::
00059 stopCallback ( std_srvs::Empty::Request& request, std_srvs::Empty::Response& response )
00060 {
00061 stopController();
00062 return true;
00063 }
00064
00065
00066
00067 bool
00068 ControlLayerBase::
00069 startCallback ( std_srvs::Empty::Request& request, std_srvs::Empty::Response& response )
00070 {
00071 startController();
00072 return true;
00073 }
00074
00075
00076
00077 bool
00078 ControlLayerBase::
00079 resetCallback ( std_srvs::Empty::Request& request, std_srvs::Empty::Response& response )
00080 {
00081 resetController();
00082 return true;
00083 }
00084
00085
00086
00087
00088
00089 void
00090 ControlLayerBase::
00091 stopController ()
00092 {
00093 status_ = STOPPED;
00094 }
00095
00096
00097
00098 void
00099 ControlLayerBase::
00100 startController ()
00101 {
00102 decision_episode_ = 0;
00103 status_ = STARTED;
00104 }
00105
00106
00107
00108 void
00109 ControlLayerBase::
00110 resetController ()
00111 {
00113 stopController ();
00114 startController ();
00115 }
00116
00117
00118
00119 void
00120 ControlLayerBase::
00121 setStatus ( const CONTROLLER_STATUS status )
00122 {
00123 status_ = status;
00124 }
00125
00126
00127
00128
00129 uint32_t
00130 ControlLayerBase::
00131 getDecisionEpisode ()
00132 {
00133 return decision_episode_;
00134 }
00135
00136
00137
00138 ControlLayerBase::CONTROLLER_STATUS
00139 ControlLayerBase::
00140 getStatus ()
00141 {
00142 return status_;
00143 }
00144
00145
00146
00147 uint32_t
00148 ControlLayerBase::
00149 getDecisionHorizon ()
00150 {
00151 return decision_horizon_;
00152 }
00153
00154
00155
00156 void
00157 ControlLayerBase::
00158 resetDecisionEpisode ()
00159 {
00160 decision_episode_ = 0;
00161 }
00162
00163
00164
00165 uint32_t
00166 ControlLayerBase::
00167 incrementDecisionEpisode ()
00168 {
00169 if ( ( decision_episode_ + 1 ) >= decision_horizon_ )
00170 {
00171 resetController();
00172 return decision_episode_;
00173 }
00174 return ++decision_episode_;
00175 }