diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h index 71f13464a..381ffe39a 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/KnowledgeBase.h @@ -27,6 +27,8 @@ #include "rosplan_knowledge_msgs/GetMetricService.h" #include "rosplan_knowledge_msgs/KnowledgeItem.h" +#include "rosplan_knowledge_msgs/ImportStateFromFileService.h" + #include "KnowledgeComparitor.h" @@ -52,6 +54,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 @@ -113,8 +116,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 2bd1bfa3f..d53a95332 100644 --- a/rosplan_knowledge_base/src/KnowledgeBase.cpp +++ b/rosplan_knowledge_base/src/KnowledgeBase.cpp @@ -280,16 +280,27 @@ namespace KCL_rosplan { pit++; } } - for(pit=model_functions.begin(); pit!=model_functions.end(); ) { - if(KnowledgeComparitor::containsInstance(*pit, name)) { - ROS_INFO("KCL: (%s) Removing domain attribute (%s)", ros::this_node::getName().c_str(), pit->attribute_name.c_str()); - pit = model_functions.erase(pit); - } else { - pit++; - } - } - } else { + // 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++; + } + } + for(pit=model_functions.begin(); pit!=model_functions.end(); ) { + if(KnowledgeComparitor::containsInstance(*pit, name)) { + ROS_INFO("KCL: (%s) Removing domain attribute (%s)", ros::this_node::getName().c_str(), pit->attribute_name.c_str()); + pit = model_functions.erase(pit); + } else { + pit++; + } + } + + } else { iit++; } } @@ -494,8 +505,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; @@ -508,8 +522,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]); + } } } @@ -593,6 +609,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 958e89e97..3be0401f1 100644 --- a/rosplan_knowledge_msgs/CMakeLists.txt +++ b/rosplan_knowledge_msgs/CMakeLists.txt @@ -49,6 +49,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/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_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 diff --git a/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/POPFPlannerInterface.cpp index c8ad1ce2f..c265e1c7e 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; @@ -72,34 +72,53 @@ 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; 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(); - } - } - planfile.close(); + int line_count = 0; + int best_plan_start_line = 0; + while (std::getline(planfile1, 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(planfile2, 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(planfile2, line)) { + if (line.length()<2) + break; + ss << line << std::endl; + } + planner_output = ss.str(); + + + } + } + } + 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()); @@ -121,7 +140,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); 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