Merge pull request #327 from gerardcanal/master

Reduce RDDL problem generation service calls for speedup in big instances
KCL-Planning · Feb 7, 2024 · c8de66a · c8de66a
2 parents 6390372 + cb95f9e
commit c8de66a
Show file tree

Hide file tree

Showing 6 changed files with 89 additions and 62 deletions.
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -20,7 +20,7 @@ jobs:
           APT_KEY_DONT_WARN_ON_DANGEROUS_USAGE=true apt-key adv --keyserver keyserver.ubuntu.com --recv-key E1DD270288B4E6030699E45FA1715D88E1DF1F24 2>&1
           echo "deb http://ppa.launchpad.net/git-core/ppa/ubuntu bionic main" | tee /etc/apt/sources.list.d/git.list 
           sudo apt update -qq
-          sudo apt install python-catkin-tools git -y -qq
+          sudo apt install python-catkin-tools git ros-melodic-tf -y -qq
       - name: Create ROS workspace
         run: |
           source /opt/ros/$ROS_VERSION/setup.bash

diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml
@@ -15,7 +15,7 @@ jobs:
           APT_KEY_DONT_WARN_ON_DANGEROUS_USAGE=true apt-key adv --keyserver keyserver.ubuntu.com --recv-key E1DD270288B4E6030699E45FA1715D88E1DF1F24 2>&1
           echo "deb http://ppa.launchpad.net/git-core/ppa/ubuntu bionic main" | tee /etc/apt/sources.list.d/git.list 
           sudo apt update -qq
-          sudo apt install python-catkin-tools git -y -qq
+          sudo apt install python-catkin-tools git ros-melodic-tf -y -qq
       - name: Create ROS workspace
         run: |
           source /opt/ros/$ROS_VERSION/setup.bash

diff --git a/rosplan_action_interface/src/ActionInterfaceManager.py b/rosplan_action_interface/src/ActionInterfaceManager.py
@@ -87,7 +87,8 @@ def run(self):
 
             # iterate through interfaces and send feedback
             for interface in self._action_interfaces.values():
-                for act in interface._action_status.keys():
+                keys = list(interface._action_status.keys()) # copy keys to avoid dict size change during iteration
+                for act in keys:
 
                     # action successful
                     if interface._action_status[act] == ActionFeedback.ACTION_SUCCEEDED_TO_GOAL_STATE:
@@ -96,7 +97,7 @@ def run(self):
 
                     # action completed (achieved or failed)
                     if interface._action_status[act] == ActionFeedback.ACTION_SUCCEEDED_TO_GOAL_STATE or interface._action_status[act] == ActionFeedback.ACTION_FAILED:
-                        rospy.loginfo('KCL: ({}) Reporting action complete: {} {}'.format(rospy.get_name(), act, interface._action_name))
+                        rospy.loginfo('KCL: ({}) Reporting action complete: {} {}. Action {}.'.format(rospy.get_name(), act, interface._action_name, "succeeded" if interface._action_status[act] == ActionFeedback.ACTION_SUCCEEDED_TO_GOAL_STATE else "failed"))
                         # publish feedback msg
                         self.publish_feedback(act[0], act[1], interface._action_status[act])
                         # remove completed action data from interface

diff --git a/rosplan_action_interface/src/ServiceActionInterface.py b/rosplan_action_interface/src/ServiceActionInterface.py
@@ -105,6 +105,7 @@ def run_thread(self, dispatch_msg):
                     value = override_result[param]
                     results_correct = self.check_result_msg(result, param, value, dispatch_msg)
                     if not results_correct:
+                        rospy.logwarn('KCL: ({}) Plan {} Action {}: Service {} failed to match result'.format(rospy.get_name(), dispatch_msg.plan_id, dispatch_msg.action_id, self.get_action_name()))
                         break
 
             if results_correct:

diff --git a/..._planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h b/..._planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h
@@ -18,8 +18,8 @@ namespace KCL_rosplan {
     private:
         inline void makeProblem(std::ofstream &pFile) override;
 
-        void makeNonFluents(std::ofstream &pFile, const std::set<std::string>& nonfluents);
-        void makeInstance(std::ofstream &pFile, const std::set<std::string>& fluents);
+        void makeNonFluents(std::ofstream &pFile, const std::set<std::string>& nonfluents, const std::map<std::string, std::string> &fluent_types);
+        void makeInstance(std::ofstream &pFile, const std::set<std::string>& fluents, const std::map<std::string, std::string> &fluent_types);
 
         /* returns map of type -> list of instances of that type */
         std::map<std::string, std::vector<std::string>> getInstances();
@@ -29,8 +29,10 @@ namespace KCL_rosplan {
                            std::set<std::string> &found_nonfluents);
         std::vector<std::string> getPredicatesFunctions();
         void printGenericFluentElement(std::ofstream &pFile,
-                                       const std::vector<rosplan_knowledge_msgs::KnowledgeItem> &elem);
-        void printGenericFluentList(std::ofstream &pFile, const std::set<std::string>& fluentlist);
+                                       std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator fluent, const std::map<std::string, std::string> &fluent_types);
+        void printGenericFluentList(std::ofstream &pFile, const std::set<std::string>& fluentlist, const std::map<std::string, std::string> &fluent_types);
+
+        void getFluentTypes(const std::set<std::string> &fluents, const std::set<std::string> &nonfluents, std::map<std::string, std::string> &fluent_types);
 
         std::string _domain_name;
         std::string _non_fluents_name;

