Merge branch 'main' of https://github.com/HPCL-EI/RoboWaiter

.gitignore

@@ -20,6 +20,8 @@ MANIFEST
 MO-VLN/
 GLIP/
 
+sub_task.ptml
+
 # PyInstaller
 #  Usually these files are written by a python script from a template
 #  before PyInstaller builds the exe, so as to inject date/other infos into it.

robowaiter/behavior_lib/_base/Act.py

@@ -7,8 +7,6 @@ class Act(Bahavior):
     def __init__(self,*args):
         super().__init__(*args)
 
-    def get_conds(self):
+    @classmethod
-        self.pre = set()
+    def get_info(self,*arg):
-        self.add = set()
+        return None
-        self.de = set()
-        return self.pre, self.add, self.de

robowaiter/behavior_lib/_base/Behavior.py

@@ -14,12 +14,27 @@ class Bahavior(ptree.behaviour.Behaviour):
     scene = None
     print_name_prefix = ""
 
+    @classmethod
+    def get_ins_name(cls,*args):
+        name = cls.__name__
+        if len(args) > 0:
+            ins_name = f'{name}({",".join(list(args))})'
+        else:
+            ins_name = f'{name}()'
+        return ins_name
+
     def __init__(self,*args):
         name = self.__class__.__name__
         if len(args)>0:
             name = f'{name}({",".join(list(args))})'
         self.name = name
+        #get valid args
+        # self.valid_arg_list = []
+        # lines = self.valid_params.strip().splitlines()
+        # for line in lines:
+        #     self.valid_arg_list.append((x.strip for x in line.split(",")))
         self.args = args
+
         super().__init__(self.name)
 
     def _update(self) -> ptree.common.Status:
@@ -28,7 +43,9 @@ class Bahavior(ptree.behaviour.Behaviour):
 
     @property
     def print_name(self):
-        return f'{self.print_name_prefix}{self.name}'
+        return f'{self.print_name_prefix}{self.get_ins_name(*self.args)}'
+
+
 
     # let behavior node interact with the scene
     def set_scene(self, scene):

robowaiter/behavior_lib/act/Make.py

@@ -0,0 +1,31 @@
+import py_trees as ptree
+from typing import Any
+from robowaiter.behavior_lib._base.Act import Act
+from robowaiter.behavior_lib._base.Behavior import Status
+
+class Make(Act):
+    can_be_expanded = True
+    num_args = 1
+    valid_args = (
+        "Coffee",
+    )
+
+    def __init__(self, *args):
+        super().__init__(*args)
+        self.target_obj = self.args[0]
+
+
+    @classmethod
+    def get_info(cls,arg):
+        info = None
+        if arg == "Coffee":
+            info = {
+                "add": {f'On(Coffee,Table)'},
+            }
+        return info
+    def _update(self) -> ptree.common.Status:
+        op_type = 1
+        self.scene.move_task_area(op_type)
+        self.scene.op_task_execute(op_type)
+        self.scene.state["condition_set"].add(self.add)
+        return Status.RUNNING

robowaiter/behavior_lib/act/MoveTo.py

@@ -4,9 +4,25 @@ from robowaiter.behavior_lib._base.Act import Act
 from robowaiter.algos.navigate.DstarLite.navigate import Navigator
 
 class MoveTo(Act):
+    can_be_expanded = True
+    num_args = 1
+    valid_args = (
+        "Bar",
+        "Table",
+    )
+
+    def __init__(self, target_place):
+        super().__init__(target_place)
+        self.target_place = target_place
+
+
+    @classmethod
+    def get_info(self,arg):
+        info = {
+            "add": {f'At(Robot,{arg})'},
+        }
+        return info
 
-    def __init__(self, *args):
-        super().__init__(*args)
 
     def _update(self) -> ptree.common.Status:
         # self.scene.test_move()

robowaiter/behavior_lib/cond/At.py

@@ -6,7 +6,6 @@ class At(Cond):
     can_be_expanded = True
     num_params = 2
     valid_params = '''
-        Coffee, Bar
         Robot, Bar
     '''
 

robowaiter/behavior_lib/cond/On.py

@@ -0,0 +1,28 @@
+import py_trees as ptree
+from typing import Any
+from robowaiter.behavior_lib._base.Cond import Cond
+
+class On(Cond):
+    can_be_expanded = True
+    num_params = 2
+    valid_params = '''
+        Robot, Bar
+    '''
+
+    def __init__(self,*args):
+        super().__init__(*args)
+
+
+    def _update(self) -> ptree.common.Status:
+        # if self.scene.status?
+        arg_str = self.arg_str
+
+        if f'At({arg_str})' in self.scene.state["condition_set"]:
+            return ptree.common.Status.SUCCESS
+        else:
+            return ptree.common.Status.FAILURE
+
+        # if self.scene.state['chat_list'] == []:
+        #     return ptree.common.Status.FAILURE
+        # else:
+        #     return ptree.common.Status.SUCCESS

