Go to the documentation of this file.00001
00021 #include <madp/QAV.h>
00022 #include <madp/PerseusConstrainedPOMDPPlanner.h>
00023 #include <madp/JointBeliefEventDriven.h>
00024 #include <madp/NullPlanner.h>
00025
00026 #include <mdm_library/controller_event_pomdp.h>
00027
00028
00029
00030 using namespace ros;
00031 using namespace std;
00032 using namespace mdm_library;
00033
00034
00035
00036 ControllerEventPOMDP::
00037 ControllerEventPOMDP ( string const& problem_file,
00038 string const& value_function_file,
00039 const CONTROLLER_STATUS initial_status ) :
00040 ControllerPOMDP ( problem_file, initial_status )
00041 {
00042 try
00043 {
00044 bool isSparse;
00045 NodeHandle private_nh ( "~" );
00046 if ( !private_nh.getParam ( "is_sparse", isSparse ) )
00047 {
00048 isSparse = false;
00049 }
00050 int false_negative_obs = -1;
00051 private_nh.getParam ( "false_negative_obs", false_negative_obs );
00052
00053 QAVParameters qavParams;
00054 qavParams.falseNegativeObs = false_negative_obs;
00055 PlanningUnitMADPDiscreteParameters puParams;
00056 puParams.SetEventObservability ( loader_->GetDecPOMDP()->GetEventObservability() );
00057 double discount;
00058 if ( !private_nh.getParam ( "discount", discount ) )
00059 {
00060 discount = 0.9;
00061
00062 }
00063 loader_->GetDecPOMDP()->SetDiscount ( discount );
00064
00065 boost::shared_ptr<PlanningUnitDecPOMDPDiscrete> np ( new NullPlanner ( puParams, MAXHORIZON, loader_->GetDecPOMDP().get() ) );
00067 Q_ = boost::shared_ptr<QAV<PerseusConstrainedPOMDPPlanner> >
00068 ( new QAV<PerseusConstrainedPOMDPPlanner> ( np,
00069 value_function_file,
00070 qavParams ) );
00071
00072 belief_ = boost::shared_ptr<JointBeliefEventDriven>
00073 ( new JointBeliefEventDriven ( loader_->GetDecPOMDP()->GetNrStates(), false_negative_obs ) );
00074 if ( initial_status == STARTED )
00075 {
00076 startController();
00077 }
00078 }
00079 catch ( E& e )
00080 {
00081 e.Print();
00082 abort();
00083 }
00084 }
00085
00086
00087
00088 void
00089 ControllerEventPOMDP::
00090 observationCallback ( const WorldSymbolConstPtr& msg )
00091 {
00092 act ( msg->world_symbol );
00093 }
00094
00095
00096
00097 void
00098 ControllerEventPOMDP::
00099 startController ()
00100 {
00101 if ( belief_ == 0 )
00102 {
00103 ROS_WARN ( "ControllerPOMDP:: Attempted to start controller, but the belief state hasn't been initialized." );
00104 return;
00105 }
00106 if ( ISD_ != 0 )
00107 {
00108 belief_->Set ( ISD_->ToVectorOfDoubles() );
00109 }
00110 else
00111 {
00112 belief_->Set ( * ( loader_->GetDecPOMDP()->GetISD() ) );
00113 }
00114 setStatus ( STARTED );
00115 resetDecisionEpisode();
00116 act ( 0 );
00117 }