diff --git a/rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp b/rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp
@@ -43,12 +43,14 @@ namespace KCL_rosplan {
         std::vector<std::string> pred_funcs = getPredicatesFunctions();
         std::set<std::string> fluents, non_fluents;
         getAllFluents(pred_funcs, fluents, non_fluents);
+        std::map<std::string, std::string> fluent_types;
+        getFluentTypes(fluents, non_fluents, fluent_types);
         pFile << "// Auto-generated problem file by ROSPlan" << std::endl;
-        makeNonFluents(pFile, non_fluents);
-        makeInstance(pFile, fluents);
+        makeNonFluents(pFile, non_fluents, fluent_types);
+        makeInstance(pFile, fluents, fluent_types);
     }
 
-    void RDDLProblemGenerator::makeNonFluents(std::ofstream &pFile, const std::set<std::string>& nonfluents) {
+    void RDDLProblemGenerator::makeNonFluents(std::ofstream &pFile, const std::set<std::string>& nonfluents, const std::map<std::string, std::string> &fluent_types) {
         pFile << "non-fluents "<< _non_fluents_name << " {" << std::endl;
         pFile << "\tdomain = " << _domain_name << ";" << std::endl;
 
@@ -86,20 +88,20 @@ namespace KCL_rosplan {
 
         // Print non-fluents
         pFile << "\tnon-fluents {" << std::endl;
-        printGenericFluentList(pFile, nonfluents);
+        printGenericFluentList(pFile, nonfluents, fluent_types);
         pFile << "\t};" << std::endl;
         pFile << "}" << std::endl;
     }
 
 
-    void RDDLProblemGenerator::makeInstance(std::ofstream &pFile, const std::set<std::string> &fluents) {
+    void RDDLProblemGenerator::makeInstance(std::ofstream &pFile, const std::set<std::string> &fluents, const std::map<std::string, std::string> &fluent_types) {
         pFile << "instance " << _domain_name << "__generated_instance {" << std::endl;
         pFile << "\tdomain = " << _domain_name << ";" << std::endl;
         pFile << "\tnon-fluents = " << _non_fluents_name << ";" << std::endl;
 
         // Initial state
         pFile << "\tinit-state {" << std::endl;
-        printGenericFluentList(pFile, fluents);
+        printGenericFluentList(pFile, fluents, fluent_types);
         pFile << "\t};" << std::endl;
 
         std::string srv_name = "/" + knowledge_base + "/state/rddl_parameters";
@@ -210,6 +212,27 @@ namespace KCL_rosplan {
         }
     }
 
+    void RDDLProblemGenerator::getFluentTypes(const std::set<std::string> &fluents, const std::set<std::string> &nonfluents, std::map<std::string, std::string> &fluent_types) {
+        for (auto pfit = fluents.begin(); pfit != fluents.end(); ++pfit) {
+            rosplan_knowledge_msgs::GetRDDLFluentType gft;
+            gft.request.fluent_name = *pfit;
+            if (!_get_fluent_type.call(gft)) {
+                ROS_ERROR("KCL: (PDDLProblemGenerator) Failed to call service %s: %s", _get_fluent_type.getService().c_str(), gft.request.fluent_name.c_str());
+                return;
+            }
+            fluent_types[*pfit] = gft.response.type;
+        }
+        for (auto pfit = nonfluents.begin(); pfit != nonfluents.end(); ++pfit) {
+            rosplan_knowledge_msgs::GetRDDLFluentType gft;
+            gft.request.fluent_name = *pfit;
+            if (!_get_fluent_type.call(gft)) {
+                ROS_ERROR("KCL: (PDDLProblemGenerator) Failed to call service %s: %s", _get_fluent_type.getService().c_str(), gft.request.fluent_name.c_str());
+                return;
+            }
+            fluent_types[*pfit] = gft.response.type;
+        }
+    }
+
     std::vector<std::string> RDDLProblemGenerator::getPredicatesFunctions() {
         std::vector<std::string> pred_funcs;
 
@@ -233,66 +256,66 @@ namespace KCL_rosplan {
     }
 
     void RDDLProblemGenerator::printGenericFluentElement(std::ofstream &pFile,
-                                                         const std::vector<rosplan_knowledge_msgs::KnowledgeItem> &elem) {
-        for (auto pfit = elem.begin(); pfit != elem.end(); ++pfit) {
-            pFile << "\t\t" << pfit->attribute_name;
-
-            // Parameters if any
-            if (pfit->values.size() > 0) {
-                pFile << "(";
-                bool comma = false;
-                for (auto it = pfit->values.begin(); it != pfit->values.end(); ++it) {
-                    if (comma) pFile << ", ";
-                    else comma = true;
-                    pFile << it->value;
-                }
-                pFile << ")";
+                                                         std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pfit, 
+                                                         const std::map<std::string, std::string> &fluent_types) {
+        pFile << "\t\t" << pfit->attribute_name;
+
+        // Parameters if any
+        if (pfit->values.size() > 0) {
+            pFile << "(";
+            bool comma = false;
+            for (auto it = pfit->values.begin(); it != pfit->values.end(); ++it) {
+                if (comma) pFile << ", ";
+                else comma = true;
+                pFile << it->value;
             }
+            pFile << ")";
+        }
 
-            // Value
-            pFile << " = ";
-            if (pfit->knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::FACT) {
-                pFile << ((pfit->is_negative == 0)? "true" : "false");
-            }
-            else { // FUNCTION
-                rosplan_knowledge_msgs::GetRDDLFluentType gft;
-                gft.request.fluent_name = pfit->attribute_name;
-                if (!_get_fluent_type.call(gft)) {
-                    ROS_ERROR("KCL: (PDDLProblemGenerator) Failed to call service %s: %s",
-                              _get_fluent_type.getService().c_str(), gft.request.fluent_name.c_str());
-                    return;
-                }
-                auto enum_type = _enumeration_types.find(gft.response.type);
-                if (enum_type != _enumeration_types.end()) {
-                    pFile << enum_type->second[pfit->function_value];
-                }
-                else pFile << pfit->function_value;
+        // Value
+        pFile << " = ";
+        if (pfit->knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::FACT) {
+            pFile << ((pfit->is_negative == 0)? "true" : "false");
+        }
+        else { // FUNCTION
+            auto enum_type = _enumeration_types.find(fluent_types.at(pfit->attribute_name));
+            if (enum_type != _enumeration_types.end()) {
+                pFile << enum_type->second[pfit->function_value];
             }
-            pFile << ";" << std::endl;
+            else pFile << pfit->function_value;
         }
+        pFile << ";" << std::endl;
     }
 
-    void RDDLProblemGenerator::printGenericFluentList(std::ofstream &pFile, const std::set<std::string> &fluentlist) {
+    void RDDLProblemGenerator::printGenericFluentList(std::ofstream &pFile, const std::set<std::string> &fluentlist, const std::map<std::string, std::string> &fluent_types) {
         ros::ServiceClient getPropsClient = _nh.serviceClient<rosplan_knowledge_msgs::GetAttributeService>(state_proposition_service);
         ros::ServiceClient getFuncsClient = _nh.serviceClient<rosplan_knowledge_msgs::GetAttributeService>(state_function_service);
-        for (auto it = fluentlist.begin(); it != fluentlist.end(); ++it) {
-            rosplan_knowledge_msgs::GetAttributeService attrSrv;
-            attrSrv.request.predicate_name = *it;
-            if (!getPropsClient.call(attrSrv)) {
-                ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s: %s",
-                          state_proposition_service.c_str(), attrSrv.request.predicate_name.c_str());
-                continue;
-            }
+
+        rosplan_knowledge_msgs::GetAttributeService prop_attrSrv; // Get all the propositions to prevent multiple calls
+        if (!getPropsClient.call(prop_attrSrv)) {
+            ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s: %s",
+                        state_proposition_service.c_str(), prop_attrSrv.request.predicate_name.c_str());
+        }
 
-            // If it was not a proposition, try functions
-            if (attrSrv.response.attributes.size() == 0 and !getFuncsClient.call(attrSrv)) {
-                ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s: %s",
-                          state_proposition_service.c_str(), attrSrv.request.predicate_name.c_str());
-                continue;
+        rosplan_knowledge_msgs::GetAttributeService func_attrSrv; // Get all the functions to prevent multiple calls
+        if (!getFuncsClient.call(func_attrSrv)) {
+            ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s: %s",
+                        state_proposition_service.c_str(), func_attrSrv.request.predicate_name.c_str());
+        }
+
+        // Check propositions
+        for (auto pit = prop_attrSrv.response.attributes.begin(); pit != prop_attrSrv.response.attributes.end(); ++pit) {
+            for (auto it = fluentlist.begin(); it != fluentlist.end(); ++it) {
+                if (pit->attribute_name == *it) printGenericFluentElement(pFile, pit, fluent_types);
             }
-            printGenericFluentElement(pFile, attrSrv.response.attributes);
         }
 
+        // Check functions
+        for (auto pit = func_attrSrv.response.attributes.begin(); pit != func_attrSrv.response.attributes.end(); ++pit) {
+            for (auto it = fluentlist.begin(); it != fluentlist.end(); ++it) {
+                if (pit->attribute_name == *it) printGenericFluentElement(pFile, pit, fluent_types);
+            }
+        }
     }
 
 }