KB uses OrderedDict for parameters and work with numeric fluents (functions).

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()

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()

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,15 +247,15 @@ 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
 
     return ""
 
 
 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()

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"]()

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,