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

更新了动作节点

This commit is contained in:
Caiyishuai 2023-11-22 17:55:57 +08:00
parent 0b8ca87c08
commit e8c5cb559f
13 changed files with 178 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
robowaiter/behavior_lib/act/FreeHands.py Normal file
View File

@ -0,0 +1,34 @@
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 FreeHands(Act):
can_be_expanded = True
num_args = 0
valid_args = set()
def __init__(self, *args):
super().__init__(*args)
@classmethod
def get_info(cls):
info = {}
info["pre"]= set()
info['add'] = {f'Holding(Nothing)'}
info['del_set'] = {f'Holding({obj})' for obj in cls.all_object}
info['cost'] = 0
return info
def _update(self) -> ptree.common.Status:
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio,update_info_count=1)
self.scene.state["condition_set"] |= (self.info["add"])
self.scene.state["condition_set"] -= self.info["del_set"]
return Status.RUNNING

3
robowaiter/behavior_lib/act/Make.py
View File

@ -39,6 +39,9 @@ class Make(Act):
def _update(self) -> ptree.common.Status:
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
self.scene.move_task_area(self.op_type)
self.scene.op_task_execute(self.op_type)

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

@ -21,7 +21,11 @@ class MoveTo(Act):
info['pre'] |= {f'Exist({arg})'}
info["add"] = {f'At(Robot,{arg})'}
info["del_set"] = {f'At(Robot,{place})' for place in cls.valid_args if place != arg}
info['cost']=5
info['cost'] = 5
# if arg!='Anything':
# info['cost']=5
# else:
# info['cost']=0
return info

10
robowaiter/behavior_lib/act/PickUp.py
View File

@ -7,6 +7,7 @@ class PickUp(Act):
can_be_expanded = True
num_args = 1
valid_args = Act.all_object
# valid_args.add("Anything")
def __init__(self, *args):
super().__init__(*args)
self.target_obj = self.args[0]
@ -20,6 +21,15 @@ class PickUp(Act):
info["del_set"] = {f'Holding(Nothing)'}
for place in cls.valid_args:
info["del_set"] |= {f'On({arg},{place})'}
info['cost'] = 1
# if arg != 'Anything':
# info['cost'] = 1
# else:
# info['cost'] = 0
#
# info["pre"] = {}
return info

8
robowaiter/behavior_lib/act/PutDown.py
View File

@ -24,6 +24,14 @@ class PutDown(Act):
info["del_set"] = {f'Holding({arg[0]})'}
info['cost'] = 1
# if arg[0]!='Anything':
# info['cost'] = 1
# else:
# info['cost'] = 0
# info["pre"] = {}
# info["add"] = {f'Holding(Nothing)'}
# info["del_set"] = {f'Holding({obj})' for obj in cls.valid_args if obj[0] != arg}
return info

3
robowaiter/behavior_lib/act/Turn.py
View File

@ -74,6 +74,9 @@ class Turn(Act):
def _update(self) -> ptree.common.Status:
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
self.scene.move_task_area(self.op_type)
self.scene.op_task_execute(self.op_type)
if self.scene.take_picture:

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

@ -16,6 +16,7 @@ class NewCustomer(Cond):
if self.scene.take_picture:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
# 获取customer的位置
# bar (247.0, 520.0, 100.0)
close_to_bar = False

44
robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py
View File

