diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 6477fae..635a22c 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -41,7 +41,7 @@ class Bahavior(ptree.behaviour.Behaviour): 'QuietTable1':(480,1300,90), 'QuietTable2':(250,-240,-65), - 'BrightTable1':(230,1200,135), + 'BrightTable1':(230,1200,-135), 'BrightTable2': (65, 1000, 135), 'BrightTable3': (-80, 850, 135), 'BrightTable4': (-270, 520, 150), diff --git a/robowaiter/behavior_lib/act/DealChat.py b/robowaiter/behavior_lib/act/DealChat.py index 1efed4c..f39cb75 100644 --- a/robowaiter/behavior_lib/act/DealChat.py +++ b/robowaiter/behavior_lib/act/DealChat.py @@ -14,7 +14,12 @@ class DealChat(Act): super().__init__() self.chat_history = "" self.function_success = False - self.func_map = {"create_sub_task": self.create_sub_task, "get_object_info": self.get_object_info, "find_location": self.find_location} + self.func_map = { + "create_sub_task": self.create_sub_task, + "get_object_info": self.get_object_info, + "stop_serve": self.stop_serve, + "find_location": self.find_location + } def _update(self) -> ptree.common.Status: # if self.scene.status? @@ -29,14 +34,18 @@ class DealChat(Act): history = self.scene.state["chat_history"][name] self.scene.state["attention"]["customer"] = name - self.scene.state["serve_state"] = {"last_chat_time": self.scene.time, } + self.scene.state["serve_state"] = { + "last_chat_time": self.scene.time, + } - function_call, response = ask_llm(sentence, history, func_map=self.func_map) + function_call, response = ask_llm(sentence,history,func_map=self.func_map) - self.scene.chat_bubble(response) # 机器人输出对话 + + self.scene.chat_bubble(response) # 机器人输出对话 return ptree.common.Status.RUNNING + def create_sub_task(self, **args): try: goal = args['goal'] @@ -54,7 +63,7 @@ class DealChat(Act): self.scene.robot.expand_sub_task_tree(goal_set) - def get_object_info(self, **args): + def get_object_info(self,**args): try: obj = args['obj'] @@ -113,3 +122,10 @@ class DealChat(Act): mp = list(self.loc_map_en[similar_word]) near_location = random.choice(mp) return near_location + + def stop_serve(self,**args): + + + return "好的" + + diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index f3e9b38..32dd194 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -34,7 +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 + info['cost'] = 2 return info def _update(self) -> ptree.common.Status: diff --git a/robowaiter/llm_client/data/fix_questions.txt b/robowaiter/llm_client/data/fix_questions.txt index 3ca9173..55a05c0 100644 --- a/robowaiter/llm_client/data/fix_questions.txt +++ b/robowaiter/llm_client/data/fix_questions.txt @@ -4,7 +4,7 @@ 做一杯咖啡 好的,我马上做咖啡 create_sub_task -{"goal":"On(Coffee,CoffeeTable)"} +{"goal":"On(Coffee,CoffeeTable)"}00 不用了 好的,您有需要再跟我说 @@ -18,7 +18,7 @@ create_sub_task 早上好呀,我想找个能晒太阳的地方。 -没问题,您右手边就有能晒太阳的位置呢。 +您右手边就有能晒太阳的位置呢。 可以带我过去嘛? @@ -27,6 +27,35 @@ create_sub_task {"goal":"At(Robot,BrightTable1)"} +嘿,RoboWaiter,过来一下! +我在来的路上啦,请稍等噢! +create_sub_task +{"goal":"At(Robot,BrightTable6)"} + +你们这有什么饮料嘛? +我们咖啡厅提供各种口味咖啡,水,冰红茶,酸奶等饮品,还提供点心蛋糕等甜品。您想点什么? + + +来杯卡布奇诺吧。 +ok,请稍等! +create_sub_task +{"goal":"On(Coffee,BrightTable6)"} + + +麻烦啦! +没事儿,为您服务是我的荣幸! + +大厅的桌子好啊,快带我去呀! +好的好的,跟我来,我带你去找找。 +create_sub_task +{"goal":"At(Robot,WaterTable)"} + + +我想来杯水,帮我孩子拿个酸奶吧。 +ok,没问题,请稍等! +create_sub_task +{"goal":"On(Water,WaterTable),On(Yogurt,WaterTable)"} + 来杯酸奶吧。 好的没问题,请稍等! create_sub_task diff --git a/robowaiter/proto/camera.py b/robowaiter/proto/camera.py index 8780bbb..27f8fa7 100644 --- a/robowaiter/proto/camera.py +++ b/robowaiter/proto/camera.py @@ -7,6 +7,7 @@ import sys import time import grpc + sys.path.append('./') sys.path.append('../') @@ -24,6 +25,10 @@ channel = grpc.insecure_channel('localhost:30001', options=[ sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) objects_dic = {} +obstacle_objs_id = [114, 115, 122, 96, 102, 83, 121, 105, 108, 89, 100, 90, + 111, 103, 95, 92, 76, 113, 101, 29, 112, 87, 109, 98, + 106, 120, 97, 86, 104, 78, 85, 81, 82, 84, 91, 93, 94, + 99, 107, 116, 117, 118, 119, 255] ''' 初始化,卸载已经加载的关卡,清除所有机器人 @@ -217,6 +222,105 @@ def show_image(img_data, scene): plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None) plt.show() +def transform_co(img_data, pixel_x_, pixel_y_,depth_, scene ,id = 0,label = 0): + im = img_data.images[0] + + # 相机外参矩阵 + out_matrix = np.array(im.parameters.matrix).reshape((4, 4)) + + d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels)) + depth = depth_ + + # 将像素坐标转换为归一化设备坐标 + normalized_x = (pixel_x_ - im.parameters.cx) / im.parameters.fx + normalized_y = (pixel_y_ - im.parameters.cy) / im.parameters.fy + + # 将归一化设备坐标和深度值转换为相机坐标 + camera_x = normalized_x * depth + camera_y = normalized_y * depth + camera_z = depth + + # 构建相机坐标向量 + camera_coordinates = np.array([camera_x, camera_y, camera_z, 1]) + # print("物体相对相机坐标的齐次坐标: ", camera_coordinates) + + # 将相机坐标转换为机器人底盘坐标 + robot_coordinates = np.dot(out_matrix, camera_coordinates)[:3] + # print("物体的相对底盘坐标为:", robot_coordinates) + + # 将物体相对机器人底盘坐标转为齐次坐标 + robot_homogeneous_coordinates = np.array([robot_coordinates[0], -robot_coordinates[1], robot_coordinates[2], 1]) + # print("物体的相对底盘的齐次坐标为:", robot_homogeneous_coordinates) + + # 机器人坐标 + X = scene.location.X + Y = scene.location.Y + Z = 0.0 + + # 机器人旋转信息 + Roll = 0.0 + Pitch = 0.0 + Yaw = scene.rotation.Yaw + + # 构建平移矩阵 + T = np.array([[1, 0, 0, X], + [0, 1, 0, Y], + [0, 0, 1, Z], + [0, 0, 0, 1]]) + + # 构建旋转矩阵 + Rx = np.array([[1, 0, 0, 0], + [0, np.cos(Roll), -np.sin(Roll), 0], + [0, np.sin(Roll), np.cos(Roll), 0], + [0, 0, 0, 1]]) + + Ry = np.array([[np.cos(Pitch), 0, np.sin(Pitch), 0], + [0, 1, 0, 0], + [-np.sin(Pitch), 0, np.cos(Pitch), 0], + [0, 0, 0, 1]]) + + Rz = np.array([[np.cos(np.radians(Yaw)), -np.sin(np.radians(Yaw)), 0, 0], + [np.sin(np.radians(Yaw)), np.cos(np.radians(Yaw)), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + + R = np.dot(Rz, np.dot(Ry, Rx)) + + # 构建机器人的变换矩阵 + T_robot = np.dot(T, R) + # print(T_robot) + + # 将物体的坐标从机器人底盘坐标系转换到世界坐标系 + world_coordinates = np.dot(T_robot, robot_homogeneous_coordinates)[:3] + + # if world_coordinates[0] < 200 and world_coordinates[1] <= 1050: + # world_coordinates[0] += 400 + # world_coordinates[1] += 400 + # elif world_coordinates[0] >= 200 and world_coordinates[1] <= 1050: + # world_coordinates[0] -= 550 + # world_coordinates[1] += 400 + # elif world_coordinates[0] >= 200 and world_coordinates[1] > 1050: + # world_coordinates[0] -= 550 + # world_coordinates[1] -= 1450 + # elif world_coordinates[0] < 200 and world_coordinates[1] > 1050: + # world_coordinates[0] += 400 + # world_coordinates[1] -= 1450 + # print("物体的世界坐标:", world_coordinates) + + # 世界偏移后的坐标 + world_offest_coordinates = [world_coordinates[0] + 700, world_coordinates[1] + 1400, world_coordinates[2]] + # print("物体世界偏移的坐标: ", world_offest_coordinates) + return world_coordinates + + # 世界偏移后的坐标 + # world_offest_coordinates = [world_coordinates[0] + 700, world_coordinates[1] + 1400, world_coordinates[2]] + # print("物体世界偏移的坐标: ", world_offest_coordinates) + + + # dict_f = {'id':id,'label':label,'world_coordinates':world_coordinates,'world_offest_coordinates':world_offest_coordinates} + # with open('./semantic.txt', 'a') as file: + # file.write(str(dict_f) + '\n') + def save_obj_info(img_data, objs_name): items = img_data.info.split(";") @@ -236,6 +340,51 @@ def save_obj_info(img_data, objs_name): return objs_name +def get_obstacle_point(scene, cur_obstacle_world_points, map_ratio): + cur_obstacle_pixel_points = [] + img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment]) + img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth]) + im_segment = img_data_segment.images[0] + im_depth = img_data_depth.images[0] + d_segment = np.frombuffer(im_segment.data, dtype=im_segment.dtype).reshape((im_segment.height, im_segment.width, im_segment.channels)) + d_depth = np.frombuffer(im_depth.data, dtype=im_depth.dtype).reshape((im_depth.height, im_depth.width, im_depth.channels)) + + + # plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None) + # plt.show() + # + # plt.imshow(d_segment, cmap="gray" if "depth" in im_segment.name.lower() else None) + # plt.show() + + d_depth = np.transpose(d_depth, (1, 0, 2)) + d_segment = np.transpose(d_segment, (1, 0, 2)) + for i in range(0, d_segment.shape[0], map_ratio): + for j in range(0, d_segment.shape[1], map_ratio): + if d_depth[i][j][0] == 600: + continue + # if d_segment[i][j] == 96: + # print(f"apple的像素坐标:({i},{j})") + # print(f"apple的深度:{d_depth[i][j][0]}") + # print(f"apple的世界坐标: {transform_co(img_data_depth, i, j, d_depth[i][j][0], scene)}") + # if d_segment[i][j] == 113: + # print(f"kettle的像素坐标:({i},{j})") + # print(f"kettle的深度:{d_depth[i][j][0]}") + # print(f"kettle的世界坐标: {transform_co(img_data_depth, i, j, d_depth[i][j][0], scene)}") + if d_segment[i][j][0] in obstacle_objs_id: + cur_obstacle_pixel_points.append([i, j]) + # print(cur_obstacle_pixel_points) + for pixel in cur_obstacle_pixel_points: + world_point = transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene) + cur_obstacle_world_points.append([world_point[0], world_point[1]]) + # print(f"{pixel}:{[world_point[0], world_point[1]]}") + return cur_obstacle_world_points + + + + + + + def get_semantic_map(camera, cur_objs, objs_name): scene = Observe(0) objs = scene.objects @@ -273,9 +422,9 @@ if __name__ == '__main__': # print(get_semantic_map(GrabSim_pb2.CameraName.Head_Segment,cur_objs)) - for camera_name in [GrabSim_pb2.CameraName.Head_Depth]: - img_data = get_camera([camera_name], i) - show_image(img_data, scene) + # for camera_name in [GrabSim_pb2.CameraName.Head_Depth]: + # img_data = get_camera([camera_name], i) + # show_image(img_data, scene) # for camera_name in [GrabSim_pb2.CameraName.Waist_Color, GrabSim_pb2.CameraName.Waist_Depth]: # img_data = get_camera([camera_name], i) # show_image(img_data, 2) diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index 46abd5c..f6e1909 100644 --- a/robowaiter/robot/robot.py +++ b/robowaiter/robot/robot.py @@ -137,6 +137,9 @@ class Robot(object): print("\n") self.last_tick_output = bt_output + return True + else: + return False if __name__ == '__main__': pass \ No newline at end of file diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index c0c1c39..5c52533 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -51,7 +51,9 @@ class Scene: robot = None event_list = [] new_event_list = [] + signal_event_list = [] show_bubble = False + event_signal = "None" default_state = { "map": { @@ -99,6 +101,8 @@ class Scene: robot.load_BT() self.robot = robot + self.robot_changed = False + # 1-7 正常执行, 8-10 控灯操作移动到6, 11-12窗帘操作不需要移动, self.op_dialog = ["","制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7 "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12 @@ -172,8 +176,9 @@ class Scene: self.deal_event() self.deal_new_event() + self.deal_signal_event() self._step() - self.robot.step() + self.robot_changed = self.robot.step() def deal_new_event(self): if len(self.new_event_list)>0: @@ -184,6 +189,20 @@ class Scene: self.new_event_list.pop(0) func(*args) + def deal_signal_event(self): + if len(self.signal_event_list)>0: + next_event = self.signal_event_list[0] + t, func,args = next_event + if t < 0 and self.robot_changed: #一直等待机器人行动,直到机器人无行动 + return + if t > 0: + time.sleep(t) + + print(f'event: {t}, {func.__name__}') + self.signal_event_list.pop(0) + func(*args) + + def deal_event(self): if len(self.event_list)>0: @@ -604,6 +623,9 @@ class Scene: 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), + + 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) @@ -714,7 +736,7 @@ class Scene: scene = stub.Do(action) print(scene.info) - def navigation_move(self, cur_objs, objs_name_set, v_list, scene_id=0, map_id=11): + def navigation_move(self, cur_objs, objs_name_set, cur_obstacle_world_points, v_list, map_ratio, scene_id=0, map_id=11): print('------------------navigation_move----------------------') scene = stub.Observe(GrabSim_pb2.SceneID(value=scene_id)) walk_value = [scene.location.X, scene.location.Y] @@ -727,6 +749,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_obstacle_world_points = camera.get_obstacle_point(scene, cur_obstacle_world_points,map_ratio) + cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, objs_name_set) # if scene.info == "Unreachable": @@ -744,11 +768,14 @@ 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_obstacle_world_points = camera.get_obstacle_point(scene, cur_obstacle_world_points, map_ratio) + 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 + return cur_objs, objs_name_set, cur_obstacle_world_points def isOutMap(self, pos, min_x=-200, max_x=600, min_y=-250, max_y=1300): if pos[0] <= min_x or pos[0] >= max_x or pos[1] <= min_y or pos[1] >= max_y: @@ -789,21 +816,21 @@ class Scene: free_array = np.array(free_list) print(f"主动探索完成!以下是场景中可以到达的点:{free_array};其余点均是障碍物不可达") - # 画地图: X行Y列,第一行在下面 - plt.clf() - plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower', - extent=(-250, 1300, - -200, 600)) - plt.show() - print("已绘制完成地图!!!") + # # 画地图: X行Y列,第一行在下面 + # plt.clf() + # plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower', + # extent=(-250, 1300, + # -200, 600)) + # plt.show() + # print("已绘制完成地图!!!") return None - # 画地图: X行Y列,第一行在下面 - plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower', - extent=(-250, 1300, - -200, 600)) - plt.show() - print("已绘制部分地图!") + # # 画地图: X行Y列,第一行在下面 + # plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower', + # extent=(-250, 1300, + # -200, 600)) + # plt.show() + # print("已绘制部分地图!") return self.getNearestFrontier(cur_pos, self.all_frontier_list) def isNewFrontier(self, pos, map): diff --git a/robowaiter/scene/tasks/AEM.py b/robowaiter/scene/tasks/AEM.py index fb04a0f..fab39ad 100644 --- a/robowaiter/scene/tasks/AEM.py +++ b/robowaiter/scene/tasks/AEM.py @@ -2,8 +2,13 @@ 环境主动探索和记忆 要求输出探索结果(语义地图)对环境重点信息记忆。生成环境的语义拓扑地图,和不少于10个环境物品的识别和位置记忆,可以是图片或者文字或者格式化数据。 """ +import math +import matplotlib as mpl import pickle +import numpy as np +from matplotlib import pyplot as plt + from robowaiter.scene.scene import Scene class SceneAEM(Scene): def __init__(self, robot): @@ -12,8 +17,20 @@ class SceneAEM(Scene): def _reset(self): pass def _run(self): + # 创建一个从白色(1)到灰色(0)的 colormap cur_objs = [] + cur_obstacle_world_points = [] objs_name_set = set() + visited_obstacle = set() + + map_ratio = 5 + # # 创建一个颜色映射,其中0表示黑色,1表示白色 + # cmap = plt.cm.get_cmap('gray') + # cmap.set_under('black') + # cmap.set_over('white') + + + file_name = '../../proto/map_1.pkl' if os.path.exists(file_name): with open(file_name, 'rb') as file: @@ -26,15 +43,43 @@ class SceneAEM(Scene): # print(reachable([237,490])) # navigation_move([[237,490]], i, map_id) # navigation_test(i,map_id) + map_map = np.zeros((math.ceil(950 / map_ratio), math.ceil(1850 / map_ratio))) while True: goal = self.explore(map, 120) # cur_pos 指的是当前机器人的位置,场景中应该也有接口可以获取 if goal is None: break - cur_objs, objs_name_set = self.navigation_move(cur_objs, objs_name_set, [[goal[0], goal[1]]], 0, 11) + cur_objs, objs_name_set, cur_obstacle_world_points= self.navigation_move(cur_objs, objs_name_set, cur_obstacle_world_points, [[goal[0], goal[1]]], map_ratio, 0, 11) + + for point in cur_obstacle_world_points: + if point[0] < -350 or point[0] > 600 or point[1] < -400 or point[1] > 1450: + continue + map_map[math.floor((point[0] + 350) / map_ratio), math.floor((point[1] + 400) / map_ratio)] = 1 + visited_obstacle.add((math.floor((point[0] + 350) / map_ratio), math.floor((point[1] + 400) / map_ratio))) + # plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower', + # extent=(-400 / map_ratio, 1450 / map_ratio, + # -350 / map_ratio, 600 / map_ratio)) + + # 使用imshow函数绘制图像,其中cmap参数设置颜色映射 + plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower', + extent=(-400 / map_ratio, 1450 / map_ratio, + -350 / map_ratio, 600 / map_ratio)) + # plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower') + # plt.axis('off') + plt.show() print("------------物品信息--------------") print(cur_objs) - - pass + # for i in range(-350, 600): + # for j in range(-400, 1450): + # i = (math.floor((i + 350) / map_ratio)) + # j = (math.floor((j + 400) / map_ratio)) + # if (i, j) not in visited_obstacle: + # map_map[i][j] = 1 + plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower', + extent=(-400 / map_ratio, 1450 / map_ratio, + -350 / map_ratio, 600 / map_ratio)) + # plt.axis('off') + plt.show() + print("已绘制完成地图!!!") if __name__ == '__main__': diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py b/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py new file mode 100644 index 0000000..8b1adcd --- /dev/null +++ b/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py @@ -0,0 +1,117 @@ +""" +视觉语言操作 +机器人根据指令人的指令调节空调,自主探索环境导航到目标点,通过手臂的运动规划能力操作空调,比如开关按钮、调温按钮、显示面板 +""" + +import time +from robowaiter.scene.scene import Scene + + + +class SceneVLM(Scene): + + def __init__(self, robot): + super().__init__(robot) + # 在这里加入场景中发生的事件, (事件发生的时间,事件函数) + + self.scene_flag = 2 + self.st1 = 3 + self.st2 = 3 + # self.st2 = self.st1 + 30 + self.st3 = 3 + self.st4 = 3 + + self.new_event_list = [ + + # 场景1:带小女孩找阳光下的空位 + # (3, self.add_walker, (5, 230, 1200)), + # (3, self.control_walker, (0, False, 200, 60, 520, 0)), + # (10, self.customer_say, (0, "早上好呀,我想找个能晒太阳的地方。")), + # (10, self.customer_say, (0,"可以带我过去嘛?")), + # (20, self.control_walker, (0, False, 50, 140, 1200, 180)), # 小女孩站在了 BrightTable1 旁边就餐啦 + + # 场景2:有个胖胖男人点单交流并要咖啡,帮他送去角落的桌子 + (3, self.add_walker, (5, 230, 1200)), # 小女孩 + # 上述准备 + (self.st2, self.add_walker, (26, -28, -150, 90)), + (self.st2, self.add_walker, (10, -70, -200, -45)), + (self.st2, self.customer_say, (1, "嘿,RoboWaiter,过来一下!")), + (self.st2+10, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛?"]]])), # 20 胖胖男到了 BrightTable6 + (self.st2+15, self.customer_say, (1, "咖啡有哪些呢?")),# 10 + (self.st2+15+5, self.customer_say, (1,"来杯卡布奇诺吧。")), # 15 + + # 场景3:有位女士要杯水和冰红茶 + # (3, self.add_walker, (5, 230, 1200)), + # (3, self.add_walker, (26, -30, -200, -90)), + # (3, self.add_walker, (10, -80, -180, -45)), + # # 上述准备 + # (self.st3, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135), + # (self.st3, self.add_walker, (32, -80, 850, 135)), # 女 'BrightTable3': (-80, 850, 135), + # (self.st3, self.add_walker, (1, 60, 420, 135)), + # (self.st3, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走 + # (self.st3, self.add_walker, (48, 60, 520,0)), + # (self.st3 + 3, self.customer_say, (6, "哎呦,今天这么多人,还有空位吗?")),# 女士问 + # (self.st3 + 10, self.customer_say, (6, "我带着孩子呢,想要宽敞亮堂的地方。")), # 女士问 + # # 好的,我明白了,那么您可以选择我们的家庭亲子座,这样可以容纳您的孩子,并且更加宽敞舒适。 + # # 这里可以加一下自主导航和探索,找到一个位置 + # # 好的,我明白了,那么我们推荐您到大厅的桌子,那里的空间比较宽敞,环境也比较明亮,适合带着孩子一起用餐。 + # (self.st3 + 15, self.customer_say, (6, "大厅的桌子好啊,快带我去呀!")), + # (self.st3 + 25, self.control_walker, (6, False, 50,-250, 480, 0)), # #290, 400 + # (self.st3 + 25, self.customer_say, (6, "我想来杯水,帮我孩子拿个酸奶吧。")), + # (self.st3 + 60, self.customer_say, (6, "谢谢!")) + + # 场景4:三人排队点单,一个要点心,一个没座位了赠送保温杯 + # (3, self.add_walker, (5, 230, 1200)), + # (3, self.add_walker, (26, -30, -200, -90)), + # (3, self.add_walker, (10, -80, -180, -45)), + # (3, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135), + # (3, self.add_walker, (32, -80, 850, 135)), + # (3, self.add_walker, (1, 60, 420, 135)), + # (3, self.add_walker, (48, 60, 520, 0)), + # # # 上述准备 + # (self.st4, self.add_walker, (31, 60, 600, -90)), # 女红色排队 + # (self.st4, self.add_walker, (9, 60, 680, -90)), # 男灰黑色排队 + # (self.st4, self.add_walker, (20, 60, 760, -90)), # 大胖男排队 + ] + + def _reset(self): + self.gen_obj() + # self.add_walkers([[47, 920]]) + pass + + def _run(self, op_type=10): + # 一个行人从门口走到 吧台 + # 打招呼需要什么 + # 行人说 哪里有位置,想晒个太阳 + # 带领行人去有太阳的地方 + # 行人说 有点热 + # 好的,这就去开空调 + self.walker_followed = False + pass + + def _step(self): + + + if self.scene_flag == 1: + # 如果机器人不在 吧台 + if self.walker_followed: + return + end = [self.status.location.X, self.status.location.Y] + if end[1]>=600 or end[1]<=450 or end[0]>=250: + # if int(self.status.location.X)!=247 or int(self.status.location.X)!=520: + self.walker_followed = True + self.control_walkers_and_say([[0,False,150,end[0],end[1],90,"wd!"]]) + self.scene_flag += 1 + + pass + +if __name__ == '__main__': + import os + from robowaiter.robot.robot import Robot + + robot = Robot() + + # create task + task = SceneVLM(robot) + task.reset() + task.run() diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1118.py b/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1118.py new file mode 100644 index 0000000..776e4e6 --- /dev/null +++ b/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1118.py @@ -0,0 +1,102 @@ +""" +视觉语言操作 +机器人根据指令人的指令调节空调,自主探索环境导航到目标点,通过手臂的运动规划能力操作空调,比如开关按钮、调温按钮、显示面板 +""" + +import time +from robowaiter.scene.scene import Scene + + + +class SceneVLM(Scene): + + def __init__(self, robot): + super().__init__(robot) + # 在这里加入场景中发生的事件, (事件发生的时间,事件函数) + + self.scene_flag = 1 + self.st1 = 3 + self.st2 = self.st1 + 30 + self.st3 = 3 + + self.new_event_list = [ + + # 场景1:带小女孩找阳光下的空位 + # (3, self.add_walker, (5, 230, 1200)), + # (3, self.control_walker, (0, False, 200, 60, 520, 0)), + # (10, self.customer_say, (0, "早上好呀,我想找个能晒太阳的地方。")), + # (10, self.customer_say, (0,"可以带我过去嘛?")), + # (20, self.control_walker, (0, False, 50, 140, 1200, 180)), # 小女孩站在了 BrightTable1 旁边就餐啦 + + # 场景2:有个胖胖男人点单交流并要咖啡,帮他送去角落的桌子 + # (3, self.add_walker, (5, 230, 1200)), # 小女孩 + # 上述准备 + # (self.st2, self.add_walker, (26, 60,-140)), + # (self.st2, self.add_walker, (10, -80, -180,-45)), # (-150 -250) + # (self.st2, self.control_walker, (1, False, 100, 70, 520, 0)), #3 + # (self.st2+15, self.customer_say, (1, "你们这有什么饮料嘛?")), # 10 + # (self.st2+15, self.customer_say, (1, "咖啡有哪些呢?")),# 10 + # (self.st2+15+5, self.customer_say, (1,"来杯卡布奇诺吧,我在靠窗边的高脚桌那。")), # 15 + # (self.st2+15+5+5, self.control_walkers_and_say, ([[[1, False, 100, -30, -200, -90, "麻烦啦!"]]])), # 20 胖胖男到了 BrightTable6 + + # 场景3:有位女士要杯水和冰红茶 + (3, self.add_walker, (5, 230, 1200)), + (3, self.add_walker, (26, -30, -200, -90)), + (3, self.add_walker, (10, -80, -180, -45)), + # 上述准备 + (self.st3, self.add_walker, (21, 65, 1000, -90)), # 男 'BrightTable2': (65, 1000, 135), + (self.st3, self.add_walker, (32, -80, 850, 135)), # 女 'BrightTable3': (-80, 850, 135), + (self.st3, self.add_walker, (1, -80, 750, 135)), + (self.st3, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走 + (self.st3, self.add_walker, (48, 60, 520,0)), + (self.st3 + 3, self.customer_say, (6, "哎呦,今天这么多人,还有空位吗?")),# 女士问 + (self.st3 + 10, self.customer_say, (6, "我带着孩子呢,想要宽敞亮堂的地方。")), # 女士问 + # 好的,我明白了,那么您可以选择我们的家庭亲子座,这样可以容纳您的孩子,并且更加宽敞舒适。 + # 这里可以加一下自主导航和探索,找到一个位置 + # 好的,我明白了,那么我们推荐您到大厅的桌子,那里的空间比较宽敞,环境也比较明亮,适合带着孩子一起用餐。 + (self.st3 + 15, self.customer_say, (6, "大厅的桌子好啊,快带我去呀!")), + (self.st3 + 20, self.control_walker, (6, False, 50,-290, 400, 180)), + + ] + + def _reset(self): + self.gen_obj() + # self.add_walkers([[47, 920]]) + pass + + def _run(self, op_type=10): + # 一个行人从门口走到 吧台 + # 打招呼需要什么 + # 行人说 哪里有位置,想晒个太阳 + # 带领行人去有太阳的地方 + # 行人说 有点热 + # 好的,这就去开空调 + self.walker_followed = False + pass + + def _step(self): + + + if self.scene_flag == 1: + # 如果机器人不在 吧台 + if self.walker_followed: + return + end = [self.status.location.X, self.status.location.Y] + if end[1]>=600 or end[1]<=450 or end[0]>=250: + # if int(self.status.location.X)!=247 or int(self.status.location.X)!=520: + self.walker_followed = True + self.control_walkers_and_say([[0,False,150,end[0],end[1],90,"wd!"]]) + self.scene_flag += 1 + + pass + +if __name__ == '__main__': + import os + from robowaiter.robot.robot import Robot + + robot = Robot() + + # create task + task = SceneVLM(robot) + task.reset() + task.run() diff --git a/robowaiter/scene/tasks/OT/Open_tasks_test.py b/robowaiter/scene/tasks/OT/Open_tasks_test.py index d39aae6..8844020 100644 --- a/robowaiter/scene/tasks/OT/Open_tasks_test.py +++ b/robowaiter/scene/tasks/OT/Open_tasks_test.py @@ -17,8 +17,9 @@ class SceneOT(Scene): def __init__(self, robot): super().__init__(robot) # 在这里加入场景中发生的事件 - self.new_event_list = [ - (3, self.customer_say, ("System","来一号桌")) + self.signal_event_list = [ + (3, self.customer_say, ("System","来一号桌")), + (-1, self.customer_say, ("System","回去吧")), # (5, self.set_goal("At(Robot,BrightTable4)")) ] diff --git a/robowaiter/scene/tasks/VLM/VLM_order.py b/robowaiter/scene/tasks/VLM/VLM_order.py index 1d773e4..c743023 100644 --- a/robowaiter/scene/tasks/VLM/VLM_order.py +++ b/robowaiter/scene/tasks/VLM/VLM_order.py @@ -10,12 +10,12 @@ class SceneVLM(Scene): def __init__(self, robot): super().__init__(robot) # 在这里加入场景中发生的事件, (事件发生的时间,事件函数) - self.new_event_list = [ + self.signal_event_list = [ (3, self.add_walker, (20,0,700)), - # (5, self.control_walker, (6, False,100, 60, 520,0)), #[walkerID,autowalk,speed,X,Y,Yaw] - # # (10, self.customer_say, (6,"给我来杯酸奶和咖啡,哦对,再倒一杯水。")), - # (8, self.customer_say, (6, "来杯酸奶吧。")), - # (7, self.control_walker, (6, False, 100, -250, 480, 0)), #(-100,600) + (1, self.control_walker, (4, False,100, 60, 520,0)), #[walkerID,autowalk,speed,X,Y,Yaw] + # (10, self.customer_say, (6,"给我来杯酸奶和咖啡,哦对,再倒一杯水。")), + (-1, self.customer_say, (4, "来杯酸奶吧。")), + (-1, self.control_walker, (4, False, 100, -250, 480, 0)), #(-100,600) # 有人提出要开空调和关窗帘 # bar (60, 520) @@ -24,10 +24,10 @@ class SceneVLM(Scene): # (35, self.customer_say, (7,"好热呀!太阳也好大!")), # (45, self.control_walkers_and_say, ([[[7, False, 100, 270, -240, -65, "谢谢,这下凉快了!"]]])), - (5, self.add_walker, (0, 0, 0)), - (6, self.control_walker, (7, False, 100, 60, 520, 180)), - (7, self.customer_say, (7,"好热呀!太阳也好大!")), - (8, self.control_walkers_and_say, ([[[7, False, 100, 270, -240, -65, "谢谢,这下凉快了!"]]])), + (3, self.add_walker, (0, 0, 0)), + (1, self.control_walker, (5, False, 100, 60, 520, 180)), + (1, self.customer_say, (5,"好热呀!太阳也好大!")), + (-1, self.control_walkers_and_say, ([[[5, False, 100, 270, -240, -65, "谢谢,这下凉快了!"]]])), ] def _reset(self): diff --git a/robowaiter/scene/tasks/VLM/VLN_greet_lead.py b/robowaiter/scene/tasks/VLM/VLN_greet_lead.py index 251d302..6eb8d3e 100644 --- a/robowaiter/scene/tasks/VLM/VLN_greet_lead.py +++ b/robowaiter/scene/tasks/VLM/VLN_greet_lead.py @@ -11,9 +11,8 @@ class SceneVLM(Scene): super().__init__(robot) # 在这里加入场景中发生的事件, (事件发生的时间,事件函数) self.new_event_list = [ - (3, self.add_walker, (5, 230, 1200)), - (5, self.control_walkers_and_say, ([[[0, False, 200, 60, 520, 0, "早上好呀,我想找个能晒太阳的地方。"]]])),# (0, 60, 520)), - (6, self.customer_say, (0,"可以带我过去嘛?")), + (3, self.add_walker, (0,60,520)), + (5, self.customer_say, (0,"可以带我去空位上嘛?我想晒太阳。")), ] def _reset(self):