00001 #include "mdp_planning_alg_node.h"
00002
00003 MdpPlanningAlgNode::MdpPlanningAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<MdpPlanningAlgorithm>()
00005 {
00006
00007
00008
00009 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};
00010
00011 policy_ = new std::vector<int>(policy_data, policy_data + sizeof(policy_data) / sizeof(int) );
00012 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}");
00013
00014
00015 this->action_from_policy_publisher_ = this->public_node_handle_.advertise<std_msgs::UInt32>("action_from_policy", 100);
00016
00017
00018
00019
00020 this->obs2action_server_ = this->public_node_handle_.advertiseService("obs2action", &MdpPlanningAlgNode::obs2actionCallback, this);
00021
00022
00023
00024
00025
00026
00027 }
00028
00029 MdpPlanningAlgNode::~MdpPlanningAlgNode(void)
00030 {
00031
00032 }
00033
00034 void MdpPlanningAlgNode::mainNodeThread(void)
00035 {
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054 }
00055
00056
00057
00058
00059 bool MdpPlanningAlgNode::obs2actionCallback(estirabot_msgs::obs2action::Request &req, estirabot_msgs::obs2action::Response &res)
00060 {
00061 ROS_INFO("MdpPlanningAlgNode::obs2actionCallback: New Request Received!");
00062
00063 res.goal_reached = false;
00064 int state = req.observation;
00065 res.action = policy_->at(state);
00066
00067 if(state == FINALSTATE){
00068 ROS_INFO("Final state reached");
00069 res.goal_reached = true;
00070 }else{
00071 res.goal_reached = false;
00072 }
00073
00074 UInt32_msg_.data = res.action;
00075 action_from_policy_publisher_.publish(UInt32_msg_);
00076
00077 return true;
00078 }
00079
00080
00081
00082
00083
00084 void MdpPlanningAlgNode::node_config_update(Config &config, uint32_t level)
00085 {
00086 this->alg_.lock();
00087
00088 this->alg_.unlock();
00089 }
00090
00091 void MdpPlanningAlgNode::addNodeDiagnostics(void)
00092 {
00093 }
00094
00095
00096 int main(int argc,char *argv[])
00097 {
00098 return algorithm_base::main<MdpPlanningAlgNode>(argc, argv, "mdp_planning_alg_node");
00099 }