robowaiter/robot/robot.py

@@ -1,7 +1,9 @@
 import io
 import contextlib
+import os
+import importlib.util
 
-from robowaiter.utils.bt.load import load_bt_from_ptml, find_node_by_name, print_tree_from_root
+from robowaiter.utils.bt.load import load_bt_from_ptml,find_node_by_name,print_tree_from_root
 from robowaiter.utils.bt.visitor import StatusVisitor
 
 from robowaiter.behavior_tree.obtea.OptimalBTExpansionAlgorithm import Action  # 调用最优行为树扩展算法
@@ -9,13 +11,13 @@ from robowaiter.behavior_tree.obtea.opt_bt_exp_main import BTOptExpInterface
 
 from robowaiter.behavior_lib.act.DelSubTree import DelSubTree
 from robowaiter.behavior_lib._base.Sequence import Sequence
-
+from robowaiter.utils.bt.load import load_behavior_tree_lib
 
 class Robot(object):
     scene = None
     response_frequency = 1
 
-    def __init__(self, ptml_path, behavior_lib_path):
+    def __init__(self,ptml_path,behavior_lib_path):
         self.ptml_path = ptml_path
         self.behavior_lib_path = behavior_lib_path
 
@@ -24,12 +26,13 @@ class Robot(object):
         self.last_tick_output = ""
         self.action_list = None
 
-    def set_scene(self, scene):
+
+    def set_scene(self,scene):
         self.scene = scene
 
     def load_BT(self):
-        self.bt = load_bt_from_ptml(self.scene, self.ptml_path, self.behavior_lib_path)
+        self.bt = load_bt_from_ptml(self.scene, self.ptml_path,self.behavior_lib_path)
-        sub_task_place_holder = find_node_by_name(self.bt.root, "SubTaskPlaceHolder")
+        sub_task_place_holder = find_node_by_name(self.bt.root,"SubTaskPlaceHolder")
         if sub_task_place_holder:
             sub_task_seq = sub_task_place_holder.parent
             sub_task_seq.children.pop()
@@ -38,10 +41,16 @@ class Robot(object):
         self.bt_visitor = StatusVisitor()
         self.bt.visitors.append(self.bt_visitor)
 
-    def expand_sub_task_tree(self, goal):
+
+    def expand_sub_task_tree(self,goal):
         if self.action_list is None:
             self.action_list = self.collect_action_nodes()
-            print(f"首次运行行为树扩展算法，收集到{len(self.action_list)}个有效动作")
+            print("\n--------------------")
+            print(f"首次运行行为树扩展算法，收集到{len(self.action_list)}个有效动作:")
+            for a in self.action_list:
+                print(a.name)
+            print("--------------------\n")
+
 
         algo = BTOptExpInterface(self.action_list)
 
@@ -52,7 +61,7 @@ class Robot(object):
         with open(file_path, 'w') as file:
             file.write(ptml_string)
 
-        sub_task_bt = load_bt_from_ptml(self.scene, file_path, self.behavior_lib_path)
+        sub_task_bt = load_bt_from_ptml(self.scene, file_path,self.behavior_lib_path)
 
         # 加入删除子树的节点
         seq = Sequence(name="Sequence", memory=False)
@@ -65,19 +74,30 @@ class Robot(object):
         print("当前行为树为：")
         print_tree_from_root(self.bt.root)
 
+    # 获取所有action的pre,add,del列表
     def collect_action_nodes(self):
