pomdp2piles.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...  
  Policy2piles::getDimensions(pomdpfile_,&numstates,&numactions,&numobservations, 1); 
  myPolicy = new Policy2piles(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 %s (%d) and action %s (%d)", myPolicy->domain.obs2string(obs).c_str(),obs,myPolicy->domain.action2string(last_action_).c_str(),last_action_); 

        //visualization
        state = myPolicy->mostProbableState();
        ROS_INFO("Most probable state %s with probability %f", myPolicy->domain.state2string(state).c_str(), myPolicy->getStateProbability(state));
    

        if(!myPolicy->domain.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 %s (%d)", myPolicy->domain.action2string(last_action_).c_str(),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("%s %f", myPolicy->domain.state2string(indexlist[state]).c_str(), 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]; 
//    for ( int i=0; i < objInB.size(); i++ )
//        this->belief_summary_msg.num_objects_B[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 = averageObjInA; 
    this->belief_summary_msg.uncertainty_A = 1 - uncertaintyA;
    this->belief_summary_msg.num_objects_B = averageObjInB; 
    this->belief_summary_msg.uncertainty_B = 1 - uncertaintyB;
    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.belief.assign(actualstates,actualstates+numstates)
    //PUBLISHING PAIRS
    this->belief_msg.states_probabilities.clear();
    for ( it=state_probability_list.begin(); it != state_probability_list.end(); it++ ){
        std::pair<double,int> aux = *it;
        this->belief_msg.states_probabilities.push_back(aux.first);
    }

    //temporary publish all states unordered
    this->belief_msg.states_probabilities.clear();
    for(int i=0; i< 25; i++){
      this->belief_msg.states_probabilities.push_back(myPolicy->getStateProbability(i));
    }

    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(); 
    }
}