Go to the documentation of this file.00001
00025 #include <mdm_library/controller_timed_mdp.h>
00026
00027
00028
00029 using namespace std;
00030 using namespace mdm_library;
00031
00032 #ifdef HAVE_MADP
00033
00034 ControllerTimedMDP::
00035 ControllerTimedMDP ( const string& policy_file_path,
00036 const string& problem_file_path,
00037 const CONTROLLER_STATUS initial_status ) :
00038 ControllerMDP ( policy_file_path, problem_file_path, initial_status ),
00039 initial_state_known_ ( false )
00040 {
00041 if ( !nh_.hasParam ( "decision_period" ) )
00042 {
00043 ROS_ERROR ( "Decision period was not specified. Synchronous MDP controllers require the 'decision_period' parameter." );
00044 abort();
00045 }
00046
00047 timer_ = nh_.createTimer ( 0, &ControllerTimedMDP::timerCallback, this, true, false );
00048 scheduleTimer();
00049 }
00050
00051 #endif
00052
00053 ControllerTimedMDP::
00054 ControllerTimedMDP ( const string& policy_file_path,
00055 const CONTROLLER_STATUS initial_status ) :
00056 ControllerMDP ( policy_file_path, initial_status ),
00057 initial_state_known_ ( false )
00058 {
00059 if ( !nh_.hasParam ( "decision_period" ) )
00060 {
00061 ROS_ERROR ( "Decision period was not specified. Synchronous MDP controllers require the 'decision_period' parameter." );
00062 abort();
00063 }
00064
00065 timer_ = nh_.createTimer ( 0, &ControllerTimedMDP::timerCallback, this, true, false );
00066 scheduleTimer();
00067 }
00068
00069
00070
00071 void
00072 ControllerTimedMDP::
00073 stateCallback ( const WorldSymbolConstPtr& msg )
00074 {
00075 s_ = msg->world_symbol;
00076 if ( !initial_state_known_ )
00077 {
00078 initial_state_known_ = true;
00079 }
00080 }
00081
00082
00083
00084 void
00085 ControllerTimedMDP::
00086 scheduleTimer()
00087 {
00088 double dp;
00089 nh_.getParam ( "decision_period", dp );
00090
00091 double timeToWait = dp - fmod ( ros::Time::now().toSec(), dp );
00092 ros::Duration d ( timeToWait );
00093
00094 timer_.stop();
00095 timer_.setPeriod ( d );
00096 timer_.start();
00097 }
00098
00099
00100
00101 void
00102 ControllerTimedMDP::
00103 timerCallback ( const ros::TimerEvent& timer_event )
00104 {
00105 if ( initial_state_known_ )
00106 {
00107 step();
00108 }
00109
00110 scheduleTimer();
00111 }
00112
00113
00114
00115 void
00116 ControllerTimedMDP::
00117 step()
00118 {
00119 act ( s_ );
00120 }