-        action_list = [
+        action_list = []
-            Action(name='MakeCoffee()', pre={'At(Robot,CoffeeMachine)'},
+        behavior_dict = load_behavior_tree_lib()
-                   add={'At(Coffee,Bar)'}, del_set=set(), cost=1),
+        for cls in behavior_dict["act"].values():
-            Action(name='MoveTo(Table)', pre={''},
+            if cls.can_be_expanded:
-                   add={'At(Robot,Table)'}, del_set=set(), cost=1),
+                if cls.num_args == 0:
-            Action(name='MoveTo(WaterTable)', pre={''},
+                    action_list.append(Action(name=cls.get_ins_name(),**cls.get_info()))
-                   add={'At(Robot,WaterTable)'}, del_set=set(), cost=1),
+                if cls.num_args == 1:
-            Action(name='ExploreEnv()', pre={'At(Robot,Bar)'},
+                    for arg in cls.valid_args:
-                   add={'EnvExplored()'}, del_set=set(), cost=1),
+                        action_list.append(Action(name=cls.get_ins_name(arg), **cls.get_info(arg)))
-            Action(name='PourWater()', pre={'At(Robot,WaterTable)'},
+                if cls.num_args > 1:
-                   add={'At(Water,WaterTable)'}, del_set=set(), cost=1),
+                    for args in cls.valid_args:
-        ]
+                        action_list.append(Action(name=cls.get_ins_name(*args),**cls.get_info(*args)))
+
+        print(action_list)
+        # action_list = [
+        #     Action(name='MakeCoffee', pre={'At(Robot,CoffeeMachine)'},
+        #            add={'At(Coffee,Bar)'}, del_set=set(), cost=1),
+        #     Action(name='MoveTo(Table)', pre={'At(Robot,Bar)'},
+        #            add={'At(Robot,Table)'}, del_set=set(), cost=1),
+        #     Action(name='ExploreEnv()', pre=set(),
+        #            add={'EnvExplored()'}, del_set=set(), cost=1),
+        # ]
         return action_list
 
     def step(self):
@@ -96,6 +116,5 @@ class Robot(object):
                 print("\n")
                 self.last_tick_output = bt_output
 
-
 if __name__ == '__main__':
     pass

robowaiter/utils/__init__.py

@@ -2,11 +2,4 @@ import os
 
 from robowaiter.utils import *
 from robowaiter.utils import *
-
+from robowaiter.utils.basic import get_root_path
-
-
-
-def get_root_path():
-    return os.path.abspath(
-        os.path.join(__file__, "../../..")
-    )

robowaiter/utils/basic.py

@@ -0,0 +1,6 @@
+import os
+
+def get_root_path():
+    return os.path.abspath(
+        os.path.join(__file__, "../../..")
+    )

robowaiter/utils/bt/load.py

@@ -1,10 +1,12 @@
 import py_trees as ptree
 from robowaiter.behavior_tree.ptml import ptmlCompiler
-
+import os
+import importlib.util
+from robowaiter.utils.basic import get_root_path
 
 def load_bt_from_ptml(scene, ptml_path, behavior_lib_path):
     ptml_bt = ptmlCompiler.load(scene, ptml_path, behavior_lib_path)
-    bt = ptree.trees.BehaviourTree(ptml_bt)
+    bt =  ptree.trees.BehaviourTree(ptml_bt)
 
     with open(ptml_path, 'r') as f:
         ptml = f.read()
@@ -29,7 +31,6 @@ def print_tree_from_root(node, indent=0):
         for child in node.children:
             print_tree_from_root(child, indent + 1)
 
-
 def find_node_by_name(tree, name):
     """
     Find a node in the behavior tree with the specified name.
@@ -47,6 +48,42 @@ def find_node_by_name(tree, name):
                 return result
     return None
 
+
+
+def get_classes_from_folder(folder_path):
+    cls_dict = {}
+    for filename in os.listdir(folder_path):
+        if filename.endswith('.py'):
+            # 构建模块的完整路径
+            module_path = os.path.join(folder_path, filename)
+            # 获取模块名（不含.py扩展名）
+            module_name = os.path.splitext(filename)[0]
+
+            # 动态导入模块
+            spec = importlib.util.spec_from_file_location(module_name, module_path)
+            module = importlib.util.module_from_spec(spec)
+            spec.loader.exec_module(module)
+
+            # 获取模块中定义的所有类
+            for name, obj in module.__dict__.items():
+                if isinstance(obj, type):
+                    cls_dict[module_name] = obj
+
+    return cls_dict
+
+
+def load_behavior_tree_lib():
+    root_path = get_root_path()
+    type_list = ["act","cond"]
+    behavior_dict = {}
+    for type in type_list:
+        path = os.path.join(root_path,"robowaiter","behavior_lib",type)
+        behavior_dict[type] = get_classes_from_folder(path)
+
+    return behavior_dict
+
+if __name__ == '__main__':
+    print(load_behavior_tree_lib())
 # class BehaviorTree(ptree):
 #     def __init__(self):
 #         super().__init__()

sub_task.ptml

@@ -1,11 +1,3 @@
 selector{
-cond At(Water,WaterTable)
+cond EnvExplored()
-sequence{
-cond At(Robot,WaterTable)
-act PourWater()
-}
-sequence{
-cond 
-act MoveTo(WaterTable)
-}
 }