@ -37,6 +37,17 @@ def state_transition(state,action):
return new_state
def conflict(c):
have_at = False
for str in c:
if 'At' in str:
if not have_at:
have_at = True
else:
return True
return False
#本文所提出的完备规划算法
class OptBTExpAlgorithm:
def __init__(self,verbose=False):
@ -51,7 +62,8 @@ class OptBTExpAlgorithm:
def clear(self):
self.bt = None
self.nodes = []
self.traversed = []
self.traversed = [] #存cond
self.expanded = [] #存整个
self.conditions = []
self.conditions_index = []
@ -77,7 +89,6 @@ class OptBTExpAlgorithm:
self.nodes.append(copy.deepcopy(cond_anc_pair)) # the set of explored but unexpanded conditions
self.traversed = [goal] # the set of expanded conditions
while len(self.nodes)!=0:
# Find the condition for the shortest cost path
@ -85,6 +96,19 @@ class OptBTExpAlgorithm:
min_cost = float ('inf')
index= -1
for i,cond_anc_pair in enumerate(self.nodes):
########### 剪枝操作
# cond_tmp = cond_anc_pair.cond_leaf.content
# valid = True
# for pn in self.expanded: # 剪枝操作
# if isinstance(pn.act_leaf.content,Action):
# if pn.act_leaf.content.name==cond_anc_pair.act_leaf.content.name and cond_tmp <= pn.cond_leaf.content:
# valid = False
# break
# if not valid:
# continue
########### 剪枝操作
if cond_anc_pair.cond_leaf.mincost < min_cost:
min_cost = cond_anc_pair.cond_leaf.mincost
pair_node = copy.deepcopy(cond_anc_pair)
@ -100,10 +124,22 @@ class OptBTExpAlgorithm:
# Mount the action node and extend BT. T = Eapand(T,c,A(c))
if c!=goal:
if c!=set():
# 挂在上去的时候判断要不要挂载
########### 剪枝操作 发现行不通
# valid = True
# for pn in self.expanded: # 剪枝操作
# if isinstance(pn.act_leaf.content,Action):
# if pn.act_leaf.content.name==pair_node.act_leaf.content.name and c <= pn.cond_leaf.content:
# valid = False
# break
# if valid:
########### 剪枝操作
sequence_structure = ControlBT(type='>')
sequence_structure.add_child(
[copy.deepcopy(pair_node.cond_leaf), copy.deepcopy(pair_node.act_leaf)])
subtree.add_child([copy.deepcopy(sequence_structure)]) # subtree 是回不断变化的，它的父亲是self.bt
self.expanded.append(copy.deepcopy(pair_node))
# 增加实时条件判断，满足条件就不再扩展
if c <= self.scene.state["condition_set"]:
return True
@ -128,6 +164,10 @@ class OptBTExpAlgorithm:
print("———— 满足条件可以扩展")
c_attr = (actions[i].pre | c) - actions[i].add
# 这样剪枝存在错误性
if conflict(c_attr):
continue
# 剪枝操作,现在的条件是以前扩展过的条件的超集
valid = True
for j in self.traversed: # 剪枝操作

16
robowaiter/scene/scene.py
View File

@ -78,8 +78,8 @@ class Scene:
"sub_goal_list": [], # 子目标列表
"status": None, # 仿真器中的观测信息，见下方详细解释
"condition_set": {'At(Robot,Bar)', 'Is(AC,Off)',
'Holding(Nothing)', 'Exist(Yogurt)', 'Exist(BottledDrink)', 'On(Yogurt,Bar)',
'On(BottledDrink,Bar)',
'Holding(Nothing)', 'Exist(Yogurt)', 'Exist(BottledDrink)',
# 'On(Yogurt,Bar)','On(BottledDrink,Bar)',
# 'Exist(Softdrink)', 'On(Softdrink,Table1)',
'Exist(VacuumCup)', 'On(VacuumCup,Table2)',
'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
@ -116,7 +116,7 @@ class Scene:
self.show_bubble = True
# 图像分割
self.take_picture = True
self.take_picture = False
self.map_ratio = 5
self.map_map = np.zeros((math.ceil(950 / self.map_ratio), math.ceil(1850 / self.map_ratio)))
self.db = DBSCAN(eps=self.map_ratio, min_samples=int(self.map_ratio / 2))
@ -705,10 +705,16 @@ class Scene:
return
print('------------------moveTo_Area----------------------')
if op_type < 8: # 动画控制相关任务的移动目标
if self.take_picture:
self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
walk_v = self.op_v_list[op_type] + [scene.rotation.Yaw, 180, 0]
if 8 <= op_type <= 10: # 控灯相关任务的移动目标
if self.take_picture:
self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
walk_v = self.op_v_list[6] + [scene.rotation.Yaw, 180, 0]
if op_type in [13, 14, 15]: # 空调相关任务的移动目标
if self.take_picture:
self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
walk_v = [240, -140.0] + [0, 180, 0]
if op_type == 16: # 抓握物体，移动到物体周围的可达区域
scene = self.status
@ -887,8 +893,12 @@ class Scene:
def op_task_execute(self, op_type, obj_id=0, release_pos=[247.0, 520.0, 100.0]):
self.control_robot_action(0, 1, "开始" + self.op_dialog[op_type]) # 输出正在执行的任务
if op_type < 8:
if self.take_picture:
self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
result = self.control_robot_action(op_type, 1)
if 8 <= op_type <= 12:
if self.take_picture:
self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
result = self.control_robot_action(self.op_typeToAct[op_type][0], self.op_typeToAct[op_type][1])
if op_type in [13, 14, 15]: # 调整空调:13代表按开关,14升温,15降温
result = self.adjust_kongtiao(op_type)

6
robowaiter/scene/tasks/CafeDailyOperations/CafeOneDay.py
View File

@ -35,8 +35,8 @@ class SceneVLM(Scene):
# # # 上述准备
(10, self.add_walker, (26, -28, -150, 90)),
(0, self.add_walker, (10, -70, -200, -45)),
(6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
(10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])),
(5, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
(10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])), #6
# 20 胖胖男到了 BrightTable6
(2, self.customer_say, (1, "咖啡有哪些呢？")), # 10
(2, self.customer_say, (1, "来杯卡布奇诺吧。")), # 15
@ -47,7 +47,7 @@ class SceneVLM(Scene):
(0, self.control_walker, (5, True, 50, 250, 1200, 180)), # 设置id=4 的2小男孩随机游走红随机游走
(0, self.add_walker, (48, 60, 520, 0)), # 生成他妈妈
(0, self.add_walkers, ([[[48, 60, 520, 0], [31, 60, 600, -90], [20, 60, 680, -90], [9, 60, 760, -90]]])),
(38, self.customer_say, (7, "哎呦，今天这么多人，还有空位吗？")), # 女士问
(38, self.customer_say, (7, "哎呦，今天这么多人，还有空位吗？")), # 女士问 38
(10, self.customer_say, (7, "我带着孩子呢，想要宽敞亮堂的地方。")), # 女士问
(8, self.customer_say, (7, "大厅的桌子好啊，快带我去呀！")),
(15, self.control_walker, (7, False, 50, -250, 480, 0)), # #290, 400

