Merge branch 'master' of https://github.com/KCL-Planning/ROSPlan

b-it-bots · Jan 25, 2020 · 8a00aae · 8a00aae
2 parents 7868150 + 0efc6a0
commit 8a00aae
Show file tree

Hide file tree

Showing 4 changed files with 46 additions and 20 deletions.
diff --git a/rosplan_knowledge_base/src/KnowledgeBase.cpp b/rosplan_knowledge_base/src/KnowledgeBase.cpp
@@ -271,6 +271,15 @@ 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 {
 					iit++;
 				}

diff --git a/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp b/rosplan_planning_system/src/PlanDispatch/SimplePlanDispatcher.cpp
@@ -187,7 +187,7 @@ namespace KCL_rosplan {
 
 		std::string feedbackTopic = "action_feedback";
 		nh.getParam("action_feedback_topic", feedbackTopic);
-		ros::Subscriber feedback_sub = nh.subscribe(feedbackTopic, 1, &KCL_rosplan::SimplePlanDispatcher::feedbackCallback, &spd);
+		ros::Subscriber feedback_sub = nh.subscribe(feedbackTopic, 1000, &KCL_rosplan::SimplePlanDispatcher::feedbackCallback, &spd);
 
 		ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str());
 		ros::spin();

diff --git a/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/TFDPlannerInterface.cpp
@@ -48,6 +48,9 @@ namespace KCL_rosplan {
      * passes the problem to the Planner; the plan to post-processing.
      */
     bool TFDPlannerInterface::runPlanner() {
+        // path is based on the default installation of the Temporal Fast Downward
+        std::string plannerPlanPath = data_path + "../../rosplan_planning_system/common/bin/tfd-src-0.4/downward/";
+        std::string tfdOutputName = "tfdplan";
 
         // save problem to file for TFD
         if(use_problem_topic && problem_instance_received) {
@@ -65,17 +68,27 @@ namespace KCL_rosplan {
         std::size_t pit = str.find("PROBLEM");
         if(pit!=std::string::npos) str.replace(pit,7,problem_path);
 
-        // path is based on the default installation of the Temporal Fast Downward
-        std::string updatePlan = "cp "+data_path+"../../rosplan_planning_system/common/bin/tfd-src-0.4/downward/tfdplan.1"+" "+data_path+"plan.pddl";
-
+        // delete old plans before running the planner
+        ROS_INFO("KCL: (%s) Removing old TFD plans with (%s) name.", ros::this_node::getName().c_str(), tfdOutputName.c_str());
+        std::string removeOldPlans = "cd " + plannerPlanPath + " && rm " + tfdOutputName + ".*";
+        runCommand(removeOldPlans);
 
         // call the planer
         ROS_INFO("KCL: (%s) (%s) Running: %s", ros::this_node::getName().c_str(), problem_name.c_str(),  str.c_str());
         std::string plan = runCommand(str.c_str());
         ROS_INFO("KCL: (%s) (%s) Planning complete", ros::this_node::getName().c_str(), problem_name.c_str());
 
-        // move plan to correct path
-        runCommand(updatePlan.c_str());
+        // get the most optimal plan in case there are many (tfdplan.[the highest number])
+        std::string bestPlanCommand = "cd " + plannerPlanPath + " && ls | grep " + tfdOutputName + " | tail -1";
+        std::string bestPlan = runCommand(bestPlanCommand.c_str());
+        // get rid of carriage return
+        if (bestPlan[bestPlan.size() - 1] == '\n') {
+            bestPlan.erase(bestPlan.size() - 1);
+        }
+
+        // prepare command to copy plan
+        std::string updatePlanCommand = "cp " + plannerPlanPath + bestPlan + " " + data_path + "plan.pddl";
+        runCommand(updatePlanCommand.c_str());
 
         // check the planner solved the problem
         std::ifstream planfile;

diff --git a/rosplan_sensing_interface/scripts/sensing_interface.py b/rosplan_sensing_interface/scripts/sensing_interface.py
@@ -23,21 +23,25 @@ def __init__(self):
         self.srv_mutex = Lock()
 
         ################################################################################################################
+        self.knowledge_base = '/rosplan_knowledge_base'
+        if rospy.has_param('~knowledge_base'):
+            self.knowledge_base = rospy.get_param('~knowledge_base')
+
         # Init clients and publishers
-        rospy.wait_for_service('/rosplan_knowledge_base/update_array')
-        self.update_kb_srv = rospy.ServiceProxy('/rosplan_knowledge_base/update_array', KnowledgeUpdateServiceArray)
-        rospy.wait_for_service('/rosplan_knowledge_base/domain/predicate_details')
-        self.get_predicates_srv = rospy.ServiceProxy('/rosplan_knowledge_base/domain/predicate_details', GetDomainPredicateDetailsService)
-        rospy.wait_for_service('/rosplan_knowledge_base/domain/functions')
-        self.get_functions_srv = rospy.ServiceProxy('/rosplan_knowledge_base/domain/functions', GetDomainAttributeService)
-        rospy.wait_for_service('/rosplan_knowledge_base/state/instances')
-        self.get_instances_srv = rospy.ServiceProxy('/rosplan_knowledge_base/state/instances', GetInstanceService)
-        rospy.wait_for_service('/rosplan_knowledge_base/state/propositions')
-        self.get_state_propositions_srv = rospy.ServiceProxy('/rosplan_knowledge_base/state/propositions', GetAttributeService)
-        rospy.wait_for_service('/rosplan_knowledge_base/state/functions')
-        self.get_state_functions_srv = rospy.ServiceProxy('/rosplan_knowledge_base/state/functions', GetAttributeService)
-
-        self.set_sensed_predicate_srv = rospy.ServiceProxy('/rosplan_knowledge_base/update_sensed_predicates', SetNamedBool)
+        rospy.wait_for_service(self.knowledge_base + '/update_array')
+        self.update_kb_srv = rospy.ServiceProxy(self.knowledge_base + '/update_array', KnowledgeUpdateServiceArray)
+        rospy.wait_for_service(self.knowledge_base + '/domain/predicate_details')
+        self.get_predicates_srv = rospy.ServiceProxy(self.knowledge_base + '/domain/predicate_details', GetDomainPredicateDetailsService)
+        rospy.wait_for_service(self.knowledge_base + '/domain/functions')
+        self.get_functions_srv = rospy.ServiceProxy(self.knowledge_base + '/domain/functions', GetDomainAttributeService)
+        rospy.wait_for_service(self.knowledge_base + '/state/instances')
+        self.get_instances_srv = rospy.ServiceProxy(self.knowledge_base + '/state/instances', GetInstanceService)
+        rospy.wait_for_service(self.knowledge_base + '/state/propositions')
+        self.get_state_propositions_srv = rospy.ServiceProxy(self.knowledge_base + '/state/propositions', GetAttributeService)
+        rospy.wait_for_service(self.knowledge_base + '/state/functions')
+        self.get_state_functions_srv = rospy.ServiceProxy(self.knowledge_base + '/state/functions', GetAttributeService)
+
+        self.set_sensed_predicate_srv = rospy.ServiceProxy(self.knowledge_base + '/update_sensed_predicates', SetNamedBool)
 
         ################################################################################################################
         # Get cfg