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

# Conflicts:
#	robowaiter/behavior_lib/act/DealChat.py

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),

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 "好的"
+
+

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:

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

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]:
+        # for camera_name in [GrabSim_pb2.CameraName.Head_Depth]:
-            img_data = get_camera([camera_name], i)
+        #     img_data = get_camera([camera_name], i)
-            show_image(img_data, scene)
+        #     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)

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

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列，第一行在下面
+            # # 画地图: X行Y列，第一行在下面
-            plt.clf()
+            # plt.clf()
-            plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower',
+            # plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower',
-                       extent=(-250, 1300,
+            #            extent=(-250, 1300,
-                               -200, 600))
+            #                    -200, 600))
-            plt.show()
+            # plt.show()
-            print("已绘制完成地图！！！")
+            # print("已绘制完成地图！！！")
 
             return None
-        # 画地图: X行Y列，第一行在下面
+        # # 画地图: X行Y列，第一行在下面
-        plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower',
+        # plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower',
-                   extent=(-250, 1300,
+        #            extent=(-250, 1300,
-                           -200, 600))
+        #                    -200, 600))
-        plt.show()
+        # plt.show()
-        print("已绘制部分地图！")
+        # print("已绘制部分地图！")
         return self.getNearestFrontier(cur_pos, self.all_frontier_list)
 
     def isNewFrontier(self, pos, map):

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)
-
+        # for i in range(-350, 600):
-        pass
+        #     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__':

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

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

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 = [
+        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)"))
         ]
 

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]
+            (1, self.control_walker, (4, False,100, 60, 520,0)), #[walkerID,autowalk,speed,X,Y,Yaw]
-            # # (10, self.customer_say, (6,"给我来杯酸奶和咖啡，哦对，再倒一杯水。")),
+            # (10, self.customer_say, (6,"给我来杯酸奶和咖啡，哦对，再倒一杯水。")),
-            # (8, self.customer_say, (6, "来杯酸奶吧。")),
+            (-1, self.customer_say, (4, "来杯酸奶吧。")),
-            # (7, self.control_walker, (6, False, 100, -250, 480, 0)),  #(-100,600)
+            (-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)),
+            (3, self.add_walker, (0, 0, 0)),
-            (6, self.control_walker, (7, False, 100, 60, 520, 180)),
+            (1, self.control_walker, (5, False, 100, 60, 520, 180)),
-            (7, self.customer_say, (7,"好热呀！太阳也好大！")),
+            (1, self.customer_say, (5,"好热呀！太阳也好大！")),
-            (8, self.control_walkers_and_say, ([[[7, False, 100, 270, -240, -65, "谢谢，这下凉快了！"]]])),
+            (-1, self.control_walkers_and_say, ([[[5, False, 100, 270, -240, -65, "谢谢，这下凉快了！"]]])),
         ]
 
     def _reset(self):

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)),
+            (3, self.add_walker,  (0,60,520)),
-            (5, self.control_walkers_and_say, ([[[0, False, 200, 60, 520, 0, "早上好呀，我想找个能晒太阳的地方。"]]])),# (0, 60, 520)),
+            (5, self.customer_say, (0,"可以带我去空位上嘛？我想晒太阳。")),
-            (6, self.customer_say, (0,"可以带我过去嘛？")),
         ]
 
     def _reset(self):