controller_timed_pomdp.cpp

Go to the documentation of this file.

#include <madp/QAV.h>
#include <madp/PerseusPOMDPPlanner.h>
#include <madp/JointBelief.h>
#include <madp/JointBeliefSparse.h>
#include <madp/NullPlanner.h>

#include <mdm_library/controller_timed_pomdp.h>



using namespace std;
using namespace mdm_library;



ControllerTimedPOMDP::
ControllerTimedPOMDP ( string const& problem_file,
                       string const& value_function_file,
                       const CONTROLLER_STATUS initial_status ) :
    ControllerPOMDP ( problem_file, initial_status ),
    o_ ( 0 )
{
    if ( !nh_.hasParam ( "decision_period" ) )
    {
        ROS_ERROR ( "Decision period was not specified. Synchronous MDP controllers require the 'decision_period' parameter." );
        abort();
    }

    try
    {
        timer_ = nh_.createTimer ( 0, &ControllerTimedPOMDP::timerCallback, this, true, false );
        scheduleTimer();

        bool isSparse;
        if ( !nh_.getParam ( "is_sparse", isSparse ) )
        {
            isSparse = false;
        }


        boost::shared_ptr<PlanningUnitDecPOMDPDiscrete> np ( new NullPlanner ( loader_->GetDecPOMDP().get() ) );
        Q_ = boost::shared_ptr<QAV<PerseusPOMDPPlanner> >
             ( new QAV<PerseusPOMDPPlanner> ( np,
                                              value_function_file ) );

        if ( isSparse )
            belief_ = boost::shared_ptr<JointBeliefSparse>
                      ( new JointBeliefSparse ( loader_->GetDecPOMDP()->GetNrStates() ) );
        else
            belief_ = boost::shared_ptr<JointBelief>
                      ( new JointBelief ( loader_->GetDecPOMDP()->GetNrStates() ) );
        if ( initial_status == STARTED )
        {
            startController();
        }
    }
    catch ( E& e )
    {
        e.Print();
        abort();
    }
}



void
ControllerTimedPOMDP::
observationCallback ( const WorldSymbolConstPtr& msg )
{
    o_ = msg->world_symbol;
}



void
ControllerTimedPOMDP::
scheduleTimer()
{
    double dp;
    nh_.getParam ( "decision_period", dp );

    double timeToWait = dp - fmod ( ros::Time::now().toSec(), dp );
    ros::Duration d ( timeToWait );

    timer_.stop();
    timer_.setPeriod ( d );
    timer_.start();
}



void
ControllerTimedPOMDP::
timerCallback ( const ros::TimerEvent& timerEvent )
{
    step();
    scheduleTimer();
}



void
ControllerTimedPOMDP::
step()
{
    act ( o_ );
}



void
ControllerTimedPOMDP::
startController ()
{
    if ( belief_ == 0 )
    {
        ROS_WARN ( "ControllerTimedPOMDP:: Attempted to start controller, but the belief state hasn't been initialized." );
        return;
    }
    if ( ISD_ != 0 )
    {
        belief_->Set ( ISD_->ToVectorOfDoubles() );
    }
    else
    {
        belief_->Set ( * ( loader_->GetDecPOMDP()->GetISD() ) );
    }
    setStatus ( STARTED );
    resetDecisionEpisode();
}