From a5a4a1badce81cb46636d038ad381250da1fa307 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Thu, 16 Nov 2023 22:48:54 +0800 Subject: [PATCH 1/5] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=A3=80=E6=B5=8B?= =?UTF-8?q?=E8=A1=8C=E4=BA=BA=E5=88=B0=20Bar=20=E5=89=8D=E5=B9=B6=E8=87=AA?= =?UTF-8?q?=E5=8A=A8=20=E6=89=93=E6=8B=9B=E5=91=BC=20=E7=9A=84=E6=9D=A1?= =?UTF-8?q?=E4=BB=B6=E8=8A=82=E7=82=B9=E5=92=8C=E5=8A=A8=E4=BD=9C=E8=8A=82?= =?UTF-8?q?=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/act/ExploreEnv.py | 5 +- robowaiter/behavior_lib/act/GreatCustomer.py | 25 ++++++++ .../behavior_lib/cond/DetectCustomer.py | 31 ++++++++++ robowaiter/robot/Default.ptml | 11 +++- robowaiter/scene/scene.py | 8 --- robowaiter/scene/tasks/VLM/VLM2.py | 60 +------------------ 6 files changed, 71 insertions(+), 69 deletions(-) create mode 100644 robowaiter/behavior_lib/act/GreatCustomer.py create mode 100644 robowaiter/behavior_lib/cond/DetectCustomer.py 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..ce186ed --- /dev/null +++ b/robowaiter/behavior_lib/act/GreatCustomer.py @@ -0,0 +1,25 @@ +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: + + self.scene.chat_bubble("欢迎光临！请问有什么可以帮您？") + return ptree.common.Status.RUNNING 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/scene/scene.py b/robowaiter/scene/scene.py index e0d18b2..567ba9d 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -700,11 +700,3 @@ 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) - # def test_yaw(self): - # walk_v = [247.0, 480.0, 180.0, 180, 0] - # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - # scene = stub.Do(action) - # time.sleep(4) - # walk_v = [247.0, 500.0, 0.0, 180, 0] - # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - # scene = stub.Do(action) \ No newline at end of file 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 From c748810ec2dabab9fcce522712473c8315bef944 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Thu, 16 Nov 2023 23:05:11 +0800 Subject: [PATCH 2/5] =?UTF-8?q?=E8=A7=A3=E5=86=B3=E4=BA=86=E9=85=B8?= =?UTF-8?q?=E5=A5=B6=E6=8E=89=E5=9C=B0=E4=B8=8A=E7=9A=84bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/_base/Behavior.py | 3 +-- robowaiter/behavior_lib/act/GreatCustomer.py | 2 ++ robowaiter/behavior_lib/act/Make.py | 1 + robowaiter/behavior_lib/act/MoveTo.py | 2 +- robowaiter/behavior_lib/act/PutDown.py | 2 +- 5 files changed, 6 insertions(+), 4 deletions(-) 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/GreatCustomer.py b/robowaiter/behavior_lib/act/GreatCustomer.py index ce186ed..5ac8844 100644 --- a/robowaiter/behavior_lib/act/GreatCustomer.py +++ b/robowaiter/behavior_lib/act/GreatCustomer.py @@ -21,5 +21,7 @@ class GreatCustomer(Act): 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 From 40e1959d1b88d9adcfba9cfdc3767481cb528ad9 Mon Sep 17 00:00:00 2001 From: liwang zhang <2638950452@qq.com> Date: Fri, 17 Nov 2023 09:42:09 +0800 Subject: [PATCH 3/5] =?UTF-8?q?=E6=B7=BB=E5=8A=A0map=E6=8E=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/scene/scene.py | 188 ++++++++++++++++++++------------------ 1 file changed, 97 insertions(+), 91 deletions(-) diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index ee888fe..f4389d3 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -1,3 +1,4 @@ +import pickle import sys import time import grpc @@ -9,9 +10,6 @@ import math from robowaiter.proto import GrabSim_pb2 from robowaiter.proto import GrabSim_pb2_grpc - - - channel = grpc.insecure_channel( "localhost:30001", options=[ @@ -32,13 +30,13 @@ def init_world(scene_num=1, mapID=11): def show_image(camera_data): print('------------------show_image----------------------') - #获取第0张照片 + # 获取第0张照片 im = camera_data.images[0] - #使用numpy(np) 数值类型矩阵的frombuffer，将im.data以流的形式（向量的形式）读入，在变型reshape成三位矩阵的形式(长度，宽度，深度)即三阶张量 + # 使用numpy(np) 数值类型矩阵的frombuffer，将im.data以流的形式（向量的形式）读入，在变型reshape成三位矩阵的形式(长度，宽度，深度)即三阶张量 d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels)) - #matplotlib中的plt方法 对矩阵d 进行图形绘制，如果 深度相机拍摄的带深度的图片（图片名字中有depth信息），则转换成黑白图即灰度图 + # matplotlib中的plt方法 对矩阵d 进行图形绘制，如果 深度相机拍摄的带深度的图片（图片名字中有depth信息），则转换成黑白图即灰度图 plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None) - #图像展示在屏幕上 + # 图像展示在屏幕上 plt.show() return d @@ -58,10 +56,10 @@ class Scene: "sub_goal_list": [], # 子目标列表 "status": None, # 仿真器中的观测信息，见下方详细解释 "condition_set": {'At(Robot,Bar)', 'Is(AC,Off)', - 'Holding(Nothing)', - # 'Holding(Yogurt)' - 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)', - 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'} + 'Holding(Nothing)', + # 'Holding(Yogurt)' + 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)', + 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'} } """ status: @@ -76,7 +74,7 @@ class Scene: collision: str, info: str """ - def __init__(self,robot=None, sceneID=0): + def __init__(self, robot=None, sceneID=0): self.sceneID = sceneID self.use_offset = False self.start_time = time.time() @@ -91,26 +89,29 @@ class Scene: # myx op # 1-7 正常执行, 8-10 控灯操作移动到6, 11-12窗帘操作不需要移动, - self.op_dialog = ["","制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7 - "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12 - "调整空调开关","调高空调温度","调低空调温度", # 13-15 - "抓握物体","放置物体"] # 16-17 - self.op_act_num = [0,3,4,6,3,2,0,1, - 0,0,0,0,0, - 0,0,0, - 0,0] - self.op_v_list = [[0.0,0.0],[250.0, 310.0],[-70.0, 480.0],[250.0, 630.0],[-70.0, 740.0],[260.0, 1120.0],[300.0, -220.0], + self.op_dialog = ["", "制作咖啡", "倒水", "夹点心", "拖地", "擦桌子", "开筒灯", "搬椅子", # 1-7 + "关筒灯", "开大厅灯", "关大厅灯", "关闭窗帘", "打开窗帘", # 8-12 + "调整空调开关", "调高空调温度", "调低空调温度", # 13-15 + "抓握物体", "放置物体"] # 16-17 + self.op_act_num = [0, 3, 4, 6, 3, 2, 0, 1, + 0, 0, 0, 0, 0, + 0, 0, 0, + 0, 0] + self.op_v_list = [[0.0, 0.0], [250.0, 310.0], [-70.0, 480.0], [250.0, 630.0], [-70.0, 740.0], [260.0, 1120.0], + [300.0, -220.0], [0.0, -70.0]] - self.op_typeToAct = {8:[6,2],9:[6,3],10:[6,4],11:[8,1],12:[8,2]} + self.op_typeToAct = {8: [6, 2], 9: [6, 3], 10: [6, 4], 11: [8, 1], 12: [8, 2]} # 空调面板位置 - self.obj_loc = [300.5, -140.0,114] + self.obj_loc = [300.5, -140.0, 114] # AEM self.visited = set() self.all_frontier_list = set() self.semantic_map = semantic_map self.auto_map = np.ones((800, 1550)) - + self.filename = "../proto/map_1.pkl" + with open(self.filename, 'rb') as file: + self.map_file = pickle.load(file) def reset(self): # 基类reset，默认执行仿真器初始化操作 @@ -141,15 +142,15 @@ class Scene: self.robot.step() def deal_event(self): - if len(self.event_list)>0: + if len(self.event_list) > 0: next_event = self.event_list[0] - t,func = next_event + t, func = next_event if self.time >= t: print(f'event: {t}, {func.__name__}') self.event_list.pop(0) func() - def create_chat_event(self,sentence): + def create_chat_event(self, sentence): def customer_say(): print(f'顾客说：{sentence}') if self.show_bubble: @@ -165,7 +166,7 @@ class Scene: def reset_sim(self): # reset world init_world() - + stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID)) def _reset(self): @@ -180,10 +181,6 @@ class Scene: # 场景自定义的step pass - - - - def walker_control_generator(self, walkerID, autowalk, speed, X, Y, Yaw): if self.use_offset: X, Y = X + loc_offset[0], Y + loc_offset[1] @@ -195,7 +192,7 @@ class Scene: ) def walk_to(self, X, Y, Yaw=100, velocity=200, dis_limit=0): - walk_v = [X,Y,Yaw,velocity,dis_limit] + walk_v = [X, Y, Yaw, velocity, dis_limit] action = GrabSim_pb2.Action( scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v ) @@ -203,7 +200,6 @@ class Scene: return scene - def reachable_check(self, X, Y, Yaw): if self.use_offset: X, Y = X + loc_offset[0], Y + loc_offset[1] @@ -219,11 +215,11 @@ class Scene: else: return True - def add_walkers(self,walker_loc=[[0, 880], [250, 1200], [-55, 750], [70, -200]]): + def add_walkers(self, walker_loc=[[0, 880], [250, 1200], [-55, 750], [70, -200]]): print('------------------add_walkers----------------------') walker_list = [] for i in range(len(walker_loc)): - loc = walker_loc[i] + [0,0, 100] + loc = walker_loc[i] + [0, 0, 100] action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc) scene = stub.Do(action) print(scene.info) @@ -383,13 +379,14 @@ class Scene: def ik_control_joints(self, handNum=2, x=30, y=40, z=80): # print('------------------ik_control_joints----------------------') # IK控制,双手, 1左手, 2右手; 暂时只动右手 - HandPostureObject = [GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=handNum, x=x, y=y, z=z, roll=0, pitch=0, yaw=0), - # GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=1, x=0, y=0, z=0, roll=0, pitch=0, yaw=0), - ] - temp = stub.GetIKControlInfos(GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject)) + HandPostureObject = [ + GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=handNum, x=x, y=y, z=z, roll=0, pitch=0, yaw=0), + # GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=1, x=0, y=0, z=0, roll=0, pitch=0, yaw=0), + ] + temp = stub.GetIKControlInfos( + GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject)) - - def move_to_obj(self,obj_id): + def move_to_obj(self, obj_id): scene = self.status obj_info = scene.objects[obj_id] # Robot @@ -399,28 +396,27 @@ class Scene: walk_v = [obj_x + 40, obj_y - 35, 130, 180, 0] obj_x += 3 obj_y += 2.5 - walk_v[0]+=1 - print("walk:",walk_v) + walk_v[0] += 1 + print("walk:", walk_v) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = stub.Do(action) print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw]) - # 移动到进行操作任务的指定地点 - def move_task_area(self,op_type,obj_id=0, release_pos=[247.0, 520.0, 100.0]): + def move_task_area(self, op_type, obj_id=0, release_pos=[247.0, 520.0, 100.0]): scene = self.status cur_pos = [scene.location.X, scene.location.Y, scene.rotation.Yaw] print("Current Position:", cur_pos, "开始任务:", self.op_dialog[op_type]) - if op_type==11 or op_type==12: # 开关窗帘不需要移动 + if op_type == 11 or op_type == 12: # 开关窗帘不需要移动 return print('------------------moveTo_Area----------------------') if op_type < 8: - walk_v = self.op_v_list[op_type] + [scene.rotation.Yaw, 180, 0] # 动画控制 - print("walk_v:",walk_v) - if op_type>=8 and op_type<=10: walk_v = self.op_v_list[6] + [scene.rotation.Yaw, 180, 0] # 控灯 - if op_type in [13,14,15]: walk_v = [240, -140.0] + [0, 180, 0] # 空调 - if op_type==16: # 抓握物体，移动到物体周围的可达区域 + walk_v = self.op_v_list[op_type] + [scene.rotation.Yaw, 180, 0] # 动画控制 + print("walk_v:", walk_v) + if op_type >= 8 and op_type <= 10: walk_v = self.op_v_list[6] + [scene.rotation.Yaw, 180, 0] # 控灯 + if op_type in [13, 14, 15]: walk_v = [240, -140.0] + [0, 180, 0] # 空调 + if op_type == 16: # 抓握物体，移动到物体周围的可达区域 scene = self.status obj_info = scene.objects[obj_id] # Robot @@ -430,11 +426,11 @@ class Scene: walk_v = [obj_x + 40, obj_y - 35, 130, 180, 0] obj_x += 3 obj_y += 2.5 - if op_type==17: # 放置物体，移动到物体周围的可达区域 + if op_type == 17: # 放置物体，移动到物体周围的可达区域 walk_v = release_pos[:-1] + [180, 180, 0] if release_pos == [340.0, 900.0, 99.0]: walk_v[2] = 130 - print("walk_v:",walk_v) + print("walk_v:", walk_v) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = stub.Do(action) print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw]) @@ -453,12 +449,12 @@ class Scene: print(scene.info) return False - def adjust_kongtiao(self,op_type): - print("self.obj_loc:",self.obj_loc) + def adjust_kongtiao(self, op_type): + print("self.obj_loc:", self.obj_loc) obj_loc = self.obj_loc[:] - print("obj_loc:",obj_loc,"self.obj_loc:", self.obj_loc) + print("obj_loc:", obj_loc, "self.obj_loc:", self.obj_loc) obj_loc[2] -= 5 - print("obj_loc:",obj_loc) + print("obj_loc:", obj_loc) if op_type == 13: obj_loc[1] -= 2 if op_type == 14: obj_loc[1] -= 0 if op_type == 15: obj_loc[1] += 2 @@ -467,42 +463,44 @@ class Scene: self.robo_recover() return True - def gen_obj(self,h=100): + def gen_obj(self, h=100): # 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典 14:cake 61:甜牛奶 scene = self.status ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z] - obj_list = [GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 40, z = 95, roll=0, pitch=0, yaw=0, type=5), - # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z=h, roll=0, pitch=0, yaw=0, type=9), - GrabSim_pb2.ObjectList.Object(x=340, y=960, z = 88, roll=0, pitch=0, yaw=0, type=9), - ] + obj_list = [ + GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 40, z=95, roll=0, pitch=0, yaw=0, + type=5), + # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z=h, roll=0, pitch=0, yaw=0, type=9), + GrabSim_pb2.ObjectList.Object(x=340, y=960, z=88, roll=0, pitch=0, yaw=0, type=9), + ] scene = stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID)) time.sleep(1.0) - def grasp_obj(self,obj_id,hand_id=1): + def grasp_obj(self, obj_id, hand_id=1): scene = self.status obj_info = scene.objects[obj_id] obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z - if obj_info.name=="CoffeeCup": + if obj_info.name == "CoffeeCup": pass - if obj_info.name=="Glass": + if obj_info.name == "Glass": pass # Finger - self.ik_control_joints(2, obj_x-9, obj_y, obj_z) # -10, 0, 0 + self.ik_control_joints(2, obj_x - 9, obj_y, obj_z) # -10, 0, 0 time.sleep(3.0) # Grasp Obj print('------------------grasp_obj----------------------') - action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Grasp, values=[hand_id, obj_id]) + action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Grasp, + values=[hand_id, obj_id]) scene = stub.Do(action) time.sleep(3.0) return True # robot的肢体恢复原位 def robo_recover(self): - action = GrabSim_pb2.Action(scene=self.sceneID,action=GrabSim_pb2.Action.ActionType.RotateJoints, # 恢复原位 + action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, # 恢复原位 values=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) scene = stub.Do(action) - def robo_stoop_parallel(self): # 0-3是躯干，4-6是脖子和头，7-13是左胳膊，14-20是右胳膊 scene = self.status @@ -510,14 +508,14 @@ class Scene: angle[0] = 15 angle[19] = -15 angle[20] = -30 - action = GrabSim_pb2.Action(scene=self.sceneID,action=GrabSim_pb2.Action.ActionType.RotateJoints, # 弯腰 + action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, # 弯腰 values=angle) scene = stub.Do(action) time.sleep(1.0) - def release_obj(self,release_pos): + def release_obj(self, release_pos): print("------------------release_obj----------------------") - if release_pos==[340.0, 900.0, 99.0]: + if release_pos == [340.0, 900.0, 99.0]: self.ik_control_joints(2, 300.0, 935, release_pos[2]) time.sleep(2.0) else: @@ -533,30 +531,31 @@ class Scene: return True # 执行过程：输出"开始(任务名)" -> 按步骤数执行任务 -> Robot输出成功或失败的对话 - 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: result = self.control_robot_action(op_type, 1) - if op_type>=8 and op_type<=12: 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降温 + 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: result = self.control_robot_action(op_type, 1) + if op_type >= 8 and op_type <= 12: 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) - if op_type ==16: # 抓握物体 + if op_type == 16: # 抓握物体 result = self.grasp_obj(obj_id) - if op_type ==17: # 放置物体 + if op_type == 17: # 放置物体 result = self.release_obj(release_pos) self.control_robot_action(0, 2) if result: - if self.op_act_num[op_type]>0: - for i in range(2,2+self.op_act_num[op_type]): - self.control_robot_action(op_type,i) + if self.op_act_num[op_type] > 0: + for i in range(2, 2 + self.op_act_num[op_type]): + self.control_robot_action(op_type, i) self.control_robot_action(0, 2) # self.control_robot_action(0, 1, "成功"+self.op_dialog[op_type]) # else: # self.control_robot_action(0, 1, self.op_dialog[op_type]+"失败") - def find_obj(self,name): + def find_obj(self, name): for id, item in enumerate(self.status.objects): if item.name == name: - print("name:",name,"id:",id,"X:",item.location.X,"Y:",item.location.Y,"Z:",item.location.Z,) + print("name:", name, "id:", id, "X:", item.location.X, "Y:", item.location.Y, "Z:", item.location.Z, ) def test_move(self): v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]] @@ -584,7 +583,8 @@ class Scene: print("walk_v", walk_v) action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = stub.Do(action) - cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, objs_name_set) + cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, + objs_name_set) # if scene.info == "Unreachable": print(scene.info) return cur_objs, objs_name_set @@ -669,11 +669,9 @@ class Scene: frontier_best = frontier return frontier_best - - - def cal_distance_to_robot(self,objx,objy,objz): + def cal_distance_to_robot(self, objx, objy, objz): scene = self.status - ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y),100] + 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) # def test_yaw(self): @@ -683,4 +681,12 @@ class Scene: # time.sleep(4) # walk_v = [247.0, 500.0, 0.0, 180, 0] # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - # scene = stub.Do(action) \ No newline at end of file + # scene = stub.Do(action) + + # 读取map文件判断是否可达 + def reachable(self, pos): + x, y = self.real2map(pos[0], pos[1]) + if self.map_file[x, y] == 1: + return False + else: + return True From 956203300ba905cd8109d035c7a7bf44d8b45f49 Mon Sep 17 00:00:00 2001 From: liwang zhang <2638950452@qq.com> Date: Fri, 17 Nov 2023 09:57:58 +0800 Subject: [PATCH 4/5] =?UTF-8?q?=E6=B7=BB=E5=8A=A0map=E6=8E=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/scene/scene.py | 26 +++++++++++++++++++------- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 567ba9d..110e9e0 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -1,3 +1,4 @@ +import pickle import sys import time import grpc @@ -110,21 +111,24 @@ class Scene: self.all_frontier_list = set() self.semantic_map = semantic_map self.auto_map = np.ones((800, 1550)) + self.filename = "../proto/map_1.pkl" + with open(self.filename, 'rb') as file: + self.map_file = pickle.load(file) def reset(self): - # 基类reset，默认执行仿真器初始化操作 - self.reset_sim() + # 基类reset，默认执行仿真器初始化操作 + self.reset_sim() - # reset state - self.state = self.default_state + # reset state + self.state = self.default_state - print("场景初始化完成") - self._reset() + print("场景初始化完成") + self._reset() - self.running = True + self.running = True def run(self): # 基类run @@ -700,3 +704,11 @@ 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 + From b899599b042a06e900a4378f1adf6c19f6d9b449 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Fri, 17 Nov 2023 12:53:16 +0800 Subject: [PATCH 5/5] =?UTF-8?q?=E6=9B=B4=E6=96=B0=20AEM=20=E5=9C=B0?= =?UTF-8?q?=E5=9B=BE=E8=B7=AF=E5=BE=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/robot/robot.py | 1 + robowaiter/scene/scene.py | 23 ++++++++++++----------- 2 files changed, 13 insertions(+), 11 deletions(-) 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 110e9e0..c625a9a 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -10,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", @@ -111,24 +112,24 @@ class Scene: self.all_frontier_list = set() self.semantic_map = semantic_map self.auto_map = np.ones((800, 1550)) - self.filename = "../proto/map_1.pkl" + 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): - # 基类reset，默认执行仿真器初始化操作 - self.reset_sim() + # 基类reset，默认执行仿真器初始化操作 + self.reset_sim() - # reset state - self.state = self.default_state + # reset state + self.state = self.default_state - print("场景初始化完成") - self._reset() + print("场景初始化完成") + self._reset() - self.running = True + self.running = True def run(self): # 基类run @@ -180,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