From 2f55b3c3e702eb2db376d2f728b3fdb986c237c5 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 1 Feb 2020 19:54:13 +0100 Subject: [PATCH 1/5] [knowledge_msgs] Added a srv for retrieving tasks --- rosplan_knowledge_msgs/CMakeLists.txt | 1 + rosplan_knowledge_msgs/srv/GetTaskService.srv | 3 +++ 2 files changed, 4 insertions(+) create mode 100644 rosplan_knowledge_msgs/srv/GetTaskService.srv diff --git a/rosplan_knowledge_msgs/CMakeLists.txt b/rosplan_knowledge_msgs/CMakeLists.txt index 167f7dc75..cf8e8e223 100644 --- a/rosplan_knowledge_msgs/CMakeLists.txt +++ b/rosplan_knowledge_msgs/CMakeLists.txt @@ -28,6 +28,7 @@ add_message_files( add_service_files( FILES GetAttributeService.srv + GetTaskService.srv GetInstanceService.srv GetMetricService.srv KnowledgeUpdateService.srv diff --git a/rosplan_knowledge_msgs/srv/GetTaskService.srv b/rosplan_knowledge_msgs/srv/GetTaskService.srv new file mode 100644 index 000000000..87f0d11c6 --- /dev/null +++ b/rosplan_knowledge_msgs/srv/GetTaskService.srv @@ -0,0 +1,3 @@ +# Get all tasks. The results are passed as an array of KnowledgeItems +--- +rosplan_knowledge_msgs/KnowledgeItem[] attributes From 3a40a68cba23541d6564853d8ee3234078110f39 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 1 Feb 2020 20:40:37 +0100 Subject: [PATCH 2/5] [knowledge_msgs/KnowledgeUpdateService] Added constants for adding and removing tasks --- rosplan_knowledge_msgs/srv/KnowledgeUpdateService.srv | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rosplan_knowledge_msgs/srv/KnowledgeUpdateService.srv b/rosplan_knowledge_msgs/srv/KnowledgeUpdateService.srv index 067ccd908..f6c6360b0 100644 --- a/rosplan_knowledge_msgs/srv/KnowledgeUpdateService.srv +++ b/rosplan_knowledge_msgs/srv/KnowledgeUpdateService.srv @@ -7,6 +7,8 @@ uint8 REMOVE_KNOWLEDGE = 2 uint8 REMOVE_GOAL = 3 uint8 ADD_METRIC = 4 uint8 REMOVE_METRIC = 5 +uint8 ADD_TASK = 6 +uint8 REMOVE_TASK = 7 uint8 update_type rosplan_knowledge_msgs/KnowledgeItem knowledge From 9066023e8befd94cd2057284bd7c41809ab2d5e2 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 1 Feb 2020 21:47:06 +0100 Subject: [PATCH 3/5] [knowledge_base] Added HDDL knowledge base interfaces These mostly extend the PDDL knowledge base interfaces --- rosplan_knowledge_base/CMakeLists.txt | 2 + .../rosplan_knowledge_base/HDDLDomainParser.h | 11 + .../HDDLKnowledgeBase.h | 18 ++ .../HDDLProblemParser.h | 11 + .../rosplan_knowledge_base/KnowledgeBase.h | 5 + .../KnowledgeBaseFactory.h | 4 +- .../PDDLKnowledgeBase.h | 22 +- .../VALVisitorProblem.h | 2 + .../src/HDDLKnowledgeBase.cpp | 19 ++ rosplan_knowledge_base/src/KnowledgeBase.cpp | 83 +++++++- .../src/KnowledgeBaseFactory.cpp | 3 +- .../src/VALVisitorProblem.cpp | 9 +- rosplan_knowledge_base/src/VALfiles/ptree.cpp | 30 +-- rosplan_knowledge_base/src/VALfiles/ptree.h | 200 ++++++++++-------- 14 files changed, 286 insertions(+), 133 deletions(-) create mode 100644 rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLDomainParser.h create mode 100644 rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLKnowledgeBase.h create mode 100644 rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLProblemParser.h create mode 100644 rosplan_knowledge_base/src/HDDLKnowledgeBase.cpp diff --git a/rosplan_knowledge_base/CMakeLists.txt b/rosplan_knowledge_base/CMakeLists.txt index 23a7152f6..c71fb2949 100644 --- a/rosplan_knowledge_base/CMakeLists.txt +++ b/rosplan_knowledge_base/CMakeLists.txt @@ -62,6 +62,7 @@ set(KNOWLEDGE_SOURCES src/PDDLDomainParser.cpp src/PDDLProblemParser.cpp src/PDDLKnowledgeBase.cpp + src/HDDLKnowledgeBase.cpp src/RDDLTaskParser.cpp src/RDDLKnowledgeBase.cpp src/RDDLExprUtils.cpp @@ -70,6 +71,7 @@ set(KNOWLEDGE_SOURCES src/VALVisitorPredicate.cpp src/VALVisitorProblem.cpp src/PPDDLKnowledgeBase.cpp + src/HDDLKnowledgeBase.cpp src/PPDDLParser.cpp src/PPDDLUtils.cpp ) diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLDomainParser.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLDomainParser.h new file mode 100644 index 000000000..1b45456f4 --- /dev/null +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLDomainParser.h @@ -0,0 +1,11 @@ +#ifndef ROSPLAN_HDDL_DOMAIN_PARSER +#define ROSPLAN_HDDL_DOMAIN_PARSER + +#include "PDDLDomainParser.h" + +namespace KCL_rosplan { + class HDDLDomainParser : public PDDLDomainParser + { + }; +} +#endif diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLKnowledgeBase.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLKnowledgeBase.h new file mode 100644 index 000000000..9479115c7 --- /dev/null +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLKnowledgeBase.h @@ -0,0 +1,18 @@ +#ifndef ROSPLAN_HDDL_Knowledgebase +#define ROSPLAN_HDDL_Knowledgebase + +#include "PDDLKnowledgeBase.h" +#include "HDDLDomainParser.h" +#include "HDDLProblemParser.h" + +namespace KCL_rosplan { + + class HDDLKnowledgeBase : public PDDLKnowledgeBase { + public: + HDDLKnowledgeBase(ros::NodeHandle& n) : PDDLKnowledgeBase(n) {}; + ~HDDLKnowledgeBase() = default; + + virtual void addInitialState() override; + }; +} +#endif diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLProblemParser.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLProblemParser.h new file mode 100644 index 000000000..aba23db26 --- /dev/null +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/HDDLProblemParser.h @@ -0,0 +1,11 @@ +#ifndef ROSPLAN_HDDL_PROBLEM_PARSER +#define ROSPLAN_HDDL_PROBLEM_PARSER + +#include "PDDLProblemParser.h" + +namespace KCL_rosplan { + class HDDLProblemParser : public PDDLProblemParser + { + }; +} +#endif diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h index 6eff45919..11e87043e 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h @@ -22,6 +22,7 @@ #include "rosplan_knowledge_msgs/DomainFormula.h" #include "rosplan_knowledge_msgs/GetAttributeService.h" +#include "rosplan_knowledge_msgs/GetTaskService.h" #include "rosplan_knowledge_msgs/GetInstanceService.h" #include "rosplan_knowledge_msgs/GetMetricService.h" #include "rosplan_knowledge_msgs/KnowledgeItem.h" @@ -65,6 +66,7 @@ namespace KCL_rosplan { /* adding items to the knowledge base */ void addKnowledge(rosplan_knowledge_msgs::KnowledgeItem &msg); void addMissionGoal(rosplan_knowledge_msgs::KnowledgeItem &msg); + void addMissionTask(rosplan_knowledge_msgs::KnowledgeItem &msg); void addMissionMetric(rosplan_knowledge_msgs::KnowledgeItem &msg); /* removing items from the knowledge base */ @@ -72,6 +74,7 @@ namespace KCL_rosplan { virtual void removeFact(const rosplan_knowledge_msgs::KnowledgeItem &msg); void removeMissionGoal(rosplan_knowledge_msgs::KnowledgeItem &msg); + void removeMissionTask(rosplan_knowledge_msgs::KnowledgeItem &msg); void removeMissionMetric(rosplan_knowledge_msgs::KnowledgeItem &msg); /* PDDL model (persistent state) */ @@ -83,6 +86,7 @@ namespace KCL_rosplan { std::vector model_facts; std::vector model_functions; std::vector model_goals; + std::vector model_tasks; rosplan_knowledge_msgs::KnowledgeItem model_metric; /* timed initial literals */ @@ -119,6 +123,7 @@ namespace KCL_rosplan { bool getPropositions(rosplan_knowledge_msgs::GetAttributeService::Request &req, rosplan_knowledge_msgs::GetAttributeService::Response &res); bool getFunctions(rosplan_knowledge_msgs::GetAttributeService::Request &req, rosplan_knowledge_msgs::GetAttributeService::Response &res); bool getGoals(rosplan_knowledge_msgs::GetAttributeService::Request &req, rosplan_knowledge_msgs::GetAttributeService::Response &res); + bool getTasks(rosplan_knowledge_msgs::GetTaskService::Request &req, rosplan_knowledge_msgs::GetTaskService::Response &res); bool getMetric(rosplan_knowledge_msgs::GetMetricService::Request &req, rosplan_knowledge_msgs::GetMetricService::Response &res); bool getTimedKnowledge(rosplan_knowledge_msgs::GetAttributeService::Request &req, rosplan_knowledge_msgs::GetAttributeService::Response &res); diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBaseFactory.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBaseFactory.h index ae936cb25..7a6b7c012 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBaseFactory.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBaseFactory.h @@ -8,6 +8,7 @@ #include "rosplan_knowledge_base/PDDLKnowledgeBase.h" #include "rosplan_knowledge_base/RDDLKnowledgeBase.h" #include "rosplan_knowledge_base/PPDDLKnowledgeBase.h" +#include "rosplan_knowledge_base/HDDLKnowledgeBase.h" namespace KCL_rosplan { typedef std::unique_ptr KnowledgeBasePtr; @@ -17,7 +18,8 @@ namespace KCL_rosplan { enum KB { PDDL, PPDDL, - RDDL + RDDL, + HDDL }; static KnowledgeBasePtr createKB(KB kb_type, ros::NodeHandle& n); diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/PDDLKnowledgeBase.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/PDDLKnowledgeBase.h index c5e7fd0a4..91491fd08 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/PDDLKnowledgeBase.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/PDDLKnowledgeBase.h @@ -37,7 +37,7 @@ namespace KCL_rosplan { class PDDLKnowledgeBase : public KnowledgeBase { - private: + protected: /* parsing domain using VAL */ PDDLDomainParser domain_parser; @@ -48,20 +48,20 @@ namespace KCL_rosplan { ~PDDLKnowledgeBase() = default; /* parse domain and probelm files */ - void parseDomain(const std::string& domain_file_path, const std::string& problem_file_path) override; + virtual void parseDomain(const std::string& domain_file_path, const std::string& problem_file_path) override; /* add the initial state to the knowledge base */ - void addInitialState() override; - void addConstants() override; + virtual void addInitialState() override; + virtual void addConstants() override; /* service methods for fetching the domain details */ - bool getDomainName(rosplan_knowledge_msgs::GetDomainNameService::Request &req, rosplan_knowledge_msgs::GetDomainNameService::Response &res) override; - bool getTypes(rosplan_knowledge_msgs::GetDomainTypeService::Request &req, rosplan_knowledge_msgs::GetDomainTypeService::Response &res) override; - bool getPredicates(rosplan_knowledge_msgs::GetDomainAttributeService::Request &req, rosplan_knowledge_msgs::GetDomainAttributeService::Response &res) override; - bool getFunctionPredicates(rosplan_knowledge_msgs::GetDomainAttributeService::Request &req, rosplan_knowledge_msgs::GetDomainAttributeService::Response &res) override; - bool getOperators(rosplan_knowledge_msgs::GetDomainOperatorService::Request &req, rosplan_knowledge_msgs::GetDomainOperatorService::Response &res) override; - bool getOperatorDetails(rosplan_knowledge_msgs::GetDomainOperatorDetailsService::Request &req, rosplan_knowledge_msgs::GetDomainOperatorDetailsService::Response &res) override; - bool getPredicateDetails(rosplan_knowledge_msgs::GetDomainPredicateDetailsService::Request &req, rosplan_knowledge_msgs::GetDomainPredicateDetailsService::Response &res) override; + virtual bool getDomainName(rosplan_knowledge_msgs::GetDomainNameService::Request &req, rosplan_knowledge_msgs::GetDomainNameService::Response &res) override; + virtual bool getTypes(rosplan_knowledge_msgs::GetDomainTypeService::Request &req, rosplan_knowledge_msgs::GetDomainTypeService::Response &res) override; + virtual bool getPredicates(rosplan_knowledge_msgs::GetDomainAttributeService::Request &req, rosplan_knowledge_msgs::GetDomainAttributeService::Response &res) override; + virtual bool getFunctionPredicates(rosplan_knowledge_msgs::GetDomainAttributeService::Request &req, rosplan_knowledge_msgs::GetDomainAttributeService::Response &res) override; + virtual bool getOperators(rosplan_knowledge_msgs::GetDomainOperatorService::Request &req, rosplan_knowledge_msgs::GetDomainOperatorService::Response &res) override; + virtual bool getOperatorDetails(rosplan_knowledge_msgs::GetDomainOperatorDetailsService::Request &req, rosplan_knowledge_msgs::GetDomainOperatorDetailsService::Response &res) override; + virtual bool getPredicateDetails(rosplan_knowledge_msgs::GetDomainPredicateDetailsService::Request &req, rosplan_knowledge_msgs::GetDomainPredicateDetailsService::Response &res) override; }; } #endif diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/VALVisitorProblem.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/VALVisitorProblem.h index fcd2bc25e..e8231559d 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/VALVisitorProblem.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/VALVisitorProblem.h @@ -61,6 +61,7 @@ namespace KCL_rosplan { std::vector facts; std::vector functions; std::vector goals; + std::vector tasks; rosplan_knowledge_msgs::KnowledgeItem metric; /* timed initial literals */ @@ -71,6 +72,7 @@ namespace KCL_rosplan { std::vector returnFacts(); std::vector returnFunctions(); std::vector returnGoals(); + std::vector returnTasks(); rosplan_knowledge_msgs::KnowledgeItem returnMetric(); std::vector returnTimedKnowledge(); diff --git a/rosplan_knowledge_base/src/HDDLKnowledgeBase.cpp b/rosplan_knowledge_base/src/HDDLKnowledgeBase.cpp new file mode 100644 index 000000000..cd297a2ae --- /dev/null +++ b/rosplan_knowledge_base/src/HDDLKnowledgeBase.cpp @@ -0,0 +1,19 @@ + +#include "rosplan_knowledge_base/HDDLKnowledgeBase.h" + +namespace KCL_rosplan { + + /* get the initial state from the domain and problem files */ + void HDDLKnowledgeBase::addInitialState() { + VALVisitorProblem problem_visitor(domain_parser.domain, problem_parser.problem); + model_instances = problem_visitor.returnInstances(); + model_facts = problem_visitor.returnFacts(); + model_functions = problem_visitor.returnFunctions(); + model_goals = problem_visitor.returnGoals(); + model_tasks = problem_visitor.returnTasks(); + model_timed_initial_literals = problem_visitor.returnTimedKnowledge(); + if (problem_parser.problem->metric) { + model_metric = problem_visitor.returnMetric(); + } + } +} // close namespace diff --git a/rosplan_knowledge_base/src/KnowledgeBase.cpp b/rosplan_knowledge_base/src/KnowledgeBase.cpp index 2aa001877..bde130108 100644 --- a/rosplan_knowledge_base/src/KnowledgeBase.cpp +++ b/rosplan_knowledge_base/src/KnowledgeBase.cpp @@ -64,7 +64,7 @@ namespace KCL_rosplan { } } break; - + case rosplan_knowledge_msgs::KnowledgeItem::FUNCTION: { // check if function exists and has the correct value @@ -160,6 +160,10 @@ namespace KCL_rosplan { addMissionGoal(req.knowledge); break; + case rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_TASK: + addMissionTask(req.knowledge); + break; + case rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_KNOWLEDGE: removeKnowledge(req.knowledge); break; @@ -168,6 +172,10 @@ namespace KCL_rosplan { removeMissionGoal(req.knowledge); break; + case rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_TASK: + removeMissionTask(req.knowledge); + break; + case rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_METRIC: addMissionMetric(req.knowledge); break; @@ -218,6 +226,7 @@ namespace KCL_rosplan { model_facts.clear(); model_functions.clear(); model_goals.clear(); + model_tasks.clear(); model_metric = empty; } @@ -255,7 +264,7 @@ namespace KCL_rosplan { if(name.compare(msg.instance_name)==0 || msg.instance_name.compare("")==0) { // remove instance from knowledge base ROS_INFO("KCL: (%s) Removing instance (%s, %s)", - ros::this_node::getName().c_str(), + ros::this_node::getName().c_str(), msg.instance_type.c_str(), (msg.instance_name.compare("")==0) ? "ALL" : msg.instance_name.c_str()); iit = model_instances[msg.instance_type].erase(iit); @@ -340,6 +349,22 @@ namespace KCL_rosplan { } } + /** + * remove mission goal + */ + void KnowledgeBase::removeMissionTask(rosplan_knowledge_msgs::KnowledgeItem &msg) { + + std::vector::iterator git; + for(git=model_tasks.begin(); git!=model_tasks.end(); ) { + if(KnowledgeComparitor::containsKnowledge(msg, *git)) { + ROS_INFO("KCL: (%s) Removing task (%s)", ros::this_node::getName().c_str(), git->attribute_name.c_str()); + git = model_tasks.erase(git); + } else { + git++; + } + } + } + /** * remove mission metric */ @@ -357,7 +382,7 @@ namespace KCL_rosplan { * add an instance, fact, or function to the knowledge base */ void KnowledgeBase::addKnowledge(rosplan_knowledge_msgs::KnowledgeItem &msg) { - + switch(msg.knowledge_type) { case rosplan_knowledge_msgs::KnowledgeItem::INSTANCE: @@ -455,12 +480,37 @@ namespace KCL_rosplan { return; } } - + // add goal ROS_INFO("KCL: (%s) Adding mission goal (%s%s)", ros::this_node::getName().c_str(), msg.attribute_name.c_str(), param_str.c_str()); model_goals.push_back(msg); } + /* + * add mission task to knowledge base + */ + void KnowledgeBase::addMissionTask(rosplan_knowledge_msgs::KnowledgeItem &msg) { + + // create parameter string for ROS_INFO messages + std::string param_str; + for (size_t i = 0; i < msg.values.size(); ++i) { + param_str += " " + msg.values[i].value; + } + + // check to make sure goal is not already added + std::vector::iterator pit; + for(pit=model_goals.begin(); pit!=model_goals.end(); pit++) { + if(KnowledgeComparitor::containsKnowledge(msg, *pit)) { + ROS_WARN("KCL: (%s) Goal (%s%s) already posted", ros::this_node::getName().c_str(), msg.attribute_name.c_str(), param_str.c_str()); + return; + } + } + + // add goal + ROS_INFO("KCL: (%s) Adding mission task (%s%s)", ros::this_node::getName().c_str(), msg.attribute_name.c_str(), param_str.c_str()); + model_tasks.push_back(msg); + } + /* * add mission metric to knowledge base */ @@ -474,7 +524,7 @@ namespace KCL_rosplan { /*----------------*/ bool KnowledgeBase::getInstances(rosplan_knowledge_msgs::GetInstanceService::Request &req, rosplan_knowledge_msgs::GetInstanceService::Response &res) { - + // fetch the instances of the correct type if(""==req.type_name) { std::map >::iterator iit; @@ -552,6 +602,12 @@ namespace KCL_rosplan { return true; } + bool KnowledgeBase::getTasks(rosplan_knowledge_msgs::GetTaskService::Request &req, rosplan_knowledge_msgs::GetTaskService::Response &res) { + for(size_t i=0; i VALVisitorProblem::returnTasks() { + problem->the_task->visit(this); + return tasks; + } + rosplan_knowledge_msgs::KnowledgeItem VALVisitorProblem::returnMetric() { problem->metric->visit(this); return metric; @@ -251,7 +256,7 @@ namespace KCL_rosplan { item.is_negative = problem_cond_neg; rosplan_knowledge_msgs::DomainInequality ineq; - + switch( c->getOp() ) { case VAL1_2::E_GREATER: ineq.comparison_type = rosplan_knowledge_msgs::DomainInequality::GREATER; break; @@ -275,7 +280,7 @@ namespace KCL_rosplan { } void VALVisitorProblem::visit_timed_goal(VAL1_2::timed_goal *c){ - ROS_WARN("KCL: (%s) Timed goal not a part of PDDL problem parsing.", ros::this_node::getName().c_str()); + ROS_WARN("KCL: (%s) Timed goal not a part of PDDL problem parsing.", ros::this_node::getName().c_str()); } /*---------*/ diff --git a/rosplan_knowledge_base/src/VALfiles/ptree.cpp b/rosplan_knowledge_base/src/VALfiles/ptree.cpp index 973d1b9b7..33a6f6e16 100644 --- a/rosplan_knowledge_base/src/VALfiles/ptree.cpp +++ b/rosplan_knowledge_base/src/VALfiles/ptree.cpp @@ -35,7 +35,7 @@ July 2001. Strathclyde Planning Group - + ---------------------------------------------------------------------------- */ @@ -77,10 +77,10 @@ ostream & operator <<(ostream & o,const parse_category & p) /*----------------------------------------------------------------------------- ---------------------------------------------------------------------------*/ -void symbol::display(int ind) const -{ +void symbol::display(int ind) const +{ TITLE(symbol); - LEAF(name); + LEAF(name); } void symbol::write(ostream & o) const @@ -89,7 +89,7 @@ void symbol::write(ostream & o) const }; void pddl_typed_symbol::display(int ind) const -{ +{ TITLE(symbol); LEAF(name); cout << "[" << this << "]\n"; FIELD(type); @@ -119,7 +119,7 @@ void func_decl_list::write(ostream & o) const wcntr->write_func_decl_list(o,this); }; -void var_symbol::write(ostream & o) const +void var_symbol::write(ostream & o) const { wcntr->write_var_symbol(o,this); }; @@ -293,7 +293,7 @@ void constraint_goal::display(int ind) const LEAF(from); }; -void constraint_goal::write(ostream & o) const +void constraint_goal::write(ostream & o) const { wcntr->write_constraint_goal(o,this); }; @@ -313,13 +313,13 @@ void preference::write(ostream & o) const void qfied_goal::display(int ind) const { TITLE(qfied_goal); - + LABEL(qfier); if (qfier == E_FORALL) cout << "forall"; else if (qfier == E_EXISTS) cout << "exists"; - else + else cout << "?quantifier"; FIELD(vars); @@ -679,7 +679,7 @@ void domain::display(int ind) const // FIELD(types); // FIELD(constants); // FIELD(pred_vars); - FIELD(predicates); + FIELD(predicates); FIELD(ops); FIELD(drvs); } @@ -779,7 +779,7 @@ void requires(pddl_req_flag flags) if (!(flags & current_analysis->req)) current_analysis->error_list.add( E_WARNING, - "Undeclared requirement " + + "Undeclared requirement " + pddl_req_flags_string(flags)); } @@ -802,7 +802,7 @@ string pddl_req_flags_string(pddl_req_flag flags) if (flags & E_EQUALITY) result += ":equality "; if (flags & E_STRIPS) result += ":strips "; if (flags & E_TYPING) result += ":typing "; - if (flags & E_DISJUNCTIVE_PRECONDS) + if (flags & E_DISJUNCTIVE_PRECONDS) result += ":disjunctive-preconditions "; if (flags & E_EXT_PRECS) result += ":existential-preconditions "; if (flags & E_UNIV_PRECS) result += ":universal-preconditions "; @@ -831,7 +831,7 @@ var_symbol* var_symbol_table_stack::symbol_get(const string& name) var_symbol* sym= NULL; // Iterate through stack from top to bottom - // (may need to change direction if changing underlying + // (may need to change direction if changing underlying // impl. of stack) for (iterator i=begin(); i!=end() && sym==NULL; ++i) sym= (*i)->symbol_probe(name); @@ -841,7 +841,7 @@ var_symbol* var_symbol_table_stack::symbol_get(const string& name) return sym; else { - // Log a warning + // Log a warning // add new symbol to current table. log_error(E_WARNING,"Undeclared variable symbol: ?" + name); return top()->symbol_put(name); @@ -875,6 +875,7 @@ void special_val_expr::visit(VisitController *v) const {v->visit_special_val_exp void func_term::visit(VisitController *v) const {v->visit_func_term(this);}; void assignment::visit(VisitController *v) const {v->visit_assignment(this);}; void goal::visit(VisitController * v) const {}; +void task::visit(VisitController * v) const {}; void constraint_goal::visit(VisitController *v) const {v->visit_constraint_goal(this);}; void preference::visit(VisitController *v) const {v->visit_preference(this);}; void simple_goal::visit(VisitController *v) const {v->visit_simple_goal(this);}; @@ -907,4 +908,3 @@ void problem::visit(VisitController *v) const {v->visit_problem(this);}; void plan_step::visit(VisitController *v) const {v->visit_plan_step(this);}; void violation_term::visit(VisitController * v) const {v->visit_violation_term(this);}; }; - diff --git a/rosplan_knowledge_base/src/VALfiles/ptree.h b/rosplan_knowledge_base/src/VALfiles/ptree.h index 23c914916..d031f3950 100644 --- a/rosplan_knowledge_base/src/VALfiles/ptree.h +++ b/rosplan_knowledge_base/src/VALfiles/ptree.h @@ -61,7 +61,7 @@ /*----------------------------------------------------------------------------- - Forward declaration of classes, + Forward declaration of classes, (because in some cases we have mutually referring structures). ----------------------------------------------------------------------------*/ @@ -117,6 +117,9 @@ class parse_category; class timed_goal; class comparison; + class task_list; + class task; + class effect; class simple_effect; class forall_effect; @@ -171,10 +174,10 @@ typedef unsigned long pddl_req_flag; // When changing these, also look at the function pddl_req_attribute_name() -enum pddl_req_attr { E_EQUALITY = 1, - E_STRIPS = 2, +enum pddl_req_attr { E_EQUALITY = 1, + E_STRIPS = 2, E_TYPING = 4, - E_DISJUNCTIVE_PRECONDS = 8, + E_DISJUNCTIVE_PRECONDS = 8, E_EXT_PRECS = 16, E_UNIV_PRECS = 32, E_COND_EFFS = 64, @@ -190,8 +193,8 @@ enum pddl_req_attr { E_EQUALITY = 1, E_CONSTRAINTS = 65536, E_OFLUENTS = 131072, E_ACTIONCOSTS = 262144 - -// Attributes which are defined as combinations of others + +// Attributes which are defined as combinations of others // are expanded by parser, and don't need to be included here. }; @@ -300,34 +303,34 @@ struct SpecialistSymbolFactory : public SymbolFactory { T * build(const string & name) {return new U(name);}; }; -template +template class symbol_table : public map { private: typedef map _Base; auto_ptr > factory; - + public : symbol_table() : factory(new SymbolFactory()) {}; - void setFactory(SymbolFactory * sf) + void setFactory(SymbolFactory * sf) { auto_ptr > x(sf); factory = x; }; template - void replaceFactory() + void replaceFactory() { auto_ptr > x(new SpecialistSymbolFactory()); factory = x; }; - + typedef typename _Base::iterator iterator; typedef typename _Base::const_iterator const_iterator; - + // symbol_ref(string) // Don't care whether symbol is already present symbol_class* symbol_ref(const string& name) @@ -449,7 +452,7 @@ typedef symbol_table operator_symbol_table; /*----------------------------------------------------------------------------- - Lists of symbols + Lists of symbols ---------------------------------------------------------------------------*/ // No destructor for symbol lists @@ -463,7 +466,7 @@ class typed_symbol_list : public list, public parse_category public: typedef typename _Base::iterator iterator; typedef typename _Base::const_iterator const_iterator; - + void set_types(pddl_type* t) { //for (typed_symbol_list::iterator i= begin(); i!=end(); ++i) @@ -491,7 +494,7 @@ class typed_symbol_list : public list, public parse_category //for (typed_symbol_list::iterator i= begin(); i!=end(); ++i) iterator i= _Base::begin(); if(i == _Base::end()) - { + { return; }; (*i)->either_types = tl; @@ -525,7 +528,7 @@ class typed_symbol_list : public list, public parse_category { (*i)->visit(v); }; - }; + }; virtual ~typed_symbol_list() {}; }; @@ -542,7 +545,7 @@ typedef typed_symbol_list pddl_type_list; /*---------------------------------------------------------------------------- Symbols used for constants, variables, types, and predicate names. - Generally, a pointer to a symbol will be used as a unique identifier. + Generally, a pointer to a symbol will be used as a unique identifier. --------------------------------------------------------------------------*/ class symbol : public parse_category @@ -557,7 +560,7 @@ class symbol : public parse_category virtual void display(int ind) const; virtual void write(ostream & o) const; virtual void visit(VisitController * v) const; - + const string getName() const {return name;}; }; @@ -586,8 +589,8 @@ class pddl_typed_symbol : public symbol pddl_typed_symbol() : symbol(""), type(NULL), either_types(NULL) {}; pddl_typed_symbol(const string& s) : symbol(s), type(NULL), either_types(NULL) {}; - - virtual ~pddl_typed_symbol() + + virtual ~pddl_typed_symbol() { delete either_types; }; @@ -597,7 +600,7 @@ class pddl_typed_symbol : public symbol }; // Parameters can be variables or constant symbols -class parameter_symbol : public pddl_typed_symbol +class parameter_symbol : public pddl_typed_symbol { public: parameter_symbol(const string& s) : pddl_typed_symbol(s) {}; @@ -605,7 +608,7 @@ class parameter_symbol : public pddl_typed_symbol }; -class var_symbol : public parameter_symbol +class var_symbol : public parameter_symbol { public: var_symbol(const string& s) : parameter_symbol(s) {}; @@ -616,7 +619,7 @@ class var_symbol : public parameter_symbol }; -class const_symbol : public parameter_symbol +class const_symbol : public parameter_symbol { public: const_symbol(const string& s) : parameter_symbol(s) {}; @@ -697,9 +700,9 @@ class pred_decl : public parse_category var_symbol_table* var_tab; public: - pred_decl(pred_symbol* h, -// typed_symbol_list* a, - var_symbol_list* a, + pred_decl(pred_symbol* h, +// typed_symbol_list* a, + var_symbol_list* a, var_symbol_table* vt) : head(h), args(a), var_tab(vt) {}; @@ -715,7 +718,7 @@ class pred_decl : public parse_category (*i)->either_types = (*j)->either_types; }; }; - + virtual ~pred_decl() { delete args; delete var_tab; }; virtual void display(int ind) const; virtual void write(ostream & o) const; @@ -733,8 +736,8 @@ class func_decl : public parse_category public: - func_decl(func_symbol* h, -// typed_symbol_list* a, + func_decl(func_symbol* h, +// typed_symbol_list* a, var_symbol_list* a, var_symbol_table* vt) : head(h), args(a), var_tab(vt) {}; @@ -745,7 +748,7 @@ class func_decl : public parse_category virtual ~func_decl() { delete args; delete var_tab; }; virtual void display(int ind) const; virtual void write(ostream & o) const; - virtual void visit(VisitController * v) const; + virtual void visit(VisitController * v) const; }; @@ -756,7 +759,7 @@ class pred_decl_list : public pc_list virtual void write(ostream & o) const; }; -class func_decl_list : public pc_list +class func_decl_list : public pc_list { public: virtual ~func_decl_list() {}; @@ -772,7 +775,7 @@ class expression : public parse_category }; class binary_expression : public expression { -protected: +protected: expression * arg1; expression * arg2; public: @@ -791,9 +794,9 @@ class binary_expression : public expression { class plus_expression : public binary_expression { public: - plus_expression(expression *a1, expression *a2) : + plus_expression(expression *a1, expression *a2) : binary_expression(a1,a2) {}; - + virtual void display(int ind) const; virtual void write(ostream & o) const; virtual void visit(VisitController * v) const; @@ -802,20 +805,20 @@ class plus_expression : public binary_expression class minus_expression : public binary_expression { public: - minus_expression(expression *a1, expression *a2) : + minus_expression(expression *a1, expression *a2) : binary_expression(a1,a2) {}; - + virtual void display(int ind) const; virtual void write(ostream & o) const; virtual void visit(VisitController * v) const; -}; +}; class mul_expression : public binary_expression { public: - mul_expression(expression *a1, expression *a2) : + mul_expression(expression *a1, expression *a2) : binary_expression(a1,a2) {}; - + virtual void display(int ind) const; virtual void write(ostream & o) const; virtual void visit(VisitController * v) const; @@ -824,9 +827,9 @@ class mul_expression : public binary_expression class div_expression : public binary_expression { public: - div_expression(expression *a1, expression *a2) : + div_expression(expression *a1, expression *a2) : binary_expression(a1,a2) {}; - + virtual void display(int ind) const; virtual void write(ostream & o) const; virtual void visit(VisitController * v) const; @@ -837,14 +840,14 @@ class uminus_expression : public expression private: expression *arg1; public: - uminus_expression(expression *a1) : + uminus_expression(expression *a1) : arg1(a1) {}; virtual ~uminus_expression() { delete arg1;}; virtual void display(int ind) const; virtual void write(ostream & o) const; virtual void visit(VisitController * v) const; - + const expression * getExpr() const {return arg1;}; }; @@ -866,7 +869,7 @@ class int_expression : public num_expression virtual void display(int ind) const; virtual void write(ostream & o) const; virtual void visit(VisitController * v) const; - + const NumScalar double_value() const {return static_cast(val);}; }; @@ -889,13 +892,13 @@ class func_term : public expression func_symbol *func_sym; parameter_symbol_list *param_list; public: - func_term(func_symbol *fs, parameter_symbol_list *psl) : + func_term(func_symbol *fs, parameter_symbol_list *psl) : func_sym(fs), param_list(psl) {}; virtual ~func_term() {delete param_list;}; virtual void display(int ind) const; virtual void write(ostream & o) const; virtual void visit(VisitController * v) const; - + const func_symbol * getFunction() const {return func_sym;}; const parameter_symbol_list * getArgs() const {return param_list;}; }; @@ -939,7 +942,7 @@ class violation_term : public expression class goal_list: public pc_list { public: - virtual ~goal_list() {}; + virtual ~goal_list() {}; virtual void display(int ind) const; virtual void write(ostream & o) const; }; @@ -951,9 +954,20 @@ class goal : public parse_category { virtual void visit(VisitController * v) const; }; +class task_list: public pc_list +{ +public: + virtual ~task_list() {}; +}; + +class task : public parse_category { +public: + virtual void visit(VisitController * v) const; +}; + class con_goal : public goal {}; -class constraint_goal : public con_goal +class constraint_goal : public con_goal { private: constraint_sort cons; @@ -982,7 +996,7 @@ class constraint_goal : public con_goal double getFrom() const {return from;}; }; -class preference : public con_goal +class preference : public con_goal { private: string name; @@ -1028,7 +1042,7 @@ class qfied_goal : public con_goal qfier(q), vars(vl), sym_tab(s), - gl(g) + gl(g) {}; virtual ~qfied_goal() { delete vars; delete sym_tab; delete gl; }; const quantifier getQuantifier() const {return qfier;}; @@ -1054,7 +1068,7 @@ class conj_goal : public con_goal virtual void visit(VisitController * v) const; }; -class disj_goal : public goal +class disj_goal : public goal { private: goal_list* goals; @@ -1111,7 +1125,7 @@ class timed_goal : public goal public: timed_goal (goal* g, time_spec t) : gl(g), ts(t) {}; virtual ~timed_goal() { delete gl; }; - goal * clearGoal() + goal * clearGoal() { goal * gl1 = gl; gl = 0; @@ -1130,11 +1144,11 @@ class comparison : public goal, public binary_expression // proposition? // No, comparison_op op; public: - comparison(comparison_op c_op, expression* e1, expression* e2) : + comparison(comparison_op c_op, expression* e1, expression* e2) : binary_expression(e1,e2), op(c_op) {}; virtual void display(int ind) const; virtual void write(ostream & o) const; - virtual void visit(VisitController * v) const; + virtual void visit(VisitController * v) const; const comparison_op getOp() const {return op;}; }; @@ -1169,7 +1183,7 @@ class effect_lists : public parse_category effect classes ---------------------------------------------------------------------------*/ -class effect : public parse_category +class effect : public parse_category { public: effect() {}; @@ -1179,7 +1193,7 @@ class effect : public parse_category }; -class simple_effect : public effect +class simple_effect : public effect { public: proposition* prop; @@ -1200,8 +1214,8 @@ class forall_effect : public effect var_symbol_table* var_tab; public: - forall_effect(effect_lists* eff, var_symbol_list* vs,var_symbol_table* vt) : - effect(), operand(eff), vars(vs), var_tab(vt) + forall_effect(effect_lists* eff, var_symbol_list* vs,var_symbol_table* vt) : + effect(), operand(eff), vars(vs), var_tab(vt) {}; virtual ~forall_effect() @@ -1229,11 +1243,11 @@ class cond_effect : public effect public: // Construct from a list - cond_effect(goal* g, effect_lists* e) : - effect(), + cond_effect(goal* g, effect_lists* e) : + effect(), cond(g), effects(e) - + {}; virtual ~cond_effect() @@ -1251,7 +1265,7 @@ class cond_effect : public effect }; -class timed_effect : public effect +class timed_effect : public effect { public: time_spec ts; @@ -1266,7 +1280,7 @@ class timed_effect : public effect class timed_initial_literal : public timed_effect { -public: +public: long double time_stamp; ~timed_initial_literal() {effs = 0;}; //effs->add_effects.clear();effs->del_effects.clear();effs->assign_effects.clear();effs->timed_effects.clear();}; @@ -1280,10 +1294,10 @@ class assignment : public effect { private: func_term *f_term; // Thing to which value is assigned. - assign_op op; // Assignment operator, e.g. + assign_op op; // Assignment operator, e.g. expression *expr; // Value that gets assigned public: - assignment(func_term *ft, assign_op a_op, expression *e) : + assignment(func_term *ft, assign_op a_op, expression *e) : f_term(ft), op(a_op), expr(e) {}; virtual ~assignment() { delete f_term; delete expr; }; virtual void display(int ind) const; @@ -1300,7 +1314,7 @@ class assignment : public effect * Structures * --------------------------------------------------------------------------*/ -class structure_def : public parse_category +class structure_def : public parse_category { public: virtual ~structure_def() {}; @@ -1316,7 +1330,7 @@ class structure_def : public parse_category Operators --------------------------------------------------------------------------*/ -class operator_list: public pc_list +class operator_list: public pc_list { public: virtual void display(int ind) const; @@ -1344,9 +1358,9 @@ class operator_ : public structure_def parameters(ps), precondition(pre), effects(effs) - + {}; - virtual ~operator_() + virtual ~operator_() { delete parameters; delete precondition; @@ -1372,7 +1386,7 @@ class operator_ : public structure_def * Structure store *-------------------------------------------------------------------------*/ -class derivations_list : public pc_list +class derivations_list : public pc_list { public: virtual void display(int ind) const; @@ -1389,15 +1403,15 @@ pc_list::~pc_list() class structure_store : public parse_category { -private: +private: operator_list * ops; derivations_list * dvs; public: structure_store() : ops(new operator_list), dvs(new derivations_list) {}; - void push_back(structure_def * s) + void push_back(structure_def * s) { - if(s) + if(s) { s->add_to(ops,dvs); } @@ -1424,10 +1438,10 @@ class derivation_rule : public structure_def proposition * get_head() const {return head;}; goal * get_body() const {return body;}; void set_body(goal * g) {body = g; body_changed = true; }; - + virtual void write(ostream & o) const; virtual void visit(VisitController * v) const; - virtual ~derivation_rule() + virtual ~derivation_rule() { delete head; if(!body_changed) delete body; @@ -1442,7 +1456,7 @@ class derivation_rule : public structure_def /*---------------------------------------------------------------------------- Classes derived from operator: action - event + event process durative_action --------------------------------------------------------------------------*/ @@ -1501,7 +1515,7 @@ class durative_action : public operator_ public: goal* dur_constraint; durative_action() {}; - virtual ~durative_action() + virtual ~durative_action() { delete dur_constraint; }; @@ -1533,7 +1547,7 @@ class domain : public parse_category func_decl_list* functions; con_goal * constraints; - domain( structure_store * ss) : + domain( structure_store * ss) : ops(ss->get_operators()), drvs(ss->get_derivations()), req(0), @@ -1547,7 +1561,7 @@ class domain : public parse_category delete ss; }; - virtual ~domain() + virtual ~domain() { delete drvs; delete ops; @@ -1580,7 +1594,7 @@ class plan_step : public parse_category public: operator_symbol* op_sym; const_symbol_list* params; - + bool start_time_given; bool duration_given; double start_time; @@ -1603,7 +1617,7 @@ class plan_step : public parse_category }; -class plan : public pc_list +class plan : public pc_list {private: double timeTaken; public: @@ -1654,7 +1668,7 @@ class length_spec : public parse_category class problem : public parse_category { - + public: char * name; char * domain_name; @@ -1663,6 +1677,7 @@ class problem : public parse_category const_symbol_list* objects; effect_lists* initial_state; goal* the_goal; + task* the_task; con_goal *constraints; metric_spec* metric; length_spec* length; @@ -1675,12 +1690,13 @@ class problem : public parse_category objects(NULL), initial_state(NULL), the_goal(NULL), + the_task(NULL), constraints(NULL), metric(NULL), - length(NULL) + length(NULL) {}; - - virtual ~problem() + + virtual ~problem() { delete [] name; delete [] domain_name; @@ -1688,6 +1704,7 @@ class problem : public parse_category delete objects; delete initial_state; delete the_goal; + delete the_task; delete constraints; delete metric; delete length; @@ -1725,7 +1742,7 @@ class var_symbol_table_stack : public sStack Analysis. Here we store various symbol tables for constants, types, and predicates. For variables, we have a stack of symbol tables which is used during - parsing. + parsing. Operators and quantified constructs have their own local scope, and their own symbol tables. *---------------------------------------------------------------------------*/ @@ -1768,7 +1785,7 @@ class analysis private: auto_ptr varTabFactory; auto_ptr strucFactory; - + public: var_symbol_table * buildPredTab() {return varTabFactory->buildPredTab();}; var_symbol_table * buildFuncTab() {return varTabFactory->buildFuncTab();}; @@ -1794,7 +1811,7 @@ class analysis effect_lists* effs, var_symbol_table* st) {return strucFactory->buildProcess(nm,ps,pre,effs,st);}; - void setFactory(VarTabFactory * vf) + void setFactory(VarTabFactory * vf) { auto_ptr x(vf); varTabFactory = x; @@ -1813,9 +1830,9 @@ class analysis func_symbol_table func_tab; operator_symbol_table op_tab; pddl_req_flag req; - + parse_error_list error_list; - + domain* the_domain; problem* the_problem; @@ -1829,7 +1846,7 @@ class analysis var_tab_stack.push(new var_symbol_table); } - virtual ~analysis() + virtual ~analysis() { delete the_domain; delete the_problem; @@ -1839,4 +1856,3 @@ class analysis }; #endif /* PTREE_H_1_2 */ - From 90b3c45ea1b4a51a29823a895290c30af2638671 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 1 Feb 2020 22:01:36 +0100 Subject: [PATCH 4/5] [knowledge_msgs/KnowledgeItem] Added a TASK constant --- rosplan_knowledge_msgs/msg/KnowledgeItem.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/rosplan_knowledge_msgs/msg/KnowledgeItem.msg b/rosplan_knowledge_msgs/msg/KnowledgeItem.msg index ed11b99b2..bd81476e0 100644 --- a/rosplan_knowledge_msgs/msg/KnowledgeItem.msg +++ b/rosplan_knowledge_msgs/msg/KnowledgeItem.msg @@ -5,6 +5,7 @@ uint8 FACT = 1 uint8 FUNCTION = 2 uint8 EXPRESSION = 3 uint8 INEQUALITY = 4 +uint8 TASK= 4 uint8 knowledge_type From 376d04a290d4ce3f3d4401f0f46fad1f27d15ed0 Mon Sep 17 00:00:00 2001 From: Alex Mitrevski Date: Sat, 1 Feb 2020 22:29:57 +0100 Subject: [PATCH 5/5] [planning_system] Added a PANDA interface and problem generator --- rosplan_planning_system/CMakeLists.txt | 5 + .../PlannerInterface/PANDAPlannerInterface.h | 36 +++++ .../ProblemGeneration/HDDLProblemGenerator.h | 17 +++ .../ProblemGeneration/PDDLProblemGenerator.h | 14 +- .../ProblemGeneration/ProblemGenerator.h | 2 + .../ProblemGeneratorFactory.h | 4 +- .../PANDAPlannerInterface.cpp | 139 ++++++++++++++++++ .../HDDLProblemGenerator.cpp | 39 +++++ .../ProblemGeneration/ProblemGenerator.cpp | 4 + .../ProblemGeneratorFactory.cpp | 3 +- .../ProblemGeneration/ProblemInterface.cpp | 2 + 11 files changed, 256 insertions(+), 9 deletions(-) create mode 100644 rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h create mode 100644 rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.h create mode 100644 rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp create mode 100644 rosplan_planning_system/src/ProblemGeneration/HDDLProblemGenerator.cpp diff --git a/rosplan_planning_system/CMakeLists.txt b/rosplan_planning_system/CMakeLists.txt index 884305b2e..9f603b35a 100644 --- a/rosplan_planning_system/CMakeLists.txt +++ b/rosplan_planning_system/CMakeLists.txt @@ -48,9 +48,11 @@ add_definitions(-Wno-deprecated-declarations) add_executable(problemInterface src/ProblemGeneration/ProblemInterface.cpp src/ProblemGeneration/PDDLProblemGenerator.cpp src/ProblemGeneration/ProblemGenerator.cpp src/ProblemGeneration/RDDLProblemGenerator.cpp + src/ProblemGeneration/HDDLProblemGenerator.cpp src/ProblemGeneration/ProblemGeneratorFactory.cpp) add_executable(popf_planner_interface src/PlannerInterface/POPFPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) add_executable(lama_planner_interface src/PlannerInterface/LAMAPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) +add_executable(panda_planner_interface src/PlannerInterface/PANDAPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) add_executable(ff_planner_interface src/PlannerInterface/FFPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) add_executable(rddlsim_planner_interface src/PlannerInterface/RDDLSIMPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) add_executable(metricff_planner_interface src/PlannerInterface/FFPlannerInterface.cpp src/PlannerInterface/PlannerInterface.cpp) @@ -73,6 +75,7 @@ add_executable(simulatedAction src/ActionInterface/RPSimulatedActionInterface.cp add_dependencies(problemInterface ${catkin_EXPORTED_TARGETS}) add_dependencies(popf_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(lama_planner_interface ${catkin_EXPORTED_TARGETS}) +add_dependencies(panda_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(ff_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(rddlsim_planner_interface ${catkin_EXPORTED_TARGETS}) add_dependencies(metricff_planner_interface ${catkin_EXPORTED_TARGETS}) @@ -95,6 +98,7 @@ add_dependencies(simulatedAction ${catkin_EXPORTED_TARGETS}) target_link_libraries(problemInterface ${catkin_LIBRARIES}) target_link_libraries(popf_planner_interface ${catkin_LIBRARIES}) target_link_libraries(lama_planner_interface ${catkin_LIBRARIES}) +target_link_libraries(panda_planner_interface ${catkin_LIBRARIES}) target_link_libraries(ff_planner_interface ${catkin_LIBRARIES}) target_link_libraries(rddlsim_planner_interface ${catkin_LIBRARIES}) target_link_libraries(metricff_planner_interface ${catkin_LIBRARIES}) @@ -146,6 +150,7 @@ install(TARGETS problemInterface popf_planner_interface lama_planner_interface + panda_planner_interface ff_planner_interface metricff_planner_interface smt_planner_interface diff --git a/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h new file mode 100644 index 000000000..d49a9db0f --- /dev/null +++ b/rosplan_planning_system/include/rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h @@ -0,0 +1,36 @@ +#include "PlannerInterface.h" +#include +#include +#include +#include +#include +#include + +#ifndef KCL_PANDA_planner_interface +#define KCL_PANDA_planner_interface + +/** + * This file contains an interface to the planner. + */ +namespace KCL_rosplan { + + class PANDAPlannerInterface: public PlannerInterface + { + private: + + /* runs external commands */ + std::string runCommand(std::string cmd); + + protected: + + bool runPlanner(); + + public: + + PANDAPlannerInterface(ros::NodeHandle& nh); + virtual ~PANDAPlannerInterface(); + }; + +} // close namespace + +#endif diff --git a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.h new file mode 100644 index 000000000..34ba6abe4 --- /dev/null +++ b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.h @@ -0,0 +1,17 @@ +#ifndef ROSPLAN_HDDL_PROBLEM_GENERATOR_H +#define ROSPLAN_HDDL_PROBLEM_GENERATOR_H + +#include "PDDLProblemGenerator.h" + +namespace KCL_rosplan { + + class HDDLProblemGenerator : public PDDLProblemGenerator { + protected: + virtual void makeProblem(std::ofstream &pFile); + void makeTasks(std::ofstream &pFile); + public: + HDDLProblemGenerator(const std::string& kb): PDDLProblemGenerator(kb) {}; + }; +} // close namespace + +#endif diff --git a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/PDDLProblemGenerator.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/PDDLProblemGenerator.h index 49cb614f5..3e600a8cc 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/PDDLProblemGenerator.h +++ b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/PDDLProblemGenerator.h @@ -11,14 +11,14 @@ namespace KCL_rosplan { class PDDLProblemGenerator : public ProblemGenerator { - private: - void makeHeader(std::ofstream &pFile); - void makeInitialState(std::ofstream &pFile); - void makeGoals(std::ofstream &pFile); - void makeMetric(std::ofstream &pFile); - void printExpression(std::ofstream &pFile, rosplan_knowledge_msgs::ExprComposite &e); + protected: + virtual void makeHeader(std::ofstream &pFile); + virtual void makeInitialState(std::ofstream &pFile); + virtual void makeGoals(std::ofstream &pFile); + virtual void makeMetric(std::ofstream &pFile); + virtual void printExpression(std::ofstream &pFile, rosplan_knowledge_msgs::ExprComposite &e); - void makeProblem(std::ofstream &pFile); + virtual void makeProblem(std::ofstream &pFile); public: PDDLProblemGenerator(const std::string& kb): ProblemGenerator(kb) {}; }; diff --git a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGenerator.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGenerator.h index 83895abaf..eb3d0e99f 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGenerator.h +++ b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGenerator.h @@ -16,6 +16,7 @@ #include "rosplan_knowledge_msgs/GetDomainAttributeService.h" #include "rosplan_knowledge_msgs/GetInstanceService.h" #include "rosplan_knowledge_msgs/GetAttributeService.h" +#include "rosplan_knowledge_msgs/GetTaskService.h" #include "rosplan_knowledge_msgs/GetMetricService.h" #ifndef KCL_ProblemGenerator @@ -39,6 +40,7 @@ namespace KCL_rosplan { std::string state_function_service; std::string state_timed_knowledge_service; std::string state_goal_service; + std::string task_service; std::string state_metric_service; virtual void makeProblem(std::ofstream &pFile) =0; diff --git a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGeneratorFactory.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGeneratorFactory.h index fc86193b1..a870138f2 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGeneratorFactory.h +++ b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/ProblemGeneratorFactory.h @@ -8,6 +8,7 @@ #include "ProblemGenerator.h" #include "PDDLProblemGenerator.h" #include "RDDLProblemGenerator.h" +#include "HDDLProblemGenerator.h" namespace KCL_rosplan { typedef std::unique_ptr ProblemGeneratorPtr; @@ -16,7 +17,8 @@ namespace KCL_rosplan { public: enum ProbGen { PDDL, - RDDL + RDDL, + HDDL }; static ProblemGeneratorPtr createProblemGenerator(ProbGen pg_type, const std::string& kb); diff --git a/rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp new file mode 100644 index 000000000..a2f3f32a7 --- /dev/null +++ b/rosplan_planning_system/src/PlannerInterface/PANDAPlannerInterface.cpp @@ -0,0 +1,139 @@ +#include "rosplan_planning_system/PlannerInterface/PANDAPlannerInterface.h" + +namespace KCL_rosplan { + + /*-------------*/ + /* constructor */ + /*-------------*/ + + PANDAPlannerInterface::PANDAPlannerInterface(ros::NodeHandle& nh) + { + node_handle = &nh; + + plan_server = new actionlib::SimpleActionServer((*node_handle), "start_planning", boost::bind(&PlannerInterface::runPlanningServerAction, this, _1), false); + + // publishing raw planner output + std::string plannerTopic = "planner_output"; + node_handle->getParam("planner_topic", plannerTopic); + plan_publisher = node_handle->advertise(plannerTopic, 1, true); + + // start planning action server + plan_server->start(); + } + + PANDAPlannerInterface::~PANDAPlannerInterface() + { + delete plan_server; + } + + /** + * Runs external commands + */ + std::string PANDAPlannerInterface::runCommand(std::string cmd) { + std::string data; + FILE *stream; + char buffer[1000]; + stream = popen(cmd.c_str(), "r"); + while ( fgets(buffer, 1000, stream) != NULL ) + data.append(buffer); + pclose(stream); + return data; + } + + /*------------------*/ + /* Plan and process */ + /*------------------*/ + + /** + * passes the problem to the Planner; the plan to post-processing. + */ + bool PANDAPlannerInterface::runPlanner() { + + // save problem to file for PANDA + if(use_problem_topic && problem_instance_received) { + ROS_INFO("KCL: (%s) (%s) Writing problem to file.", ros::this_node::getName().c_str(), problem_name.c_str()); + std::ofstream dest; + dest.open((problem_path).c_str()); + dest << problem_instance; + dest.close(); + } + + // prepare the planner command line + std::string str = planner_command; + std::size_t dit = str.find("DOMAIN"); + if(dit!=std::string::npos) str.replace(dit,6,domain_path); + std::size_t pit = str.find("PROBLEM"); + if(pit!=std::string::npos) str.replace(pit,7,problem_path); + std::string commandString = str + " > " + data_path + "plan.hddl"; + + // call the planer + ROS_INFO("KCL: (%s) (%s) Running: %s", ros::this_node::getName().c_str(), problem_name.c_str(), commandString.c_str()); + std::string plan = runCommand(commandString.c_str()); + ROS_INFO("KCL: (%s) (%s) Planning complete", ros::this_node::getName().c_str(), problem_name.c_str()); + + // check the planner solved the problem + std::ifstream planfile; + planfile.open((data_path + "plan.hddl").c_str()); + std::string line; + std::stringstream ss; + + int curr, next; + bool solved = false; + double planDuration; + + while (std::getline(planfile, line)) { + if (line.find("SOLUTION SEQUENCE", 0) != std::string::npos) { + solved = true; + } else { + // if a solution has been found, we read the plan + if (solved) + { + // read a plan (might not be the last plan) + planDuration = 0; + ss.str(""); + while (std::getline(planfile, line)) { + if (line.length()<2) + break; + ss << line << std::endl; + } + planner_output = ss.str(); + } + } + } + planfile.close(); + + if(!solved) ROS_INFO("KCL: (%s) (%s) Plan was unsolvable.", ros::this_node::getName().c_str(), problem_name.c_str()); + else ROS_INFO("KCL: (%s) (%s) Plan was solved.", ros::this_node::getName().c_str(), problem_name.c_str()); + + return solved; + } + +} // close namespace + + /*-------------*/ + /* Main method */ + /*-------------*/ + + int main(int argc, char **argv) { + + srand (static_cast (time(0))); + + ros::init(argc,argv,"rosplan_planner_interface"); + ros::NodeHandle nh("~"); + + KCL_rosplan::PANDAPlannerInterface pi(nh); + + // subscribe to problem instance + std::string problemTopic = "problem_instance"; + nh.getParam("problem_topic", problemTopic); + ros::Subscriber problem_sub = nh.subscribe(problemTopic, 1, &KCL_rosplan::PlannerInterface::problemCallback, dynamic_cast(&pi)); + + // start the planning services + ros::ServiceServer service1 = nh.advertiseService("planning_server", &KCL_rosplan::PlannerInterface::runPlanningServerDefault, dynamic_cast(&pi)); + ros::ServiceServer service2 = nh.advertiseService("planning_server_params", &KCL_rosplan::PlannerInterface::runPlanningServerParams, dynamic_cast(&pi)); + + ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str()); + ros::spin(); + + return 0; + } diff --git a/rosplan_planning_system/src/ProblemGeneration/HDDLProblemGenerator.cpp b/rosplan_planning_system/src/ProblemGeneration/HDDLProblemGenerator.cpp new file mode 100644 index 000000000..64b23de97 --- /dev/null +++ b/rosplan_planning_system/src/ProblemGeneration/HDDLProblemGenerator.cpp @@ -0,0 +1,39 @@ +#include "rosplan_planning_system/ProblemGeneration/HDDLProblemGenerator.h" + +namespace KCL_rosplan { + void HDDLProblemGenerator::makeTasks(std::ofstream &pFile) { + ros::NodeHandle nh; + ros::ServiceClient getCurrentTasksClient = nh.serviceClient(task_service); + + pFile << "(:htn\n :tasks (and" << std::endl; + + // get current tasks + rosplan_knowledge_msgs::GetTaskService currentTaskSrv; + if (!getCurrentTasksClient.call(currentTaskSrv)) { + ROS_ERROR("KCL: (HDDLProblemGenerator) Failed to call service %s", task_service.c_str()); + } else { + for(size_t i=0;i 5) ? problem_path.substr(problem_path.find_last_of('.')) : ""; if (extension == ".pddl" or extension == ".ppddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::PDDL; else if (extension == ".rddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::RDDL; + else if (extension == ".hddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::HDDL; else { ROS_ERROR("KCL: (%s) Unexpected problem file extension %s. Please provide a problem file written in PDDL (.pddl extension) or RDDL (.rddl extension).", ros::this_node::getName().c_str(), extension.c_str());