1. modified popf planner interface to parse best plan when mutiple plans 2. added a service to import kb elements directly from a PDDL file

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 */

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;

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;

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;

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<rosplan_knowledge_msgs::KnowledgeItem>::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; j<iit->second.size(); j++)
-					res.instances.push_back(iit->second[j]);
+				for(size_t j=0; j<iit->second.size(); j++){
+                    res.instances.push_back(iit->second[j]);
+                    res.constants.push_back(iit->second[j]);
+				}
+
 			}
 		} else {
 			std::map<std::string,std::vector<std::string> >::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; j<iit->second.size(); j++)
-					res.instances.push_back(iit->second[j]);
+				for(size_t j=0; j<iit->second.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);

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<char>(domainIn), std::istreambuf_iterator<char>());
+        }
+
+        if(req.problem_string_response) {
+
+            std::ifstream problemIn(req.problem_path.c_str());
+            if(problemIn) res.state_string = std::string(std::istreambuf_iterator<char>(problemIn), std::istreambuf_iterator<char>());
+        }
+
+        return true;
+    }
+
     /* get constants from the domain file */
     void PDDLKnowledgeBase::addConstants() {
 

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;
+    }
 }

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;
+    }
 }

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

rosplan_knowledge_msgs/srv/GetInstanceService.srv

@@ -3,3 +3,4 @@
 string type_name
 ---
 string[] instances
+string[] constants

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

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);

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<bool> constants;
+                bool isConstant = false;
+                for(size_t i=0;i<instanceSrv.response.instances.size();i++) {
+                    for (size_t j = 0; j < instanceSrv.response.constants.size(); j++) {
+                        if (instanceSrv.response.instances[i].compare(instanceSrv.response.constants[j]) == 0) {
+                            isConstant = true;
+                        }
+                    }
+                    constants.push_back(isConstant);
+                }
+
+                // skip if all instances are constants
+                bool isAnyNonConstant = false;
+                for(size_t i=0;i<constants.size();i++) {
+                    if(!constants[i])isAnyNonConstant = true;
+                }
+				if(!isAnyNonConstant) continue;
+
+
+				// write to file if at least one instance is not a constant
 				pFile << "    ";
-				for(size_t i=0;i<instanceSrv.response.instances.size();i++) {
-					pFile << instanceSrv.response.instances[i] << " ";
+				for(size_t i=0;i<constants.size();i++) {
+				    if (!constants[i])pFile << instanceSrv.response.instances[i] << " ";
 				}
 				pFile << "- " << typeSrv.response.types[t] << std::endl;
 			}