00001 #include "prada_planner_alg_node.h" 00002 00003 PradaPlannerAlgNode::PradaPlannerAlgNode(void) : 00004 algorithm_base::IriBaseAlgorithm<PradaPlannerAlgorithm>() 00005 { 00006 //init class attributes if necessary 00007 //this->loop_rate_ = 2;//in [Hz] 00008 00009 // [init publishers] 00010 00011 // [init subscribers] 00012 00013 // [init services] 00014 this->getBestAction_server_ = this->public_node_handle_.advertiseService("getBestAction", &PradaPlannerAlgNode::getBestActionCallback, this); 00015 00016 // [init clients] 00017 00018 // [init action servers] 00019 00020 // [init action clients] 00021 00022 // other 00023 } 00024 00025 PradaPlannerAlgNode::~PradaPlannerAlgNode(void) 00026 { 00027 // [free dynamic memory] 00028 } 00029 00030 void PradaPlannerAlgNode::mainNodeThread(void) 00031 { 00032 // [fill msg structures] 00033 00034 // [fill srv structure and make request to the server] 00035 00036 // [fill action structure and make request to the action server] 00037 00038 // [publish messages] 00039 } 00040 00041 /* [subscriber callbacks] */ 00042 00043 /* [service callbacks] */ 00044 bool PradaPlannerAlgNode::getBestActionCallback(iri_planning_msgs::PradaPlanSrv::Request &req, iri_planning_msgs::PradaPlanSrv::Response &res) 00045 { 00046 ROS_DEBUG("PradaPlannerAlgNode::getBestActionCallback: New Request Received!"); 00047 00048 //use appropiate mutex to shared variables if necessary 00049 //this->alg_.lock(); 00050 //this->getBestAction_mutex_.enter(); 00051 00052 if (req.updated_rules) 00053 this->alg_.reloadRules(); 00054 00055 this->alg_.setPlanStateFromFile(); 00056 00057 // check if a solution was found 00058 std::stringstream ss; 00059 relational::Literal *action; 00060 relational::LitL plan = alg_.getCompletePlan( alg_.getRecommendedPlanLength(relational::reason::getConstants().N) ); 00061 if (plan.d0 > 0) { // if there is a plan 00062 for (size_t i = 0; i < plan.d0; i++) { // d0 == size() in std::vector 00063 estirabot_msgs::ArrayIndexes target_ellipses; 00064 00065 // get action name 00066 action = plan(i); 00067 ss.str(""); 00068 ss << action->s->name; 00069 res.actions.push_back(ss.str()); 00070 00071 ROS_DEBUG_STREAM("PradaPlannerAlgNode::getBestActionCallback: adding action "<<ss.str()); 00072 // get action params 00073 for (size_t j = 0; j < action->args.d0; j ++) { 00074 target_ellipses.indexes.push_back(action->args(j) - 60); 00075 ROS_DEBUG_STREAM(action->args(j)); 00076 } 00077 res.target_ellipses.push_back(target_ellipses); 00078 } 00079 00080 res.succeeded = true; 00081 } 00082 else { 00083 res.succeeded = false; 00084 ss << "None"; 00085 res.actions.push_back(ss.str()); 00086 } 00087 00088 return true; 00089 } 00090 00091 /* [action callbacks] */ 00092 00093 /* [action requests] */ 00094 00095 void PradaPlannerAlgNode::node_config_update(Config &config, uint32_t level) 00096 { 00097 this->alg_.lock(); 00098 00099 this->alg_.unlock(); 00100 } 00101 00102 void PradaPlannerAlgNode::addNodeDiagnostics(void) 00103 { 00104 } 00105 00106 /* main function */ 00107 int main(int argc,char *argv[]) 00108 { 00109 return algorithm_base::main<PradaPlannerAlgNode>(argc, argv, "prada_planner_alg_node"); 00110 }