diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 92eb9d4..8d5460e 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -29,8 +29,7 @@ class Bahavior(ptree.behaviour.Behaviour): 'Bar2': (240.0, 40.0, 70.0), 'WaterTable':(-70.0, 500.0, 107), 'CoffeeTable':(250.0, 310.0, 100.0), - 'Table1': (340.0, 900.0, 98.0), - # 'Table1': (345.0, 895.0, 98.0), + 'Table1': (340.0, 900.0, 99.0), 'Table2': (-55.0, 0.0, 107), 'Table3':(-55.0, 150.0, 107) } diff --git a/robowaiter/behavior_lib/act/ExploreEnv.py b/robowaiter/behavior_lib/act/ExploreEnv.py index 3b832b1..8179d2b 100644 --- a/robowaiter/behavior_lib/act/ExploreEnv.py +++ b/robowaiter/behavior_lib/act/ExploreEnv.py @@ -3,7 +3,8 @@ from typing import Any from robowaiter.behavior_lib._base.Act import Act class ExploreEnv(Act): - can_be_expanded = True + # can_be_expanded = True + can_be_expanded = False num_args=0 valid_args=() @@ -20,8 +21,6 @@ class ExploreEnv(Act): def _update(self) -> ptree.common.Status: # explore algorithm - - self.scene.state["condition_set"]|= self.info["add"] return ptree.common.Status.RUNNING diff --git a/robowaiter/behavior_lib/act/GreatCustomer.py b/robowaiter/behavior_lib/act/GreatCustomer.py new file mode 100644 index 0000000..5ac8844 --- /dev/null +++ b/robowaiter/behavior_lib/act/GreatCustomer.py @@ -0,0 +1,27 @@ +import py_trees as ptree +from robowaiter.behavior_lib._base.Act import Act +from robowaiter.algos.navigator.navigate import Navigator + +class GreatCustomer(Act): + can_be_expanded = False + num_args = 0 + valid_args = () + + def __init__(self, *args): + super().__init__(*args) + + @classmethod + def get_info(cls): + info = {} + info['pre'] = set() + info["add"] = set() + info["del_set"] = set() + info['cost']=0 + return info + + def _update(self) -> ptree.common.Status: + + goal = Act.place_xyz_dic['Bar'] + self.scene.walk_to(goal[0]-5,goal[1], 180, 180, 0) + self.scene.chat_bubble("欢迎光临!请问有什么可以帮您?") + return ptree.common.Status.RUNNING diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index 88f828e..f3e9b38 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -34,6 +34,7 @@ class Make(Act): info["add"] |= {f'On({arg},WaterTable)'} elif arg == cls.valid_args[2]: info["add"] |= {f'On({arg},Bar)'} + info['cost'] = 10 return info def _update(self) -> ptree.common.Status: diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index 6c31ad5..3ca9167 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -21,7 +21,7 @@ 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']=10 + info['cost']=5 return info diff --git a/robowaiter/behavior_lib/act/PutDown.py b/robowaiter/behavior_lib/act/PutDown.py index 7187a69..48723ca 100644 --- a/robowaiter/behavior_lib/act/PutDown.py +++ b/robowaiter/behavior_lib/act/PutDown.py @@ -23,7 +23,7 @@ class PutDown(Act): info["add"] = {f'Holding(Nothing)',f'On({arg[0]},{arg[1]})'} info["del_set"] = {f'Holding({arg[0]})'} - info['cost'] = 100 + info['cost'] = 1 return info diff --git a/robowaiter/behavior_lib/cond/DetectCustomer.py b/robowaiter/behavior_lib/cond/DetectCustomer.py new file mode 100644 index 0000000..76d2232 --- /dev/null +++ b/robowaiter/behavior_lib/cond/DetectCustomer.py @@ -0,0 +1,31 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Cond import Cond +import itertools + +class DetectCustomer(Cond): + can_be_expanded = False + num_params = 0 + valid_args = () + + def __init__(self,*args): + super().__init__(*args) + + + def _update(self) -> ptree.common.Status: + + # 获取customer的位置 + # bar (247.0, 520.0, 100.0) + close_to_bar = False + scene = self.scene.status + for walker in scene.walkers: + x, y, yaw = walker.pose.X, walker.pose.Y, walker.pose.Yaw + # 到达一定区域就打招呼 + if y >= 450 and y <= 620 and x >= 40 and x <= 100 and yaw>=-10 and yaw <=10: + close_to_bar = True + break + + if close_to_bar: + return ptree.common.Status.SUCCESS + else: + return ptree.common.Status.FAILURE \ No newline at end of file diff --git a/robowaiter/robot/Default.ptml b/robowaiter/robot/Default.ptml index 62a677c..1a1c2b2 100644 --- a/robowaiter/robot/Default.ptml +++ b/robowaiter/robot/Default.ptml @@ -14,5 +14,14 @@ selector act SubTaskPlaceHolder() } } - + sequence + { + selector + { + cond At(Robot,Bar) + act MoveTo(Bar) + } + cond DetectCustomer() + act GreatCustomer + } } \ No newline at end of file diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index 85d135a..bc79121 100644 --- a/robowaiter/robot/robot.py +++ b/robowaiter/robot/robot.py @@ -37,6 +37,7 @@ class Robot(object): 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()") diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index b69c219..98d2041 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -1,3 +1,4 @@ +import pickle import sys import time import grpc @@ -9,8 +10,9 @@ import math from robowaiter.proto import GrabSim_pb2 from robowaiter.proto import GrabSim_pb2_grpc - - +import os +from robowaiter.utils import get_root_path +root_path = get_root_path() channel = grpc.insecure_channel( "localhost:30001", @@ -110,6 +112,9 @@ class Scene: self.all_frontier_list = set() self.semantic_map = semantic_map self.auto_map = np.ones((800, 1550)) + self.filename = os.path.join(root_path, 'robowaiter/proto/map_1.pkl') + with open(self.filename, 'rb') as file: + self.map_file = pickle.load(file) def reset(self): @@ -176,8 +181,8 @@ class Scene: def reset_sim(self): # reset world init_world() - stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID)) + stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) def _reset(self): # 场景自定义的reset @@ -709,4 +714,13 @@ class Scene: ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y),100] return math.sqrt((ginger_x - objx) ** 2 + (ginger_y - objy) ** 2 + (ginger_z - objz) ** 2) + # 根据map文件判断是否可达 + def reachable(self, pos): + x, y = self.real2map(pos[0], pos[1]) + if self.map_file[x, y] == 0: + return True + else: + return False + + diff --git a/robowaiter/scene/tasks/VLM/VLM2.py b/robowaiter/scene/tasks/VLM/VLM2.py index fa3595f..7a13acc 100644 --- a/robowaiter/scene/tasks/VLM/VLM2.py +++ b/robowaiter/scene/tasks/VLM/VLM2.py @@ -11,28 +11,11 @@ class SceneVLM(Scene): super().__init__(robot) # 在这里加入场景中发生的事件, (事件发生的时间,事件函数) self.event_list = [ - # (5, self.create_chat_event("测试VLM:做一杯咖啡")), - # (5, self.create_chat_event("测试VLM:倒一杯水")), - # (5, self.create_chat_event("测试VLM:开空调")), - # (5, self.create_chat_event("测试VLM:关空调")), - # (5, self.create_chat_event("测试VLM:开大厅灯")), - # (5, self.create_chat_event("测试VLM:拖地")), - # (7, self.create_chat_event("测试VLM:擦桌子")), - # (5, self.create_chat_event("测试VLM:整理椅子")), - # (5, self.create_chat_event("测试VLM:把冰红茶放到Table2")), - # (5, self.create_chat_event("测试VLM:关大厅灯")) - # (5, self.create_chat_event("测试VLM:做一杯咖啡放到吧台上")), - # (5, self.create_chat_event("测试VLM:做一杯咖啡放到水杯桌上并倒水")), - # (8, self.create_chat_event("测试VLN:前往1号桌")), ] def _reset(self): - - # self.gen_obj(type=5) - # self.gen_obj(type=9) - # self.op_task_execute(op_type=16, obj_id=0) - # self.move_task_area(op_type=4) + self.gen_obj() pass def _run(self, op_type=10): @@ -42,17 +25,13 @@ class SceneVLM(Scene): # 带领行人去有太阳的地方 # 行人说 有点热 # 好的,这就去开空调 - - scene = self.add_walkers([[47, 920],[70,-200]]) + scene = self.add_walkers([[47, 920]]) self.control_walker( - [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=47, Y=520, Yaw=0)]) + [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=0)]) cont = scene.walkers[0].name+":请问可以带我去空位上嘛?我想晒太阳" self.control_robot_action(0,3,cont) - # self.clean_walker() - - # 共17个操作 # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7 # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12 @@ -73,39 +52,6 @@ class SceneVLM(Scene): # self.move_task_area(op_type, release_pos=pos) # self.op_task_execute(op_type, release_pos=pos) # [325.0, 860.0, 100] - # 流程测试 - # 抓握放置:抓吧台前生成的酸奶,放到抹布桌上 - self.gen_obj() - # self.move_task_area(16, obj_id=0) - # self.op_task_execute(16, obj_id=0) - # pos = [340.0, 900.0, 99.0] - # self.move_task_area(17, release_pos=pos) - # self.op_task_execute(17, release_pos=pos) - # - # # 做咖啡:做完的咖啡放到水杯桌上 - # self.move_task_area(1) - # self.op_task_execute(1) - # - # self.find_obj("CoffeeCup") - # - # self.move_task_area(16, obj_id=275) - # self.op_task_execute(16, obj_id=275) - # pos = [-70.0, 500.0, 107] - # self.move_task_area(17, release_pos=pos) - # self.op_task_execute(17, release_pos=pos) - # - # # 倒水:倒完的水放到旁边桌子上 - # self.move_task_area(2) - # self.op_task_execute(2) - - # - # self.move_task_area(16, obj_id=190) - # self.op_task_execute(16, obj_id=190) - # pos = [-55.0, 0.0, 107] - # self.move_task_area(17, release_pos=pos) - # self.op_task_execute(17, release_pos=pos) - - # self.test_yaw() pass