diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 5929a092d..472325730 100644 --- 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 index a8bbc4fc2..823c3f31a 100644 --- 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 index 917954200..18ff6360c 100755 --- 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 index 6540f62a9..fb6fc9dd0 100644 --- 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/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h index a02342c53..ec3d9b8d1 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h +++ b/rosplan_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& nonfluents); - void makeInstance(std::ofstream &pFile, const std::set& fluents); + void makeNonFluents(std::ofstream &pFile, const std::set& nonfluents, const std::map &fluent_types); + void makeInstance(std::ofstream &pFile, const std::set& fluents, const std::map &fluent_types); /* returns map of type -> list of instances of that type */ std::map> getInstances(); @@ -29,8 +29,10 @@ namespace KCL_rosplan { std::set &found_nonfluents); std::vector getPredicatesFunctions(); void printGenericFluentElement(std::ofstream &pFile, - const std::vector &elem); - void printGenericFluentList(std::ofstream &pFile, const std::set& fluentlist); + std::vector::iterator fluent, const std::map &fluent_types); + void printGenericFluentList(std::ofstream &pFile, const std::set& fluentlist, const std::map &fluent_types); + + void getFluentTypes(const std::set &fluents, const std::set &nonfluents, std::map &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 index b09519d41..55e27c653 100644 --- 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 pred_funcs = getPredicatesFunctions(); std::set fluents, non_fluents; getAllFluents(pred_funcs, fluents, non_fluents); + std::map 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& nonfluents) { + void RDDLProblemGenerator::makeNonFluents(std::ofstream &pFile, const std::set& nonfluents, const std::map &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 &fluents) { + void RDDLProblemGenerator::makeInstance(std::ofstream &pFile, const std::set &fluents, const std::map &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 &fluents, const std::set &nonfluents, std::map &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 RDDLProblemGenerator::getPredicatesFunctions() { std::vector pred_funcs; @@ -233,66 +256,66 @@ namespace KCL_rosplan { } void RDDLProblemGenerator::printGenericFluentElement(std::ofstream &pFile, - const std::vector &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::iterator pfit, + const std::map &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 &fluentlist) { + void RDDLProblemGenerator::printGenericFluentList(std::ofstream &pFile, const std::set &fluentlist, const std::map &fluent_types) { ros::ServiceClient getPropsClient = _nh.serviceClient(state_proposition_service); ros::ServiceClient getFuncsClient = _nh.serviceClient(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); + } + } } } \ No newline at end of file