EmbodiedAI
/
RoboWaiter
mirror of https://github.com/HPCL-EI/RoboWaiter.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Caiyishuai 2023-11-13 15:43:23 +08:00
parent 806847ac15 f43abea0ac
commit d5f8c289bf
12 changed files with 191 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -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.

8
robowaiter/behavior_lib/_base/Act.py
View File

@ -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
@classmethod
def get_info(self,*arg):
return None

19
robowaiter/behavior_lib/_base/Behavior.py
View File

@ -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):

31
robowaiter/behavior_lib/act/Make.py Normal file
View File

@ -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

20
robowaiter/behavior_lib/act/MoveTo.py
View File

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

1
robowaiter/behavior_lib/cond/At.py
View File

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

28
robowaiter/behavior_lib/cond/On.py Normal file
View File

@ -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

65
robowaiter/robot/robot.py
View File

@ -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

9
robowaiter/utils/__init__.py
View File

@ -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

6
robowaiter/utils/basic.py Normal file
View File

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

45
robowaiter/utils/bt/load.py
View File

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

10
sub_task.ptml
View File

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