00001 00025 #ifndef _CONTROLLER_POMDP_H_ 00026 #define _CONTROLLER_POMDP_H_ 00027 00028 #include <string> 00029 00030 #include <boost/shared_ptr.hpp> 00031 00032 #include <madp/QFunctionJointBeliefInterface.h> 00033 #include <madp/JointBeliefInterface.h> 00034 #include <madp/FSDist_COF.h> 00035 #include <mdm_library/control_layer_base.h> 00036 #include <mdm_library/WorldSymbol.h> 00037 #include <mdm_library/BeliefStateInfo.h> 00038 #include <mdm_library/FactoredDistribution.h> 00039 #include <mdm_library/decpomdp_loader.h> 00040 00041 00042 00043 namespace mdm_library 00044 { 00049 class ControllerPOMDP: public ControlLayerBase 00050 { 00051 public: 00057 ControllerPOMDP ( const std::string& problem_file, 00058 const CONTROLLER_STATUS initial_status = STARTED ); 00059 00063 size_t getNumberOfActions (); 00064 00068 size_t getNumberOfStates (); 00069 00073 size_t getNumberOfObservations (); 00074 00075 protected: 00083 void act ( const uint32_t observation ); 00084 00089 virtual void observationCallback ( const WorldSymbolConstPtr& msg ) = 0; 00090 00097 virtual void extBeliefCallback ( const BeliefStateInfoConstPtr& msg ); 00098 00106 virtual void isdCallback ( const FactoredDistributionConstPtr& msg ); 00107 00109 void publishAction ( uint32_t a ); 00110 00115 void publishExpectedReward ( uint32_t a ); 00116 00118 void publishCurrentBelief (); 00119 00121 void normalizeBelief ( boost::shared_ptr<JointBeliefInterface> b ); 00122 00124 boost::shared_ptr<DecPOMDPLoader> loader_; 00126 boost::shared_ptr<JointBeliefInterface> belief_; 00128 boost::shared_ptr<QFunctionJointBeliefInterface> Q_; 00130 uint32_t prev_action_; 00132 boost::shared_ptr<FSDist_COF> ISD_; 00133 00135 ros::Subscriber observation_sub_; 00137 ros::Subscriber ext_belief_estimate_sub_; 00139 ros::Subscriber isd_sub_; 00141 ros::Publisher current_belief_pub_; 00143 ros::Publisher action_pub_; 00145 ros::Publisher exp_reward_pub_; 00146 }; 00147 } 00148 00149 #endif