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. diff --git a/robowaiter/behavior_lib/_base/Act.py b/robowaiter/behavior_lib/_base/Act.py index 244a060..ee948d5 100644 --- a/robowaiter/behavior_lib/_base/Act.py +++ b/robowaiter/behavior_lib/_base/Act.py @@ -7,8 +7,6 @@ class Act(Bahavior): def __init__(self,*args): super().__init__(*args) - def get_conds(self): - self.pre = set() - self.add = set() - self.de = set() - return self.pre, self.add, self.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..84ed74b 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/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): diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py new file mode 100644 index 0000000..8388cc2 --- /dev/null +++ b/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 \ No newline at end of file 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 313e562..6d7b275 100644 --- a/robowaiter/robot/robot.py +++ b/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) - sub_task_place_holder = find_node_by_name(self.bt.root, "SubTaskPlaceHolder") + 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") 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(name='MakeCoffee()', pre={'At(Robot,CoffeeMachine)'}, - add={'At(Coffee,Bar)'}, del_set=set(), cost=1), - Action(name='MoveTo(Table)', pre={''}, - add={'At(Robot,Table)'}, del_set=set(), cost=1), - Action(name='MoveTo(WaterTable)', pre={''}, - add={'At(Robot,WaterTable)'}, del_set=set(), cost=1), - Action(name='ExploreEnv()', pre={'At(Robot,Bar)'}, - add={'EnvExplored()'}, del_set=set(), cost=1), - Action(name='PourWater()', pre={'At(Robot,WaterTable)'}, - add={'At(Water,WaterTable)'}, 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.get_ins_name(),**cls.get_info())) + if cls.num_args == 1: + for arg in cls.valid_args: + 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.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 + pass \ No newline at end of file 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 72e829c..bf90ea9 100644 --- a/robowaiter/utils/bt/load.py +++ b/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__() +# super().__init__() \ No newline at end of file diff --git a/sub_task.ptml b/sub_task.ptml index f534388..d9cb899 100644 --- a/sub_task.ptml +++ b/sub_task.ptml @@ -1,11 +1,3 @@ selector{ -cond At(Water,WaterTable) -sequence{ -cond At(Robot,WaterTable) -act PourWater() -} -sequence{ -cond -act MoveTo(WaterTable) -} +cond EnvExplored() }