#include <controller_pomdp.h>
Public Member Functions | |
ControllerPOMDP (const std::string &problem_file, const CONTROLLER_STATUS initial_status=STARTED) | |
size_t | getNumberOfActions () |
size_t | getNumberOfObservations () |
size_t | getNumberOfStates () |
Protected Member Functions | |
void | act (const uint32_t observation) |
virtual void | extBeliefCallback (const BeliefStateInfoConstPtr &msg) |
virtual void | isdCallback (const FactoredDistributionConstPtr &msg) |
void | normalizeBelief (boost::shared_ptr< JointBeliefInterface > b) |
virtual void | observationCallback (const WorldSymbolConstPtr &msg)=0 |
void | publishAction (uint32_t a) |
void | publishCurrentBelief () |
void | publishExpectedReward (uint32_t a) |
Protected Attributes | |
ros::Publisher | action_pub_ |
boost::shared_ptr < JointBeliefInterface > | belief_ |
ros::Publisher | current_belief_pub_ |
ros::Publisher | exp_reward_pub_ |
ros::Subscriber | ext_belief_estimate_sub_ |
boost::shared_ptr< FSDist_COF > | ISD_ |
ros::Subscriber | isd_sub_ |
boost::shared_ptr< DecPOMDPLoader > | loader_ |
ros::Subscriber | observation_sub_ |
uint32_t | prev_action_ |
boost::shared_ptr < QFunctionJointBeliefInterface > | Q_ |
ControllerPOMDP is an abstract class that provides the basic functionality for Factored Discrete POMDPs where the agent policy is implicitly described by a value function (or Q-value function).
Definition at line 49 of file controller_pomdp.h.
ControllerPOMDP::ControllerPOMDP | ( | const std::string & | problem_file, |
const CONTROLLER_STATUS | initial_status = STARTED |
||
) |
Constructor.
problem_file | the POMDP problem file in any MADP-compatible format. |
initial_status | whether this controller should be auto-started upon construction or not. |
Definition at line 43 of file controller_pomdp.cpp.
void ControllerPOMDP::act | ( | const uint32_t | observation | ) | [protected] |
Main POMDP control step. Selects an action given an observation. The belief state will be updated according to the current observation and last action. The action with the highest value at the updated belief will be selected and published in the "action" topic in the current namespace.
observation | the observation symbol for this decision step. |
Definition at line 61 of file controller_pomdp.cpp.
void ControllerPOMDP::extBeliefCallback | ( | const BeliefStateInfoConstPtr & | msg | ) | [protected, virtual] |
The callback for external belief state estimates. Keep in mind that this information *will* override the POMDP's internal belief state. This can be used in scenarios where a belief state is naturally being estimated elsewhere, and the controller is only required to select an appropriate response.
Definition at line 184 of file controller_pomdp.cpp.
size_t ControllerPOMDP::getNumberOfActions | ( | ) |
Returns the number of actions in the POMDP, without exposing the DecPOMDPLoader.
Definition at line 157 of file controller_pomdp.cpp.
size_t ControllerPOMDP::getNumberOfObservations | ( | ) |
Returns the number of observations in the POMDP, without exposing the DecPOMDPLoader.
Definition at line 175 of file controller_pomdp.cpp.
size_t ControllerPOMDP::getNumberOfStates | ( | ) |
Returns the number of states in the POMDP, without exposing the DecPOMDPLoader.
Definition at line 166 of file controller_pomdp.cpp.
void ControllerPOMDP::isdCallback | ( | const FactoredDistributionConstPtr & | msg | ) | [protected, virtual] |
The callback for initial state distribution estimates. This information *will not* override the POMDP's internal belief state. It will set the ISD for this problem, which will be loaded after a controller reset, which will happen after the horizon runs out, or the controller is stopped/started by a higher level module.
handles normalization internally.
Definition at line 197 of file controller_pomdp.cpp.
void ControllerPOMDP::normalizeBelief | ( | boost::shared_ptr< JointBeliefInterface > | b | ) | [protected] |
Normalizes a given JointBeliefInterface so that it sums to 1.
normalizing belief to 1
Definition at line 236 of file controller_pomdp.cpp.
virtual void mdm_library::ControllerPOMDP::observationCallback | ( | const WorldSymbolConstPtr & | msg | ) | [protected, pure virtual] |
The callback for observations coming in through the "observation" topic in the current namespace. Each particular POMDP execution strategy (synchronous/asynchronous) will have to implement this callback.
Implemented in mdm_library::ControllerEventPOMDP, and mdm_library::ControllerTimedPOMDP.
void ControllerPOMDP::publishAction | ( | uint32_t | a | ) | [protected] |
Publishes an action symbol to the "action" topic.
Definition at line 114 of file controller_pomdp.cpp.
void ControllerPOMDP::publishCurrentBelief | ( | ) | [protected] |
Publishes the belief state after an update, useful for debugging purposes.
Definition at line 143 of file controller_pomdp.cpp.
void ControllerPOMDP::publishExpectedReward | ( | uint32_t | a | ) | [protected] |
Publishes the -immediate- expected reward for taking action "a" in the current belief state, to the "reward" topic.
Definition at line 126 of file controller_pomdp.cpp.
Publisher to the "action" topic.
Definition at line 143 of file controller_pomdp.h.
boost::shared_ptr<JointBeliefInterface> mdm_library::ControllerPOMDP::belief_ [protected] |
Pointer to the internal belief state.
Definition at line 126 of file controller_pomdp.h.
Publisher to the "current_belief" topic.
Definition at line 141 of file controller_pomdp.h.
Publisher to the "reward" topic.
Definition at line 145 of file controller_pomdp.h.
Subscriber to the "ext_belief_estimate" topic.
Definition at line 137 of file controller_pomdp.h.
boost::shared_ptr<FSDist_COF> mdm_library::ControllerPOMDP::ISD_ [protected] |
Initial State Distribution.
Definition at line 132 of file controller_pomdp.h.
Subscriber to the "initial_state_distribution" topic.
Definition at line 139 of file controller_pomdp.h.
boost::shared_ptr<DecPOMDPLoader> mdm_library::ControllerPOMDP::loader_ [protected] |
Pointer to a "loader" object which will actually parse the problem file.
Definition at line 124 of file controller_pomdp.h.
Subscriber to the "observation" topic.
Definition at line 135 of file controller_pomdp.h.
uint32_t mdm_library::ControllerPOMDP::prev_action_ [protected] |
Previous action. Needed for belief updates.
Definition at line 130 of file controller_pomdp.h.
boost::shared_ptr<QFunctionJointBeliefInterface> mdm_library::ControllerPOMDP::Q_ [protected] |
Pointer to this problem's Q-Value function.
Definition at line 128 of file controller_pomdp.h.