mdp_planning_alg_node.cpp

Go to the documentation of this file.

#include "mdp_planning_alg_node.h"

MdpPlanningAlgNode::MdpPlanningAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<MdpPlanningAlgorithm>()
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  //TODO read policy vector from parameter server and proper print
  int policy_data[] = {0, 21, 10, 11, 14, 20, 20, 20, 20, 20, 0, 21, 0, 11, 0, 1, 21, 1, 1, 1, 4, 21, 10, 11, 1};

  policy_ = new std::vector<int>(policy_data, policy_data + sizeof(policy_data) / sizeof(int) );
  ROS_WARN("Using default,hardcoded MDP policy:\n{1, 22, 11, 12, 15, 21, 21, 21, 21, 21, 1, 22, 1, 12, 1, 2, 22, 2, 2, 2, 5, 22, 11, 12, 2}");

  // [init publishers]
  this->action_from_policy_publisher_ = this->public_node_handle_.advertise<std_msgs::UInt32>("action_from_policy", 100);
  
  // [init subscribers]
  
  // [init services]
  this->obs2action_server_ = this->public_node_handle_.advertiseService("obs2action", &MdpPlanningAlgNode::obs2actionCallback, this);
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

MdpPlanningAlgNode::~MdpPlanningAlgNode(void)
{
  // [free dynamic memory]
}

void MdpPlanningAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->UInt32_msg.data = my_var;
  
  // [fill srv structure and make request to the server]
  //planning_action_id_srv_.request.data = my_var; 
  //ROS_INFO("MdpPlanningAlgNode:: Sending New Request!"); 
  //if (planning_action_id_client_.call(planning_action_id_srv)) 
  //{ 
    //ROS_INFO("MdpPlanningAlgNode:: Response: %s", planning_action_id_srv_.response.result); 
  //} 
  //else 
  //{ 
    //ROS_INFO("MdpPlanningAlgNode:: Failed to Call Server on topic planning_action_id "); 
  //}
  
  // [fill action structure and make request to the action server]

  // [publish messages]
}

/*  [subscriber callbacks] */

/*  [service callbacks] */
bool MdpPlanningAlgNode::obs2actionCallback(estirabot_msgs::obs2action::Request &req, estirabot_msgs::obs2action::Response &res) 
{ 
  ROS_INFO("MdpPlanningAlgNode::obs2actionCallback: New Request Received!"); 

  res.goal_reached = false;
  int state = req.observation; //MDP full observability
  res.action = policy_->at(state); 
  //TODO bond mdp with pomdp domain description instead of hardcoding FINALSTATE id
  if(state == FINALSTATE){ 
      ROS_INFO("Final state reached");
      res.goal_reached = true;
  }else{
      res.goal_reached = false;
  }

  UInt32_msg_.data = res.action; 
  action_from_policy_publisher_.publish(UInt32_msg_);

  return true; 
}

/*  [action callbacks] */

/*  [action requests] */

void MdpPlanningAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void MdpPlanningAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<MdpPlanningAlgNode>(argc, argv, "mdp_planning_alg_node");
}