pomdp.cpp

Go to the documentation of this file.

#include "pomdp.h"

Pomdp::Pomdp(){

  int numstates, numactions, numobservations;
  last_action_ = -1;
  //init class attributes if necessary
  this->actioncount = 0;

  pomdpfile_ = std::string("data/pomdpfile.txt");
  if(node_handle_.getParam("pomdpfile", pomdpfile_)) {
    ROS_INFO("pomdpfile global path found, using %s", pomdpfile_.c_str());
  } else {
    std::ifstream pomdpdata(pomdpfile_.c_str());
    ROS_WARN("pomdpfile global path not found, using default %s", pomdpfile_.c_str());
    if(!pomdpdata){
      ROS_FATAL("File %s doesn't exist: aborting.",pomdpfile_.c_str()); 
    }
  }

  srand( time(NULL) ); //we do this at policy too...  
  GenericPolicy::getDimensions(pomdpfile_,&numstates,&numactions,&numobservations); 
  myPolicy = new GenericPolicy(pomdpfile_,numactions,numstates,numobservations);

  //init class attributes if necessary
  this->execution_flag = false;

  //string for port names

  // [init publishers]
  this->focused_obj_label_publisher = this->node_handle_.advertise<std_msgs::Int32>("/planner/focused_obj_label", 5);
  this->belief_summary_publisher = this->node_handle_.advertise<estirabot_msgs::belief_summary>("/planner/belief_summary", 5);
  this->belief_publisher = this->node_handle_.advertise<estirabot_msgs::belief>("/planner/belief", 5);
  
  // [init subscribers]
  
  // [init services]
  execution_flow_control_server_ = this->node_handle_.advertiseService("/planner/execution_flow_control", &Pomdp::execution_flow_controlCallback, this);
  obs2action_server_ = this->node_handle_.advertiseService("/planner/obs2action", &Pomdp::obs2actionCallback, this);
  resetPOMDP_server_ = this->node_handle_.advertiseService("/planner/resetPOMDP", &Pomdp::resetPOMDPCallback, this);
  
  // [init clients]

  // [init action servers]
  
  // [init action clients]

}

void Pomdp::mainLoop(void){

}

bool Pomdp::resetPOMDPCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){

    myPolicy->resetToInitialBelief();
    publish_belief();
    return true; 
}
bool Pomdp::obs2actionCallback(estirabot_msgs::obs2action::Request &req, estirabot_msgs::obs2action::Response &res){

    int obs, state;

    if(!req.first){
        //myPolicy->logState(LOGFILE);
        //update belief
        //obs = myPolicy->domain.get_observation(req.num_objectsA, req.num_objectsB, MAXNUMB); 
        obs = req.observation;
        myPolicy->transformState(last_action_,obs);
        ROS_INFO("Belief Updated with observation %d and action %d",obs,last_action_); 

        //visualization
        state = myPolicy->mostProbableState();
        ROS_INFO("Most probable state %d with probability %f", state, myPolicy->getStateProbability(state));
    

        if(!myPolicy->isFinal(state) || myPolicy->getStateProbability(state) < 0.90){ 
            res.goal_reached = false;
        }else{
            ROS_INFO("Final state reached with probability %f", myPolicy->getStateProbability(myPolicy->mostProbableState()));
            res.goal_reached = true;
        }
    }
    //get action
    last_action_ = myPolicy->getBestAction();
    ROS_INFO("Best action is %d ", last_action_);

    res.action = last_action_;

    publish_belief();

    return true; 
}

bool Pomdp::execution_flow_controlCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){

    //stop execution when called
    this->execution_flag = !this->execution_flag;
    return true; 
}

void Pomdp::publish_belief(){
    std::list<std::pair<double,int> > state_probability_list;
    std::vector<int> indexlist;
    std::list<std::pair<double,int> >::iterator it;
    std::vector<double> objInA, objInB;  
    double averageObjInA=0, averageObjInB=0;
    double uncertaintyA=0, uncertaintyB=0;
    int numA, numB, firstNumA, firstNumB;

    ROS_INFO("Belief (top four):");
    myPolicy->probableStatesList(state_probability_list, indexlist, 0.05);
    for(int state=0; state < 4;state++){
        if(myPolicy->getStateProbability(indexlist[state]) < 0.05)
            break;
        if((unsigned int)indexlist[state] > indexlist.size()) 
            continue;
        ROS_INFO("%f", myPolicy->getStateProbability(indexlist[state]));
    }

//    myPolicy->getBeliefAverageAtPiles(objInA,objInB);
//    for ( unsigned int i=0; i < objInA.size(); i++ )
//        averageObjInA += i*objInA[i]; 
//    for ( unsigned int i=0; i < objInB.size(); i++ )
//        averageObjInB += i*objInB[i]; 
//
//    myPolicy->domain.get_num_obj(state_probability_list.front().second, firstNumA, firstNumB, MAXNUMB);
//
//    for ( it=state_probability_list.begin(); it != state_probability_list.end(); it++ ){
//        std::pair<double,int> aux = *it;
//        if(aux.first == 0)
//            break;
//        myPolicy->domain.get_num_obj(aux.second, numA, numB, MAXNUMB);
//        if( numA == firstNumA) 
//            uncertaintyA += aux.first;
//        if( numB == firstNumB)
//            uncertaintyB += aux.first;
//    }
//    
    this->belief_summary_msg.num_objects_A = 0; 
    this->belief_summary_msg.uncertainty_A = 0;
    this->belief_summary_msg.num_objects_B = 0; 
    this->belief_summary_msg.uncertainty_B = 0;
    this->belief_summary_msg.uncertainty_total = 1 - myPolicy->getStateProbability(indexlist[0]);
    ROS_INFO("Publishing belief and overall uncertainty %f", belief_summary_msg.uncertainty_total);
    belief_summary_publisher.publish(this->belief_summary_msg);

    this->belief_msg.states_probabilities.clear();
    myPolicy->probableStatesList(this->belief_msg.states_probabilities);
    this->belief_msg.action_values.clear();
    myPolicy->actionValueList(this->belief_msg.action_values);

    belief_publisher.publish(this->belief_msg);

}

/* main function */
int main(int argc,char *argv[])
{
    ros::init(argc, argv, "pomdp");
    Pomdp pomdp;
    ros::Rate loop_rate(10); 
    while(ros::ok()){
      pomdp.mainLoop();
      ros::spinOnce();
      loop_rate.sleep(); 
    }
}