100
robowaiter/scene/tasks/CafeDailyOperations/VLN_all.py
View File

@ -13,7 +13,7 @@ class SceneVLM(Scene):
super().__init__(robot)
# 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
self.scene_flag = 1
self.scene_flag = 2
self.st1 = 3
# self.st2 = self.st1 + 30
# self.st3 = self.st2 + 65
@ -24,58 +24,58 @@ class SceneVLM(Scene):
self.signal_event_list = [
# 场景1：带小女孩找阳光下的空位
(3, self.add_walker, (5, 230, 1200)), # 0号"Girl02_C_3"
(1, self.control_walker, (0, False, 200, 60, 520, 0)),
(9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")),
(1, self.customer_say, (0, "可以带我过去嘛？")),
(13, self.control_walker, (0, False, 50, 140, 1200, 180)), # 小女孩站在了 BrightTable1 旁边就餐啦
# # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
# (3, self.add_walker, (5, 230, 1200)), # 小女孩
# # # 上述准备
(10, self.add_walker, (26, -28, -150, 90)),
(0, self.add_walker, (10, -70, -200, -45)),
(6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
(10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])),
# 20 胖胖男到了 BrightTable6
(2, self.customer_say, (1, "咖啡有哪些呢？")), # 10
(2, self.customer_say, (1, "来杯卡布奇诺吧。")), # 15
# # 场景3：有位女士要杯水和冰红茶
(3, self.add_walkers,
([[[21, 65, 1000, -90], [32, -80, 850, 135], [1, 60, 420, 135], [29, -290, 400, 180]]])),
(0, self.control_walker, (5, True, 50, 250, 1200, 180)), # 设置id=4 的2小男孩随机游走红随机游走
(0, self.add_walker, (48, 60, 520, 0)), # 生成他妈妈
(0, self.add_walkers, ([[[48, 60, 520, 0], [31, 60, 600, -90], [20, 60, 680, -90], [9, 60, 760, -90]]])),
(43, self.customer_say, (7, "哎呦，今天这么多人，还有空位吗？")), # 女士问
(10, self.customer_say, (7, "我带着孩子呢，想要宽敞亮堂的地方。")), # 女士问
(8, self.customer_say, (7, "大厅的桌子好啊，快带我去呀！")),
(15, self.control_walker, (7, False, 50, -250, 480, 0)), # #290, 400
(3, self.customer_say, (7, "来杯酸奶吧。")), # (3, self.customer_say, (7, "我想来杯水，帮我孩子拿个酸奶吧。")),
# # ### 9号灰色男 排队排着排着，不排了
(0, self.control_walker, (10, False, 100, 100, 760, 180)),
(0, self.control_walker, (10, True, 100, 0, 0, 180)),
(90, self.customer_say, (7, "谢谢你的水和酸奶！")), # 倒水+取放酸奶 90s
(10, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])),
(5, self.customer_say, (8,"你可以帮我拿来吗，我在前门的桌子前等你。")),
(1, self.control_walker,(8, False, 80, -10, 520, 90)),# 红女士在吧台前后退一步
(1, self.control_walker, (8, False, 80, 240, 1000, -45)), # 红女士走到Table1前
(1, self.control_walker, (9, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
(2, self.control_walker, (10, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
(6, self.customer_say, (8,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
(5, self.customer_say, (8, "不用了。")), # 红女士在Table1前
(8, self.remove_walkers, ([[0, 7, 8]])),
(3, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
(2, self.customer_say, (6, "好热呀！太阳也好大！")),
(20, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
# (3, self.add_walker, (5, 230, 1200)), # 0号"Girl02_C_3"
# (1, self.control_walker, (0, False, 200, 60, 520, 0)),
# (9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")),
# (1, self.customer_say, (0, "可以带我过去嘛？")),
# (13, self.control_walker, (0, False, 50, 140, 1200, 180)), # 小女孩站在了 BrightTable1 旁边就餐啦
#
# # # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
# # (3, self.add_walker, (5, 230, 1200)), # 小女孩
# # # # 上述准备
# (10, self.add_walker, (26, -28, -150, 90)),
# (0, self.add_walker, (10, -70, -200, -45)),
# (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
# (10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])),
# # 20 胖胖男到了 BrightTable6
# (2, self.customer_say, (1, "咖啡有哪些呢？")), # 10
# (2, self.customer_say, (1, "来杯卡布奇诺吧。")), # 15
#
# # # 场景3：有位女士要杯水和冰红茶
# (3, self.add_walkers,
# ([[[21, 65, 1000, -90], [32, -80, 850, 135], [1, 60, 420, 135], [29, -290, 400, 180]]])),
# (0, self.control_walker, (5, True, 50, 250, 1200, 180)), # 设置id=4 的2小男孩随机游走红随机游走
# (0, self.add_walker, (48, 60, 520, 0)), # 生成他妈妈
# (0, self.add_walkers, ([[[48, 60, 520, 0], [31, 60, 600, -90], [20, 60, 680, -90], [9, 60, 760, -90]]])),
# (38, self.customer_say, (7, "哎呦，今天这么多人，还有空位吗？")), # 女士问
# (10, self.customer_say, (7, "我带着孩子呢，想要宽敞亮堂的地方。")), # 女士问
# (8, self.customer_say, (7, "大厅的桌子好啊，快带我去呀！")),
# (15, self.control_walker, (7, False, 50, -250, 480, 0)), # #290, 400
# (3, self.customer_say, (7, "我想来杯水，帮我孩子拿个酸奶吧。")),
# # # ### 9号灰色男 排队排着排着，不排了
# (0, self.control_walker, (10, False, 100, 100, 760, 180)),
# (0, self.control_walker, (10, True, 100, 0, 0, 180)),
# (90, self.customer_say, (7, "谢谢你的水和酸奶！")), # 倒水+取放酸奶 90s
#
#
# (10, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])),
# (5, self.customer_say, (8,"你可以帮我拿来吗，我在前门的桌子前等你。")),
# (1, self.control_walker,(8, False, 80, -10, 520, 90)),# 红女士在吧台前后退一步
# (1, self.control_walker, (8, False, 80, 240, 1000, -45)), # 红女士走到Table1前
# (1, self.control_walker, (9, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
# (2, self.control_walker, (10, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
# (6, self.customer_say, (8,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
# (5, self.customer_say, (8, "不用了。")), # 红女士在Table1前
#
#
# (8, self.remove_walkers, ([[0, 7, 8]])),
# (3, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
# (2, self.customer_say, (6, "好热呀！太阳也好大！")),
# (20, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
# # 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
(24, self.clean_walkers, ()),
(3, self.clean_walkers, ()),
(1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
(3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),

2
robowaiter/scene/tasks/OT/OT_coffee_water_dessert.py
View File

@ -11,7 +11,7 @@ class SceneVLM(Scene):
super().__init__(robot)
# 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
self.signal_event_list = [
(5, self.customer_say, (5, "给我来杯咖啡，哦对，再倒一杯水。")),
(8, self.customer_say, (5, "给我来杯咖啡，哦对，再倒一杯水。")),
(1, self.control_walker_ls,([[[5, False, 100, -250, 480, 0],[6, False, 100, 60, 520, 0]]])),
(-1, self.customer_say, (5, "感谢，这些够啦，你去忙吧。")),
(10, self.customer_say, (6, "我想来份点心和酸奶。")),

8
robowaiter/scene/tasks/OT/Open_tasks_test.py
View File

@ -18,13 +18,15 @@ class SceneOT(Scene):
super().__init__(robot)
# 在这里加入场景中发生的事件
self.signal_event_list = [
(3, self.customer_say, ("System", "酸奶。")),
# (3, self.customer_say, ("System","来一号桌")),
# (-1, self.customer_say, ("System","回去吧")),
# (5, self.set_goal("At(Robot,BrightTable4)"))
]
self.event_list = [
(3, self.set_goal("On(VacuumCup,Bar)"))
]
# self.event_list = [
# # (3, self.set_goal("On(VacuumCup,Bar)"))
# (3, self.set_goal("On(Yogurt,Bar)"))
# ]
def _reset(self):
# self.add_walkers([[0, 880], [250, 1200]])