[WIP] PANDA planner integration

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
 )

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

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

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

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,13 +66,15 @@ 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 */
 		void removeKnowledge(rosplan_knowledge_msgs::KnowledgeItem &msg);
         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<rosplan_knowledge_msgs::KnowledgeItem> model_facts;
 		std::vector<rosplan_knowledge_msgs::KnowledgeItem> model_functions;
 		std::vector<rosplan_knowledge_msgs::KnowledgeItem> model_goals;
+		std::vector<rosplan_knowledge_msgs::KnowledgeItem> 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);
 

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<KnowledgeBase> KnowledgeBasePtr;
@@ -17,7 +18,8 @@ namespace KCL_rosplan {
         enum KB {
             PDDL,
             PPDDL,
-            RDDL
+            RDDL,
+            HDDL
         };
 
         static KnowledgeBasePtr createKB(KB kb_type, ros::NodeHandle& n);

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

rosplan_knowledge_base/include/rosplan_knowledge_base/VALVisitorProblem.h

@@ -61,6 +61,7 @@ namespace KCL_rosplan {
 		std::vector<rosplan_knowledge_msgs::KnowledgeItem> facts;
 		std::vector<rosplan_knowledge_msgs::KnowledgeItem> functions;
 		std::vector<rosplan_knowledge_msgs::KnowledgeItem> goals;
+		std::vector<rosplan_knowledge_msgs::KnowledgeItem> tasks;
 		rosplan_knowledge_msgs::KnowledgeItem metric;
 
 		/* timed initial literals */
@@ -71,6 +72,7 @@ namespace KCL_rosplan {
         std::vector<rosplan_knowledge_msgs::KnowledgeItem> returnFacts();
         std::vector<rosplan_knowledge_msgs::KnowledgeItem> returnFunctions();
         std::vector<rosplan_knowledge_msgs::KnowledgeItem> returnGoals();
+        std::vector<rosplan_knowledge_msgs::KnowledgeItem> returnTasks();
 		rosplan_knowledge_msgs::KnowledgeItem returnMetric();
 
 		std::vector<rosplan_knowledge_msgs::KnowledgeItem> returnTimedKnowledge();

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

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<rosplan_knowledge_msgs::KnowledgeItem>::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<rosplan_knowledge_msgs::KnowledgeItem>::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<std::string,std::vector<std::string> >::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<model_goals.size(); i++)
+			res.attributes.push_back(model_tasks[i]);
+		return true;
+	}
+
 	bool KnowledgeBase::getMetric(rosplan_knowledge_msgs::GetMetricService::Request  &req, rosplan_knowledge_msgs::GetMetricService::Response &res) {
 		res.metric = model_metric;
 		return true;
@@ -591,6 +647,7 @@ namespace KCL_rosplan {
 		stateServer3 = _nh.advertiseService("state/functions",			&KCL_rosplan::KnowledgeBase::getFunctions, this);
 		stateServer4 = _nh.advertiseService("state/timed_knowledge",	&KCL_rosplan::KnowledgeBase::getTimedKnowledge, this);
 		stateServer5 = _nh.advertiseService("state/goals",				&KCL_rosplan::KnowledgeBase::getGoals, this);
+		stateServer5 = _nh.advertiseService("state/tasks",				&KCL_rosplan::KnowledgeBase::getTasks, this);
 		stateServer6 = _nh.advertiseService("state/metric",			    &KCL_rosplan::KnowledgeBase::getMetric, this);
 
 		// set sensed predicates
@@ -625,21 +682,26 @@ int main(int argc, char **argv) {
 	    kb_type = KCL_rosplan::KnowledgeBaseFactory::PDDL;
             ROS_INFO("KCL: (%s) Starting a PDDL Knowledge Base", ros::this_node::getName().c_str());
         }
-        else if (extension == ".ppddl") {
-            kb_type = KCL_rosplan::KnowledgeBaseFactory::PPDDL;
-            VAL1_2::parse_category::recoverWriteController(); // This avoids a segfault on finish when PDDL kb is not used
-            ROS_INFO("KCL: (%s) Starting a PPDDL Knowledge Base", ros::this_node::getName().c_str());
+    else if (extension == ".ppddl") {
+        kb_type = KCL_rosplan::KnowledgeBaseFactory::PPDDL;
+        VAL1_2::parse_category::recoverWriteController(); // This avoids a segfault on finish when PDDL kb is not used
+        ROS_INFO("KCL: (%s) Starting a PPDDL Knowledge Base", ros::this_node::getName().c_str());
         }
 	else if (extension == ".rddl") {
 	    kb_type = KCL_rosplan::KnowledgeBaseFactory::RDDL;
             VAL1_2::parse_category::recoverWriteController(); // This avoids a segfault on finish when PDDL kb is not used
             ROS_INFO("KCL: (%s) Starting a RDDL Knowledge Base", ros::this_node::getName().c_str());
         }
-        else {
+    else if (extension == ".hddl") {
+	    kb_type = KCL_rosplan::KnowledgeBaseFactory::HDDL;
+            ROS_INFO("KCL: (%s) Starting an HDDL Knowledge Base", ros::this_node::getName().c_str());
+        }
+    else {
             ROS_ERROR("KCL: (%s) Unexpected domain file extension %s (expected PDDL/RDDL)", ros::this_node::getName().c_str(), extension.c_str());
             ros::shutdown();
         }
 
+
 	KCL_rosplan::KnowledgeBasePtr kb = KCL_rosplan::KnowledgeBaseFactory::createKB(kb_type, n);
 
 	// parse domain
@@ -651,4 +713,3 @@ int main(int argc, char **argv) {
 
 	return 0;
 }
-

rosplan_knowledge_base/src/KnowledgeBaseFactory.cpp

@@ -10,10 +10,11 @@ namespace KCL_rosplan {
         if (kb_type == KB::PDDL) return KnowledgeBasePtr(new PDDLKnowledgeBase(n));
         else if (kb_type == KB::PPDDL) return KnowledgeBasePtr(new PPDDLKnowledgeBase(n));
         else if (kb_type == KB::RDDL) return KnowledgeBasePtr(new RDDLKnowledgeBase(n));
+        else if (kb_type == KB::HDDL) return KnowledgeBasePtr(new HDDLKnowledgeBase(n));
         else {
             ROS_ERROR("KCL: (%s) Unknown Knowledge Base type.", ros::this_node::getName().c_str());
             ros::shutdown();
         }
 
     }
-}
+}