diff --git a/bin/execute_plan.py b/bin/execute_plan.py new file mode 100755 index 0000000..0e3aafe --- /dev/null +++ b/bin/execute_plan.py @@ -0,0 +1,15 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from __future__ import print_function +from rosplan_pytools.rosplan.controller import planning_system as ps + + +def main(): + + ps.initialize() + ps.execute_plan() + + +if __name__ == "__main__": + main() diff --git a/examples/add_elements_to_kb.py b/examples/add_elements_to_kb.py new file mode 100755 index 0000000..76a62b7 --- /dev/null +++ b/examples/add_elements_to_kb.py @@ -0,0 +1,65 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from __future__ import print_function +from collections import OrderedDict + +import rospy +from rosplan_pytools.rosplan.controller import knowledge_base as kb + + +def _dump_kb(): + + print("INSTANCES ---") + print(str(kb.list_instances())) + for instance in kb.list_instances(): + print("[" + str(instance) + ", " + str(kb.get_instance_type(instance)) + "]") + print("\n") + + print("PREDICATES ---") + for predicate in kb.list_predicates(): + print("[" + str(predicate) + "]") + print("\n") + + print("FUNCTIONS ---") + for function in kb.list_functions(): + print("[" + str(function) + "]") + print("\n") + + print("GOALS ---") + for goal in kb.list_goals(): + print("[" + str(goal) + "]") + print("\n") + + +def main(): + + rospy.init_node("example_add_elements_to_kb") + + kb.initialize() + kb.reset() + + print("Empty KB") + _dump_kb() + + kb.add_instance("r", "robot") + kb.add_predicate("robot-at", OrderedDict([("p", "home")])) + kb.add_predicate("free-tool") + kb.add_function("occupancy", OrderedDict([('b', 'box_1')]), 0.0) + + print("KB with information") + _dump_kb() + + kb.remove_instance("r", "robot") + kb.remove_predicate("robot-at", OrderedDict([("p", "home")])) + kb.remove_predicate("free-tool") + kb.remove_function("occupancy", OrderedDict([('b', 'box_1')]), 0.0) + + print("Empty") + _dump_kb() + + kb.reset() + + +if __name__ == "__main__": + main() diff --git a/src/rosplan_pytools/rosplan/controller/knowledge_base.py b/src/rosplan_pytools/rosplan/controller/knowledge_base.py index bbf118a..5b4446c 100644 --- a/src/rosplan_pytools/rosplan/controller/knowledge_base.py +++ b/src/rosplan_pytools/rosplan/controller/knowledge_base.py @@ -27,7 +27,7 @@ KB_ITEM_INSTANCE = KnowledgeItem.INSTANCE KB_ITEM_FACT = KnowledgeItem.FACT - +KB_ITEM_FUNCTION = KnowledgeItem.FUNCTION _services = {} @@ -56,15 +56,18 @@ def _initialize_services(prefix): rospy.ServiceProxy(prefix + "/domain/types", GetDomainTypeService) - _services["get_current_instances"] = \ + _services["get_instances"] = \ rospy.ServiceProxy(prefix + "/state/instances", GetInstanceService) - _services["get_current_goals"] = \ + _services["get_goals"] = \ rospy.ServiceProxy(prefix + "/state/goals", GetAttributeService) _services["get_propositions"] = \ rospy.ServiceProxy(prefix + "/state/propositions", GetAttributeService) + _services["get_functions"] = \ + rospy.ServiceProxy(prefix + "/state/functions", + GetAttributeService) _services["query_knowledge_base"] = \ rospy.ServiceProxy(prefix + "/query_state", @@ -107,7 +110,7 @@ def _is_predicate_negative(name): return new_name, is_negative -def _make_instance(type_name, item_name): +def _make_kb_item_from_instance(type_name, item_name): kb_item = KnowledgeItem() kb_item.knowledge_type = KB_ITEM_INSTANCE @@ -116,7 +119,7 @@ def _make_instance(type_name, item_name): return kb_item -def _make_predicate(type_name, parameters): +def _make_fact(type_name, parameters): new_type_name, is_negative = _is_predicate_negative(type_name) kb_item = KnowledgeItem() @@ -127,26 +130,86 @@ def _make_predicate(type_name, parameters): return kb_item -def _make_kb_item(*args, **kwargs): +def _make_kb_item_from_predicate(*args, **kwargs): if len(args) == 1 and isinstance(args[0], KnowledgeItem): kb_item = args[0] + elif len(args) == 1 and isinstance(args[0], str): + type_name = args[0] + kb_item = _make_fact(type_name, kwargs) + elif len(args) == 2 and isinstance(args[0], str) \ and isinstance(args[1], OrderedDict): type_name = args[0] parameters = args[1] - kb_item = _make_predicate(type_name, parameters) + kb_item = _make_fact(type_name, parameters) else: + # This option of pass parameters as kwargs could not be used in python 2.7 + # due to a bug in ROSPlan, see: + # - https://github.com/KCL-Planning/ROSPlan/issues/253 + # - https://github.com/KCL-Planning/ROSPlan/issues/258 + # However, in python 3 this work because dictionary used by kwargs is an + # OrderedDict instead a normal Dict. + # + # type_name = args[0] + # kb_item = _make_predicate(type_name, kwargs) + + message = ("Parameters should be passed as OrderedDict instead as kwargs, due to a bug in ROSPlan. See:\n" + " - https://github.com/KCL-Planning/ROSPlan/issues/253\n" + " - https://github.com/KCL-Planning/ROSPlan/issues/253") + raise ValueError(message) + + return kb_item + + +def _make_function(type_name, parameters, value): + + new_type_name, is_negative = _is_predicate_negative(type_name) + kb_item = KnowledgeItem() + kb_item.knowledge_type = KB_ITEM_FUNCTION + kb_item.is_negative = is_negative + kb_item.attribute_name = new_type_name + kb_item.values = dict_to_keyval(parameters) + kb_item.function_value = value + return kb_item + + +def _make_kb_item_from_function(*args, **kwargs): + + if len(args) == 1 and isinstance(args[0], KnowledgeItem): + kb_item = args[0] + + elif (len(args) == 3 and isinstance(args[0], str) + and isinstance(args[1], OrderedDict) + and isinstance(args[2], float)): type_name = args[0] - kb_item = _make_predicate(type_name, kwargs) + parameters = args[1] + value = args[2] + kb_item = _make_function(type_name, parameters, value) + + else: + # This option of pass parameters as kwargs could not be used in python 2.7 + # due to a bug in ROSPlan, see: + # - https://github.com/KCL-Planning/ROSPlan/issues/253 + # - https://github.com/KCL-Planning/ROSPlan/issues/258 + # However, in python 3 this work because dictionary used by kwargs is an + # OrderedDict instead a normal Dict. + # + # type_name = args[0] + # kb_item = _make_predicate(type_name, kwargs) + + message = ("Parameters should be passed as OrderedDict instead as kwargs, due to a bug in ROSPlan. See:\n" + " - https://github.com/KCL-Planning/ROSPlan/issues/253\n" + " - https://github.com/KCL-Planning/ROSPlan/issues/253") + raise ValueError(message) return kb_item def _instance_exists(item_name): - instance_names = _services["get_current_instances"]("").instances + instance_names = _services["get_instances"]("").instances return item_name in instance_names @@ -165,7 +228,7 @@ def add_instance(item_name, type_name): raise TypeError return _services["update_knowledge_base"](KB_UPDATE_ADD_KNOWLEDGE, - _make_instance(type_name, item_name)).success + _make_kb_item_from_instance(type_name, item_name)).success def remove_instance(item_name, type_name): @@ -174,7 +237,7 @@ def remove_instance(item_name, type_name): raise TypeError return _services["update_knowledge_base"](KB_UPDATE_RM_KNOWLEDGE, - _make_instance(type_name, item_name)).success + _make_kb_item_from_instance(type_name, item_name)).success def get_instance_type(item_name): @@ -184,7 +247,7 @@ def get_instance_type(item_name): types = _services["get_domain_types"]().types for type_name in types: - instance_names = _services["get_current_instances"](type_name).instances + instance_names = _services["get_instances"](type_name).instances if item_name in instance_names: return type_name @@ -192,7 +255,7 @@ def get_instance_type(item_name): def list_instances(type_name=""): - instance_names = _services["get_current_instances"](type_name).instances + instance_names = _services["get_instances"](type_name).instances return instance_names @@ -203,12 +266,12 @@ def remove_all_instances(): def add_predicate(*args, **kwargs): - kb_item = _make_kb_item(*args, **kwargs) + kb_item = _make_kb_item_from_predicate(*args, **kwargs) return _services["update_knowledge_base"](KB_UPDATE_ADD_KNOWLEDGE, kb_item).success def remove_predicate(*args, **kwargs): - kb_item = _make_kb_item(*args, **kwargs) + kb_item = _make_kb_item_from_predicate(*args, **kwargs) return _services["update_knowledge_base"](KB_UPDATE_RM_KNOWLEDGE, kb_item).success @@ -222,18 +285,38 @@ def remove_all_predicates(): remove_predicate(predicate) +def add_function(*args, **kwargs): + kb_item = _make_kb_item_from_function(*args, **kwargs) + return _services["update_knowledge_base"](KB_UPDATE_ADD_KNOWLEDGE, kb_item).success + + +def remove_function(*args, **kwargs): + kb_item = _make_kb_item_from_function(*args, **kwargs) + return _services["update_knowledge_base"](KB_UPDATE_RM_KNOWLEDGE, kb_item).success + + +def list_functions(): + functions = _services["get_functions"]().attributes + return functions + + +def remove_all_functions(): + for function in list_functions(): + remove_function(function) + + def add_goal(*args, **kwargs): - kb_item = _make_kb_item(*args, **kwargs) + kb_item = _make_kb_item_from_predicate(*args, **kwargs) return _services["update_knowledge_base"](KB_UPDATE_ADD_GOAL, kb_item).success def remove_goal(*args, **kwargs): - kb_item = _make_kb_item(*args, **kwargs) + kb_item = _make_kb_item_from_predicate(*args, **kwargs) return _services["update_knowledge_base"](KB_UPDATE_RM_GOAL, kb_item).success def list_goals(): - goals = _services["get_current_goals"]("").attributes + goals = _services["get_goals"]("").attributes return goals @@ -249,3 +332,4 @@ def reset(): def remove_all(): _services["clear_knowledge"]() remove_all_predicates() + remove_all_functions() diff --git a/src/rosplan_pytools/rosplan/controller/planning_system.py b/src/rosplan_pytools/rosplan/controller/planning_system.py index ef8693e..bf51de7 100644 --- a/src/rosplan_pytools/rosplan/controller/planning_system.py +++ b/src/rosplan_pytools/rosplan/controller/planning_system.py @@ -7,6 +7,7 @@ from std_srvs.srv import Empty from rosplan_dispatch_msgs.srv import DispatchService + DEFAULT_PROBLEM_NODE_NAME = "/rosplan_problem_generator" DEFAULT_PLANNER_NODE_NAME = "/rosplan_plan_generator" DEFAULT_PARSER_NODE_NAME = "/rosplan_plan_parser" @@ -89,9 +90,5 @@ def plan(): _services["dispatch_plan"]() -def plan_and_wait(): - pass - - def cancel(): _services["cancel_dispatch"]() diff --git a/src/rosplan_pytools/rosplan/interfaces/action_interface.py b/src/rosplan_pytools/rosplan/interfaces/action_interface.py index 9466f1c..5c50f50 100644 --- a/src/rosplan_pytools/rosplan/interfaces/action_interface.py +++ b/src/rosplan_pytools/rosplan/interfaces/action_interface.py @@ -76,6 +76,8 @@ def _action_receiver(msg): else: action.execute(**keyval_to_dict(msg.parameters)) + rospy.loginfo("[RPpt][AIF] action '%s' completed" % action_name) + except Exception as e: rospy.logwarn("[RPpt][AIF] action '%s' failed." % msg.name, exc_info=1) feedback.publish(ActionFeedback(msg.action_id,