From 9a3143cafcfa42395c0989421164e9359f6f2e12 Mon Sep 17 00:00:00 2001
From: ChenXL97 <908926798@qq.com>
Date: Mon, 13 Nov 2023 14:53:00 +0800
Subject: [PATCH 1/3] =?UTF-8?q?=E7=8E=B0=E5=9C=A8=E5=8F=AF=E4=BB=A5?=
 =?UTF-8?q?=E8=87=AA=E5=8A=A8=E7=94=9F=E6=88=90=E8=A1=8C=E4=B8=BA=E6=A0=91?=
 =?UTF-8?q?=E6=89=A9=E5=B1=95=E5=8A=A8=E4=BD=9C?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 robowaiter/behavior_lib/_base/Act.py          |  9 ++---
 robowaiter/behavior_lib/_base/Behavior.py     |  6 +++
 .../act/{MakeCoffee.py => Make.py}            | 24 ++++++++----
 robowaiter/behavior_lib/act/MoveTo.py         | 20 +++++++++-
 robowaiter/behavior_lib/cond/At.py            |  1 -
 robowaiter/behavior_lib/cond/On.py            | 28 ++++++++++++++
 robowaiter/robot/robot.py                     | 35 ++++++++++++-----
 robowaiter/utils/__init__.py                  |  9 +----
 robowaiter/utils/basic.py                     |  6 +++
 robowaiter/utils/bt/load.py                   | 38 ++++++++++++++++++-
 sub_task.ptml                                 |  4 --
 11 files changed, 143 insertions(+), 37 deletions(-)
 rename robowaiter/behavior_lib/act/{MakeCoffee.py => Make.py} (54%)
 create mode 100644 robowaiter/behavior_lib/cond/On.py
 create mode 100644 robowaiter/utils/basic.py

diff --git a/robowaiter/behavior_lib/_base/Act.py b/robowaiter/behavior_lib/_base/Act.py
index b402ed9..34ee91c 100644
--- a/robowaiter/behavior_lib/_base/Act.py
+++ b/robowaiter/behavior_lib/_base/Act.py
@@ -7,8 +7,7 @@ class Act(Bahavior):
     def __init__(self,*args):
         super().__init__(*args)
 
-    def get_conds(self):
-        pre = set()
-        add = set()
-        de = set()
-        return pre, add, de
\ No newline at end of file
+    @classmethod
+    def get_info(self,*arg):
+        return None
+
diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py
index 2a99f03..129be1d 100644
--- a/robowaiter/behavior_lib/_base/Behavior.py
+++ b/robowaiter/behavior_lib/_base/Behavior.py
@@ -19,7 +19,13 @@ class Bahavior(ptree.behaviour.Behaviour):
         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:
diff --git a/robowaiter/behavior_lib/act/MakeCoffee.py b/robowaiter/behavior_lib/act/Make.py
similarity index 54%
rename from robowaiter/behavior_lib/act/MakeCoffee.py
rename to robowaiter/behavior_lib/act/Make.py
index d4903fe..2d0be6c 100644
--- a/robowaiter/behavior_lib/act/MakeCoffee.py
+++ b/robowaiter/behavior_lib/act/Make.py
@@ -3,17 +3,27 @@ from typing import Any
 from robowaiter.behavior_lib._base.Act import Act
 from robowaiter.behavior_lib._base.Behavior import Status
 
-class MakeCoffee(Act):
+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
 
-    @property
-    def cond_sets(self):
-        pre = {"At(Robot,Bar)"}
-        add = {"At(Coffee,Bar)"}
-        de = {}
-        return pre,add,de
 
     def _update(self) -> ptree.common.Status:
         op_type = 1
diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py
index e01eb53..000869d 100644
--- a/robowaiter/behavior_lib/act/MoveTo.py
+++ b/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()
diff --git a/robowaiter/behavior_lib/cond/At.py b/robowaiter/behavior_lib/cond/At.py
index a660621..1c88605 100644
--- a/robowaiter/behavior_lib/cond/At.py
+++ b/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
     '''
 
diff --git a/robowaiter/behavior_lib/cond/On.py b/robowaiter/behavior_lib/cond/On.py
new file mode 100644
index 0000000..e679cfd
--- /dev/null
+++ b/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
diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py
index 880d55b..8133fcb 100644
--- a/robowaiter/robot/robot.py
+++ b/robowaiter/robot/robot.py
@@ -1,5 +1,7 @@
 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.visitor import StatusVisitor
@@ -9,7 +11,7 @@ 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
@@ -67,15 +69,30 @@ class Robot(object):
         print("当前行为树为：")
         print_tree_from_root(self.bt.root)
 
+    # 获取所有action的pre,add,del列表
     def collect_action_nodes(self):
-        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={'At(Robot,Bar)'},
-                   add={'EnvExplored()'}, del_set=set(), cost=1),
-        ]
+        action_list = []
+        behavior_dict = load_behavior_tree_lib()
+        for cls in behavior_dict["act"].values():
+            if cls.can_be_expanded:
+                if cls.num_args == 0:
+                    action_list.append(Action(name=cls.__name__,**cls.get_info()))
+                if cls.num_args == 1:
+                    for arg in cls.valid_args:
+                        action_list.append(Action(name=cls.__name__, **cls.get_info(arg)))
+                if cls.num_args > 1:
+                    for args in cls.valid_args:
+                        action_list.append(Action(name=cls.__name__,**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):
diff --git a/robowaiter/utils/__init__.py b/robowaiter/utils/__init__.py
index 5b043f1..119bc84 100644
--- a/robowaiter/utils/__init__.py
+++ b/robowaiter/utils/__init__.py
@@ -2,11 +2,4 @@ import os
 
 from robowaiter.utils import *
 from robowaiter.utils import *
-
-
-
-
-def get_root_path():
-    return os.path.abspath(
-        os.path.join(__file__, "../../..")
-    )
+from robowaiter.utils.basic import get_root_path
\ No newline at end of file
diff --git a/robowaiter/utils/basic.py b/robowaiter/utils/basic.py
new file mode 100644
index 0000000..2d7c60d
--- /dev/null
+++ b/robowaiter/utils/basic.py
@@ -0,0 +1,6 @@
+import os
+
+def get_root_path():
+    return os.path.abspath(
+        os.path.join(__file__, "../../..")
+    )
diff --git a/robowaiter/utils/bt/load.py b/robowaiter/utils/bt/load.py
index cb7f61a..bf90ea9 100644
--- a/robowaiter/utils/bt/load.py
+++ b/robowaiter/utils/bt/load.py
@@ -1,6 +1,8 @@
 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)
@@ -48,6 +50,40 @@ def find_node_by_name(tree, name):
 
 
 
+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__()
\ No newline at end of file
diff --git a/sub_task.ptml b/sub_task.ptml
index 1b4d81e..d9cb899 100644
--- a/sub_task.ptml
+++ b/sub_task.ptml
@@ -1,7 +1,3 @@
 selector{
 cond EnvExplored()
-selector{
-cond At(Robot,Bar)
-act ExploreEnv()
-}
 }

From 2d933bf7a42d4ce9726fa3406e3a95d587cb81c6 Mon Sep 17 00:00:00 2001
From: ChenXL97 <908926798@qq.com>
Date: Mon, 13 Nov 2023 15:05:21 +0800
Subject: [PATCH 2/3] =?UTF-8?q?=E7=8E=B0=E5=9C=A8=E6=97=A0=E5=8F=82?=
 =?UTF-8?q?=E6=95=B0=E8=8A=82=E7=82=B9=E6=89=93=E5=8D=B0=E6=97=B6=E4=B9=9F?=
 =?UTF-8?q?=E4=BC=9A=E5=B8=A6=E7=A9=BA=E6=8B=AC=E5=8F=B7()?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 robowaiter/behavior_lib/_base/Behavior.py | 13 ++++++++++++-
 robowaiter/robot/robot.py                 | 13 +++++++++----
 2 files changed, 21 insertions(+), 5 deletions(-)

diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py
index 129be1d..84ed74b 100644
--- a/robowaiter/behavior_lib/_base/Behavior.py
+++ b/robowaiter/behavior_lib/_base/Behavior.py
@@ -14,6 +14,15 @@ 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:
@@ -34,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):
diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py
index 8133fcb..6d7b275 100644
--- a/robowaiter/robot/robot.py
+++ b/robowaiter/robot/robot.py
@@ -45,7 +45,12 @@ class Robot(object):
     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)
 
@@ -76,13 +81,13 @@ class Robot(object):
         for cls in behavior_dict["act"].values():
             if cls.can_be_expanded:
                 if cls.num_args == 0:
-                    action_list.append(Action(name=cls.__name__,**cls.get_info()))
+                    action_list.append(Action(name=cls.get_ins_name(),**cls.get_info()))
                 if cls.num_args == 1:
                     for arg in cls.valid_args:
-                        action_list.append(Action(name=cls.__name__, **cls.get_info(arg)))
+                        action_list.append(Action(name=cls.get_ins_name(arg), **cls.get_info(arg)))
                 if cls.num_args > 1:
                     for args in cls.valid_args:
-                        action_list.append(Action(name=cls.__name__,**cls.get_info(*args)))
+                        action_list.append(Action(name=cls.get_ins_name(*args),**cls.get_info(*args)))
 
         print(action_list)
         # action_list = [

From f43abea0ac7a077af660a774c0df5dafe3cf1674 Mon Sep 17 00:00:00 2001
From: ChenXL97 <908926798@qq.com>
Date: Mon, 13 Nov 2023 15:36:07 +0800
Subject: [PATCH 3/3] =?UTF-8?q?=E5=B0=86sub=5Ftask.ptml=E5=8A=A0=E5=85=A5g?=
 =?UTF-8?q?itignore?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .gitignore | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.gitignore b/.gitignore
index 9ed45df..85e33e4 100644
--- a/.gitignore
+++ b/.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.