From 1616222b4b9546a00f75be6839c55d32ae9e51d0 Mon Sep 17 00:00:00 2001 From: Dorian Date: Sun, 10 Nov 2019 13:09:08 +0000 Subject: [PATCH 1/5] modified interface to parse the best plan if multiple plans found --- .../PlannerInterface/POPFPlannerInterface.cpp | 54 ++++++++++++------- 1 file changed, 34 insertions(+), 20 deletions(-) diff --git a/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp index c8ad1ce2f..80047ccdc 100644 --- a/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp @@ -20,7 +20,7 @@ namespace KCL_rosplan { // start planning action server plan_server->start(); } - + POPFPlannerInterface::~POPFPlannerInterface() { delete plan_server; @@ -79,26 +79,40 @@ namespace KCL_rosplan { int curr, next; bool solved = false; + bool last = false; double planDuration; - while (std::getline(planfile, line)) { - - if (line.find("; Plan found", 0) != std::string::npos || line.find(";;;; Solution Found", 0) != std::string::npos) { - solved = true; - } else if (line.find("; Time", 0) == std::string::npos) { - // consume useless lines - } else { - // 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(); - } - } + int line_count = 0; + int best_plan_start_line = 0; + while (std::getline(planfile, line)) { + line_count++; + + if (line.find("; Plan found", 0) != std::string::npos || line.find(";;;; Solution Found", 0) != std::string::npos) { + solved = true; + best_plan_start_line = line_count; + + } + } + line_count = 0; + while (std::getline(planfile, line)) { + line_count++; + if (line_count >= best_plan_start_line){ + if (line.find("; Time", 0) == std::string::npos) { + // consume useless lines + } + else { + // read best 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()); @@ -121,7 +135,7 @@ namespace KCL_rosplan { ros::NodeHandle nh("~"); KCL_rosplan::POPFPlannerInterface pi(nh); - + // subscribe to problem instance std::string problemTopic = "problem_instance"; nh.getParam("problem_topic", problemTopic); From 5e801a19ffafa96dd854dad925187009eb30d4d2 Mon Sep 17 00:00:00 2001 From: Dorian Date: Sun, 10 Nov 2019 16:31:27 +0000 Subject: [PATCH 2/5] modified popf planner interface to parse best plan when mutiple plans are found --- .../PlannerInterface/POPFPlannerInterface.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp index 80047ccdc..c265e1c7e 100644 --- a/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp @@ -72,8 +72,10 @@ namespace KCL_rosplan { 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.pddl").c_str()); + std::ifstream planfile1; + std::ifstream planfile2; + planfile1.open((data_path + "plan.pddl").c_str()); + planfile2.open((data_path + "plan.pddl").c_str()); std::string line; std::stringstream ss; @@ -84,7 +86,7 @@ namespace KCL_rosplan { int line_count = 0; int best_plan_start_line = 0; - while (std::getline(planfile, line)) { + while (std::getline(planfile1, line)) { line_count++; if (line.find("; Plan found", 0) != std::string::npos || line.find(";;;; Solution Found", 0) != std::string::npos) { @@ -94,7 +96,7 @@ namespace KCL_rosplan { } } line_count = 0; - while (std::getline(planfile, line)) { + while (std::getline(planfile2, line)) { line_count++; if (line_count >= best_plan_start_line){ if (line.find("; Time", 0) == std::string::npos) { @@ -104,16 +106,19 @@ namespace KCL_rosplan { // read best plan planDuration = 0; ss.str(""); - while (std::getline(planfile, line)) { + while (std::getline(planfile2, line)) { if (line.length()<2) break; ss << line << std::endl; } planner_output = ss.str(); + + } } } - planfile.close(); + planfile1.close(); + planfile2.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()); From 7a1f08def1be0de3e4ef9398c4c7b336f0cdcf3c Mon Sep 17 00:00:00 2001 From: Dorian Date: Mon, 11 Nov 2019 14:41:28 +0000 Subject: [PATCH 3/5] added a service to import kb elements directly from a PDDL file --- .../rosplan_knowledge_base/KnowledgeBase.h | 6 +++++- .../PDDLKnowledgeBase.h | 7 ++++++- .../PPDDLKnowledgeBase.h | 3 +++ .../RDDLKnowledgeBase.h | 5 ++++- rosplan_knowledge_base/src/KnowledgeBase.cpp | 1 + .../src/PDDLKnowledgeBase.cpp | 21 +++++++++++++++++++ .../src/PPDDLKnowledgeBase.cpp | 5 +++++ .../src/RDDLKnowledgeBase.cpp | 5 +++++ rosplan_knowledge_msgs/CMakeLists.txt | 1 + .../srv/ImportStateFromFileService.srv | 9 ++++++++ 10 files changed, 60 insertions(+), 3 deletions(-) create mode 100644 rosplan_knowledge_msgs/srv/ImportStateFromFileService.srv diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h index 6eff45919..ce1653a45 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h @@ -26,6 +26,8 @@ #include "rosplan_knowledge_msgs/GetMetricService.h" #include "rosplan_knowledge_msgs/KnowledgeItem.h" +#include "rosplan_knowledge_msgs/ImportStateFromFileService.h" + #include "KnowledgeComparitor.h" @@ -51,6 +53,7 @@ namespace KCL_rosplan { ros::ServiceServer updateServer1; // updateKnowledge ros::ServiceServer updateServer2; // updateKnowledgeArray ros::ServiceServer updateServer3; // updateKnowledgeConstraintsOneOf + ros::ServiceServer updateServer4; // updateKnowledgeFromPDDLFile // fetch knowledge ros::ServiceServer stateServer1; // getInstances @@ -110,8 +113,9 @@ namespace KCL_rosplan { /* add the initial state to the knowledge base */ virtual void addInitialState() =0; virtual void addConstants() =0; + virtual bool importState(rosplan_knowledge_msgs::ImportStateFromFileService::Request &req, rosplan_knowledge_msgs::ImportStateFromFileService::Response &res) =0; - /* service methods for querying the model */ + /* service methods for querying the model */ bool queryKnowledge(rosplan_knowledge_msgs::KnowledgeQueryService::Request &req, rosplan_knowledge_msgs::KnowledgeQueryService::Response &res); /* service methods for fetching the current state */ diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/PDDLKnowledgeBase.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/PDDLKnowledgeBase.h index c5e7fd0a4..cf3df45dd 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/PDDLKnowledgeBase.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/PDDLKnowledgeBase.h @@ -25,6 +25,8 @@ #include "rosplan_knowledge_msgs/GetMetricService.h" #include "rosplan_knowledge_msgs/KnowledgeItem.h" +#include "rosplan_knowledge_msgs/ImportStateFromFileService.h" + #include "KnowledgeBase.h" #include "KnowledgeComparitor.h" #include "PDDLDomainParser.h" @@ -54,7 +56,10 @@ namespace KCL_rosplan { void addInitialState() override; void addConstants() override; - /* service methods for fetching the domain details */ + /* import state from file*/ + bool importState(rosplan_knowledge_msgs::ImportStateFromFileService::Request &req, rosplan_knowledge_msgs::ImportStateFromFileService::Response &res); + + /* 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; diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/PPDDLKnowledgeBase.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/PPDDLKnowledgeBase.h index 54b93cd34..bb721405e 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/PPDDLKnowledgeBase.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/PPDDLKnowledgeBase.h @@ -51,6 +51,9 @@ namespace KCL_rosplan { void addInitialState() override; void addConstants() override; + /* import state from file*/ + bool importState(rosplan_knowledge_msgs::ImportStateFromFileService::Request &req, rosplan_knowledge_msgs::ImportStateFromFileService::Response &res); + /* 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; diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h index 404bc5f63..a0e72482c 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h @@ -104,7 +104,10 @@ namespace KCL_rosplan { void addInitialState() override; void addConstants() override; - /* service methods for fetching the domain details */ + /* import state from file*/ + bool importState(rosplan_knowledge_msgs::ImportStateFromFileService::Request &req, rosplan_knowledge_msgs::ImportStateFromFileService::Response &res); + + /* 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; diff --git a/rosplan_knowledge_base/src/KnowledgeBase.cpp b/rosplan_knowledge_base/src/KnowledgeBase.cpp index 2050b2d65..b5218bd36 100644 --- a/rosplan_knowledge_base/src/KnowledgeBase.cpp +++ b/rosplan_knowledge_base/src/KnowledgeBase.cpp @@ -575,6 +575,7 @@ namespace KCL_rosplan { updateServer1 = _nh.advertiseService("update", &KCL_rosplan::KnowledgeBase::updateKnowledge, this); updateServer2 = _nh.advertiseService("update_array", &KCL_rosplan::KnowledgeBase::updateKnowledgeArray, this); updateServer3 = _nh.advertiseService("update_constraints_oneof", &KCL_rosplan::KnowledgeBase::updateKnowledgeConstraintsOneOf, this); + updateServer4 = _nh.advertiseService("import_state", &KCL_rosplan::KnowledgeBase::importState, this); // fetch knowledge stateServer1 = _nh.advertiseService("state/instances", &KCL_rosplan::KnowledgeBase::getInstances, this); diff --git a/rosplan_knowledge_base/src/PDDLKnowledgeBase.cpp b/rosplan_knowledge_base/src/PDDLKnowledgeBase.cpp index 83fe83750..3acf89757 100644 --- a/rosplan_knowledge_base/src/PDDLKnowledgeBase.cpp +++ b/rosplan_knowledge_base/src/PDDLKnowledgeBase.cpp @@ -148,6 +148,27 @@ namespace KCL_rosplan { /* add initial state to knowledge base */ /*-------------------------------------*/ + /* import state from file */ + bool PDDLKnowledgeBase::importState(rosplan_knowledge_msgs::ImportStateFromFileService::Request &req, rosplan_knowledge_msgs::ImportStateFromFileService::Response &res) { + + this->parseDomain(req.domain_path, req.problem_path); + res.state_imported = true; + + if(req.domain_string_response) { + + std::ifstream domainIn(req.domain_path.c_str()); + if(domainIn) res.domain_string = std::string(std::istreambuf_iterator(domainIn), std::istreambuf_iterator()); + } + + if(req.problem_string_response) { + + std::ifstream problemIn(req.problem_path.c_str()); + if(problemIn) res.state_string = std::string(std::istreambuf_iterator(problemIn), std::istreambuf_iterator()); + } + + return true; + } + /* get constants from the domain file */ void PDDLKnowledgeBase::addConstants() { diff --git a/rosplan_knowledge_base/src/PPDDLKnowledgeBase.cpp b/rosplan_knowledge_base/src/PPDDLKnowledgeBase.cpp index 1f7048039..9f9f33303 100644 --- a/rosplan_knowledge_base/src/PPDDLKnowledgeBase.cpp +++ b/rosplan_knowledge_base/src/PPDDLKnowledgeBase.cpp @@ -255,4 +255,9 @@ namespace KCL_rosplan { model_metric.expr = PPDDLUtils::getExpression(*metric, domain_parser.domain, var_decl); } + + /* import state from file - not implemented */ + bool PPDDLKnowledgeBase::importState(rosplan_knowledge_msgs::ImportStateFromFileService::Request &req, rosplan_knowledge_msgs::ImportStateFromFileService::Response &res) { + return false; + } } diff --git a/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp b/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp index e2f3ed287..510f41f0a 100644 --- a/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp +++ b/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp @@ -572,4 +572,9 @@ namespace KCL_rosplan { ROS_ERROR("KCL: (%s) getFluentType: Fluent %s was not found!", ros::this_node::getName().c_str(), req.fluent_name.c_str()); return true; } + + /* import state from file - not implemented */ + bool RDDLKnowledgeBase::importState(rosplan_knowledge_msgs::ImportStateFromFileService::Request &req, rosplan_knowledge_msgs::ImportStateFromFileService::Response &res) { + return false; + } } diff --git a/rosplan_knowledge_msgs/CMakeLists.txt b/rosplan_knowledge_msgs/CMakeLists.txt index 167f7dc75..a8f0f7b84 100644 --- a/rosplan_knowledge_msgs/CMakeLists.txt +++ b/rosplan_knowledge_msgs/CMakeLists.txt @@ -48,6 +48,7 @@ add_service_files( ReloadRDDLDomainProblem.srv GetEnumerableTypeService.srv GetRDDLFluentType.srv + ImportStateFromFileService.srv ) ## Generate added messages and services with any dependencies listed here diff --git a/rosplan_knowledge_msgs/srv/ImportStateFromFileService.srv b/rosplan_knowledge_msgs/srv/ImportStateFromFileService.srv new file mode 100644 index 000000000..4b9ff7586 --- /dev/null +++ b/rosplan_knowledge_msgs/srv/ImportStateFromFileService.srv @@ -0,0 +1,9 @@ +# request a pddl problem to be imported to the kb from the problem_path +string domain_path +string problem_path +bool domain_string_response +bool problem_string_response +--- +bool state_imported +string domain_string +string state_string \ No newline at end of file From e64f8e59265c01f3bc8ba96995345126827a764e Mon Sep 17 00:00:00 2001 From: Dorian Date: Tue, 12 Nov 2019 14:14:37 +0000 Subject: [PATCH 4/5] Modified getInstances/GetInstanceService to add constants list to the response. Modified PDDL Problem Generator to exclude instances that are constants. --- rosplan_knowledge_base/src/KnowledgeBase.cpp | 13 ++++++--- .../srv/GetInstanceService.srv | 1 + .../PDDLProblemGenerator.cpp | 28 +++++++++++++++++-- 3 files changed, 35 insertions(+), 7 deletions(-) diff --git a/rosplan_knowledge_base/src/KnowledgeBase.cpp b/rosplan_knowledge_base/src/KnowledgeBase.cpp index b5218bd36..407ab9878 100644 --- a/rosplan_knowledge_base/src/KnowledgeBase.cpp +++ b/rosplan_knowledge_base/src/KnowledgeBase.cpp @@ -476,8 +476,11 @@ namespace KCL_rosplan { } // constants for(iit=domain_constants.begin(); iit != domain_constants.end(); iit++) { - for(size_t j=0; jsecond.size(); j++) - res.instances.push_back(iit->second[j]); + for(size_t j=0; jsecond.size(); j++){ + res.instances.push_back(iit->second[j]); + res.constants.push_back(iit->second[j]); + } + } } else { std::map >::iterator iit; @@ -490,8 +493,10 @@ namespace KCL_rosplan { // constants iit = domain_constants.find(req.type_name); if(iit != domain_constants.end()) { - for(size_t j=0; jsecond.size(); j++) - res.instances.push_back(iit->second[j]); + for(size_t j=0; jsecond.size(); j++){ + res.instances.push_back(iit->second[j]); + res.constants.push_back(iit->second[j]); + } } } diff --git a/rosplan_knowledge_msgs/srv/GetInstanceService.srv b/rosplan_knowledge_msgs/srv/GetInstanceService.srv index 5b46cf5d2..9896a554a 100644 --- a/rosplan_knowledge_msgs/srv/GetInstanceService.srv +++ b/rosplan_knowledge_msgs/srv/GetInstanceService.srv @@ -3,3 +3,4 @@ string type_name --- string[] instances +string[] constants diff --git a/rosplan_planning_system/src/ProblemGeneration/PDDLProblemGenerator.cpp b/rosplan_planning_system/src/ProblemGeneration/PDDLProblemGenerator.cpp index 3569dcc11..4a9b5e176 100644 --- a/rosplan_planning_system/src/ProblemGeneration/PDDLProblemGenerator.cpp +++ b/rosplan_planning_system/src/ProblemGeneration/PDDLProblemGenerator.cpp @@ -47,10 +47,32 @@ namespace KCL_rosplan { if (!getInstancesClient.call(instanceSrv)) { ROS_ERROR("KCL: (PDDLProblemGenerator) Failed to call service %s: %s", state_instance_service.c_str(), instanceSrv.request.type_name.c_str()); } else { - if(instanceSrv.response.instances.size() == 0) continue; + if(instanceSrv.response.instances.size() == 0) continue; + + // check if instance is constant + std::vector constants; + bool isConstant = false; + for(size_t i=0;i Date: Tue, 12 Nov 2019 14:59:11 +0000 Subject: [PATCH 5/5] added removal of affected domain functions when an instance is removed from kb --- rosplan_knowledge_base/src/KnowledgeBase.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/rosplan_knowledge_base/src/KnowledgeBase.cpp b/rosplan_knowledge_base/src/KnowledgeBase.cpp index 407ab9878..44e9b0b31 100644 --- a/rosplan_knowledge_base/src/KnowledgeBase.cpp +++ b/rosplan_knowledge_base/src/KnowledgeBase.cpp @@ -271,6 +271,17 @@ namespace KCL_rosplan { pit++; } } + + // remove affected domain functions + std::vector::iterator fit; + for(fit=model_functions.begin(); fit!=model_functions.end(); ) { + if(KnowledgeComparitor::containsInstance(*fit, name)) { + ROS_INFO("KCL: (%s) Removing domain function (%s)", ros::this_node::getName().c_str(), fit->attribute_name.c_str()); + fit = model_functions.erase(fit); + } else { + fit++; + } + } } else { iit++; }