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

robowaiter/behavior_lib/_base/Behavior.py

@@ -30,6 +30,17 @@ class Bahavior(ptree.behaviour.Behaviour):
     # all_object = set()
     # all_place=set()
 
+    num_of_obj_on_place={
+        'Bar': 0,  # (247.0, 520.0, 100.0)
+        'Bar2': 0,
+        'WaterTable': 0,
+        'CoffeeTable': 0,
+        'Table1': 0,
+        'Table2': 0,
+        'Table3': 0,
+        'BrightTable6': 0,
+    }
+
     place_xyz_dic={
         'Bar': (247.0, 520.0, 100.0), #(247.0, 520.0, 100.0)
         'Bar2': (240.0, 40.0, 100.0),

robowaiter/behavior_lib/act/Clean.py

@@ -44,6 +44,7 @@ class Clean(Act):
 
         self.scene.state["condition_set"] |= (self.info["add"])
         self.scene.state["condition_set"] -= self.info["del_set"]
+        self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
 
         # print("After Clean condition_set:",self.scene.state["condition_set"] )
         return Status.RUNNING

robowaiter/behavior_lib/act/Make.py

@@ -54,6 +54,8 @@ class Make(Act):
                 #     obj_info = obj_dict[id]
                 #     obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
                 #     print(id,obj.name,obj_x,obj_y,obj_z)
+        if self.scene.take_picture:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
 
         self.scene.state["condition_set"] |= (self.info["add"])
         self.scene.state["condition_set"] -= self.info["del_set"]

robowaiter/behavior_lib/act/MoveTo.py

@@ -32,6 +32,9 @@ class MoveTo(Act):
         # goal = self.scene.state['map']['obj_pos'][self.args[0]]
         # navigator.navigate_old(goal, animation=False)
 
+        if self.scene.take_picture:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
+
         # 走到固定的地点
         if self.target_place in Act.place_xy_yaw_dic:
             goal = Act.place_xy_yaw_dic[self.target_place]
@@ -87,10 +90,12 @@ class MoveTo(Act):
             # print("MoveTo",obj_x, obj_y, obj_z," obj_id:",obj_id," obj_info:",obj_info.name)
 
 
+        if self.scene.take_picture:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
+
         # goal = self.scene.state['map']['obj_pos'][self.args[0]]
         # self.scene.walk_to(goal[0],goal[1]) # X, Y, Yaw=None, velocity=200, dis_limit=0
 
-
         self.scene.state["condition_set"] |= (self.info["add"])
         self.scene.state["condition_set"] -= self.info["del_set"]
         return ptree.common.Status.RUNNING

robowaiter/behavior_lib/act/PickUp.py

@@ -56,6 +56,9 @@ class PickUp(Act):
         self.scene.move_task_area(op_type=16, obj_id=obj_id)
         self.scene.op_task_execute(op_type=16, obj_id=obj_id)
 
+        if self.scene.take_picture:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
+
         self.scene.state["condition_set"] |= (self.info["add"])
         self.scene.state["condition_set"] -= self.info["del_set"]
         return Status.RUNNING

robowaiter/behavior_lib/act/PutDown.py

@@ -33,8 +33,16 @@ class PutDown(Act):
         release_pos = list(Act.place_xyz_dic[self.target_place])
         # # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 70.0], 水杯桌:[-70.0, 500.0, 107]
         # # 桌子2:[-55.0, 0.0, 107],桌子3:[-55.0, 150.0, 107]
+
+        if Act.num_of_obj_on_place[self.target_place]>=1:
+            release_pos[1] += 25
+
+        Act.num_of_obj_on_place[self.target_place]+=1
+
         self.scene.move_task_area(op_type, release_pos=release_pos)
         self.scene.op_task_execute(op_type, release_pos=release_pos)
+        if self.scene.take_picture:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
 
         self.scene.state["condition_set"] |= (self.info["add"])
         self.scene.state["condition_set"] -= self.info["del_set"]

robowaiter/behavior_lib/act/Turn.py

@@ -61,13 +61,13 @@ class Turn(Act):
                 info["add"] = {f'Is({arg[0]},Off)'}
                 info["del_set"] = {f'Is({arg[0]},On)'}
         elif arg[0]=='ACTemperature':
-            info["pre"] = {f'Holding(Nothing)'}
+            info["pre"] = {f'Holding(Nothing)',f'Is(AC,On)'}
             if arg[1]=="Up":
-                info["pre"] |= {f'Is({arg[0]},Down)'}
+                # info["pre"] |= {f'Is({arg[0]},Down)'}
                 info["add"] = {f'Is({arg[0]},Up)'}
                 info["del_set"] = {f'Is({arg[0]},Down)'}
             elif arg[1]=="Down":
-                info["pre"] |= {f'Is({arg[0]},Up)'}
+                # info["pre"] |= {f'Is({arg[0]},Up)'}
                 info["add"] = {f'Is({arg[0]},Down)'}
                 info["del_set"] = {f'Is({arg[0]},Up)'}
         return info
@@ -76,7 +76,8 @@ class Turn(Act):
 
         self.scene.move_task_area(self.op_type)
         self.scene.op_task_execute(self.op_type)
-
+        if self.scene.take_picture:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
         self.scene.state["condition_set"] |= (self.info["add"])
         self.scene.state["condition_set"] -= self.info["del_set"]
 

robowaiter/behavior_lib/cond/At.py

@@ -18,6 +18,8 @@ class At(Cond):
     def _update(self) -> ptree.common.Status:
         # if self.scene.status?
 
+        # self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
+
         if self.name in self.scene.state["condition_set"]:
             return ptree.common.Status.SUCCESS
         else:

robowaiter/behavior_lib/cond/FocusingCustomer.py

@@ -11,6 +11,8 @@ class FocusingCustomer(Cond):
     def _update(self) -> ptree.common.Status:
         # if self.scene.status?
         if "customer" in self.scene.state['attention']:
+            if self.scene.take_picture:
+                self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
             return ptree.common.Status.SUCCESS
         else:
             goal = Cond.place_xy_yaw_dic['Bar']

robowaiter/behavior_lib/cond/Holding.py

@@ -14,6 +14,8 @@ class Holding(Cond):
     def _update(self) -> ptree.common.Status:
         # if self.scene.status?
 
+        # self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
+
         if self.name in self.scene.state["condition_set"]:
             return ptree.common.Status.SUCCESS
         else:

robowaiter/behavior_lib/cond/Is.py

@@ -24,6 +24,7 @@ class Is(Cond):
 
     def _update(self) -> ptree.common.Status:
         # if self.scene.status?
+        # self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
 
         if self.name in self.scene.state["condition_set"]:
             return ptree.common.Status.SUCCESS

robowaiter/behavior_lib/cond/NewCustomer.py

@@ -14,6 +14,8 @@ class NewCustomer(Cond):
 
     def _update(self) -> ptree.common.Status:
 
+        if self.scene.take_picture:
+            self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
         # 获取customer的位置
         # bar (247.0, 520.0, 100.0)
         close_to_bar = False

robowaiter/behavior_lib/cond/On.py

@@ -18,6 +18,7 @@ class On(Cond):
 
         # print("self.name:",self.name)
         # print("On: condition_set:",self.scene.state["condition_set"])
+        # self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio)
 
         if self.name in self.scene.state["condition_set"]:
             return ptree.common.Status.SUCCESS

robowaiter/llm_client/data/fix_questions.txt

@@ -6,7 +6,7 @@
 create_sub_task
 {"goal":"On(Coffee,CoffeeTable)"}00
 
-不用了
+不用了。
 好的，您有需要再跟我说
 stop_serve
 {}
@@ -91,8 +91,26 @@ create_sub_task
 {"goal":"Is(AC,On),Is(Curtain,Off)"}
 
 
+下班啦！别忘了打扫卫生。
+收到！请问您需要来点什么吗？
+create_sub_task
+"{""Is(Floor,Clean)"",""Is(Table1,Clean)"",""Is(Chairs,Clean)"",""Is(AC,Off)"",""Is(HallLight,Off)"",""Is(TubeLight,Off)"",""Is(Curtain,Off)""}"
+
 
 请问洗手间在哪里？
 洗手间在这附近
 get_object_info
 {"obj":"洗手间"}
+
+
+
+给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。
+好的，请稍等。
+create_sub_task
+{"goal":"On(BottledDrink,WaterTable),On(Yogurt,WaterTable)"}
+
+
+好热呀，想开空调，想要温度调低点！
+抱歉，我马上去开空调并调低空调温度。
+create_sub_task
+{"goal":"Is(ACTemperature,Down)"}

robowaiter/proto/camera.py

@@ -29,7 +29,7 @@ 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]
+                    99, 107, 116, 117, 118, 119, 255, 251]
 not_key_objs_id = {255,254,253,107,81}
 
 '''
@@ -367,12 +367,11 @@ def get_id_object_pixels(id, scene):
         world_points.append(transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene))
     return world_points
 
-
+def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
-
-
-def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
     cur_obstacle_pixel_points = []
     object_pixels = {}
+    obj_detect_count = 0
+    cur_objs_id = []
     colors = [
         'red',
         'pink',
@@ -403,10 +402,13 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
     for item in items:
         key, value = item.split(":")
         objs_id[int(key)] = value
+    objs_id[251] = "walker"
     # plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None)
     # plt.show()
-    #
+    plt.subplot(2, 2, 1)
-    # plt.imshow(d_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
+    plt.imshow(d_segment, cmap="gray" if "depth" in im_segment.name.lower() else None)
+    plt.axis("off")
+    plt.title("相机分割")
     # plt.show()
 
     d_depth = np.transpose(d_depth, (1, 0, 2))
@@ -439,16 +441,21 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
         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]]}")
-
+    plt.subplot(2, 2, 2)
     plt.imshow(d_color, cmap="gray" if "depth" in im_depth.name.lower() else None)
+    plt.axis('off')
+    plt.title("目标检测")
+    # plt.tight_layout()
 
     for key, value in object_pixels.items():
-        if key == 101 or key == 0:
+        if key == 0 or key not in objs_id.keys():
             continue
-        if key in [91, 84]:
+
+        if key in [91, 84, 96, 87, 102, 106, 120, 85,113, 101, 83, 251]:
             X = np.array(value)
             db.fit(X)
             labels = db.labels_
+
             # 将数据按照聚类标签分组，并打印每个分组的数据
             for i in range(max(labels) + 1):  # 从0到最大聚类标签的值
                 group_data = X[labels == i]  # 获取当前标签的数据
@@ -458,6 +465,9 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
                 y_min = min(p[1] for p in group_data)
                 if x_max - x_min < 10 or y_max - y_min < 10:
                     continue
+                if key != 251:
+                    obj_detect_count += 1
+                cur_objs_id.append(objs_id[key])
                 # 在指定的位置绘制方框
                 # 创建矩形框
                 rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1, edgecolor=colors[key % 10],
@@ -465,7 +475,11 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
                 plt.text(x_min, y_min, f'{objs_id[key]}', fontdict={'family': 'serif', 'size': 10, 'color': 'green'}, ha='center',
                          va='center')
                 plt.gca().add_patch(rect)
+
         else:
+            if key != 251:
+                obj_detect_count += 1
+            cur_objs_id.append(objs_id[key])
             x_max = max(p[0] for p in value)
             y_max = max(p[1] for p in value)
             x_min = min(p[0] for p in value)
@@ -484,11 +498,12 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
         # rect = patches.Rectangle((0, 255), 15, 30, linewidth=1, edgecolor='g',
         #                          facecolor='none')
 
+    plt.subplot(2, 7, 14)  # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
 
-        # 将矩形框添加到图像中
+    plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10)
-        # plt.gca().add_patch(rect)
+
-    plt.show()
+    # plt.show()
-    return cur_obstacle_world_points
+    return cur_obstacle_world_points, cur_objs_id
 
 
 

robowaiter/robot/robot.py

@@ -92,8 +92,9 @@ class Robot(object):
         else:
             print('Warning: have none sub task sequence')
             self.scene.sub_task_seq = seq
-        print("当前行为树为：")
+        # print("当前行为树为：")
-        print_tree_from_root(self.bt.root)
+        # print_tree_from_root(self.bt.root)
+        print("行为树扩展完成！")
 
     # 获取所有action的pre,add,del列表
     def collect_action_nodes(self):

robowaiter/scene/scene.py

@@ -4,6 +4,8 @@ import time
 import grpc
 import numpy as np
 import matplotlib.pyplot as plt
+from matplotlib import patches
+
 from robowaiter.proto import camera
 from robowaiter.proto import semantic_map
 from robowaiter.algos.navigator.navigate import Navigator
@@ -13,6 +15,12 @@ from robowaiter.proto import GrabSim_pb2_grpc
 import copy
 import os
 from robowaiter.utils import get_root_path
+from sklearn.cluster import DBSCAN
+from matplotlib import pyplot as plt
+plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
+plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
+
+
 root_path = get_root_path()
 
 channel = grpc.insecure_channel(
@@ -32,6 +40,11 @@ def init_world(scene_num=1, mapID=11):
     stub.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=mapID))
     time.sleep(3)  # wait for the map to load
 
+def get_camera(part, scene_id=0):
+    # print('------------------get_camera----------------------')
+    action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id)
+    return stub.Capture(action)
+
 
 def show_image(camera_data):
     print('------------------show_image----------------------')
@@ -100,6 +113,10 @@ class Scene:
         self.sub_task_seq = None
 
         self.show_bubble = True
+        # 图像分割
+        self.take_picture = False
+        self.map_ratio = 5
+        self.db = DBSCAN(eps=self.map_ratio, min_samples=int(self.map_ratio / 2))
 
         # init robot
         if robot:
@@ -568,7 +585,17 @@ class Scene:
 
 
     def move_to_obj(self,obj_id):
+
         scene = self.status
+        # 抬头
+        # value = [0]*21
+        # for i in range(21):
+        #     value[i] = self.status.joints[i].angle
+        # value[5] = 0
+        # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
+        # scene = stub.Do(action)
+        # time.sleep(1.0)
+
         obj_info = scene.objects[obj_id]
         # Robot
         obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
@@ -587,6 +614,16 @@ class Scene:
     # 移动到进行操作任务的指定地点
     def move_task_area(self, op_type, obj_id=0, release_pos=[247.0, 520.0, 100.0]):
         scene = self.status
+
+        # 抬头
+        # value = [0]*21
+        # for i in range(21):
+        #     value[i] = self.status.joints[i].angle
+        # value[5] = 0
+        # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
+        # scene = stub.Do(action)
+        # time.sleep(1.0)
+
         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:  # 开关窗帘不需要移动
@@ -634,6 +671,15 @@ class Scene:
 
     # 调整空调开关、温度
     def adjust_kongtiao(self,op_type):
+        # 低头
+        value = [0]*21
+        for i in range(21):
+            value[i] = self.status.joints[i].angle
+        value[5] = 30
+        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
+        scene = stub.Do(action)
+        time.sleep(1.0)
+
         obj_loc = self.obj_loc[:]
         obj_loc[2] -= 5
         if op_type == 13: obj_loc[1] -= 2
@@ -664,6 +710,19 @@ class Scene:
     def grasp_obj(self,obj_id,hand_id=1):
         print('------------------adjust_joints----------------------')
         scene = self.status
+
+        # 低头
+        value = [0]*21
+        for i in range(21):
+            value[i] = self.status.joints[i].angle
+        value[5] = 30
+        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
+        scene = stub.Do(action)
+        time.sleep(1.0)
+
+        if self.take_picture:
+            self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
+
         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":
@@ -713,6 +772,19 @@ class Scene:
     # 实现放置操作
     def release_obj(self,release_pos):
         print("------------------adjust_joints----------------------")
+        # 低头
+        value = [0]*21
+        for i in range(21):
+            value[i] = self.status.joints[i].angle
+        value[5] = 30
+        action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
+        scene = stub.Do(action)
+        time.sleep(1.0)
+
+        if self.take_picture:
+            self.get_obstacle_point(self.db, self.status, map_ratio=self.map_ratio)
+
+
         if release_pos==[340.0, 900.0, 99.0]:
             self.ik_control_joints(2, release_pos[0]-40, release_pos[1]+35, release_pos[2])
             time.sleep(2.0)
@@ -766,7 +838,7 @@ class Scene:
             scene = stub.Do(action)
             print(scene.info)
 
-    def navigation_move(self, cur_objs, objs_name_set, cur_obstacle_world_points, v_list, map_ratio, db, scene_id=0, map_id=11):
+    def navigation_move(self, plt, cur_objs, cur_obstacle_world_points, v_list, map_ratio, db, 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]
@@ -779,10 +851,10 @@ 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,
+            # cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
-                                                              objs_name_set)
+            #                                                   objs_name_set)
 
-            cur_obstacle_world_points = camera.get_obstacle_point(db, scene, cur_obstacle_world_points,map_ratio)
+            cur_obstacle_world_points, cur_objs_id= camera.get_obstacle_point(plt, db, scene, cur_obstacle_world_points,map_ratio)
 
 
             # if scene.info == "Unreachable":
@@ -801,15 +873,15 @@ class Scene:
                 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,
+                # cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
-                                                                  objs_name_set)
+                #                                                   objs_name_set)
 
-                cur_obstacle_world_points = camera.get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio)
+                cur_obstacle_world_points, cur_objs_id= camera.get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio)
 
 
                 # if scene.info == "Unreachable":
                 print(scene.info)
-        return cur_objs, objs_name_set, cur_obstacle_world_points
+        return cur_obstacle_world_points, cur_objs_id
 
     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:
@@ -905,3 +977,235 @@ class Scene:
             return True
         else:
             return False
+
+
+
+    @staticmethod
+    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
+
+    @staticmethod
+    def get_obstacle_point(db, scene, map_ratio):
+        # db = DBSCAN(eps=4, min_samples=2)
+        cur_obstacle_pixel_points = []
+        object_pixels = {}
+        colors = [
+            'red',
+            'pink',
+            'purple',
+            'blue',
+            'cyan',
+            'green',
+            'yellow',
+            'orange',
+            'brown',
+            'gold',
+        ]
+        not_key_objs_id = {255, 254, 253, 107, 81}
+
+        img_data_segment = get_camera([GrabSim_pb2.CameraName.Head_Segment])
+        img_data_depth = get_camera([GrabSim_pb2.CameraName.Head_Depth])
+        img_data_color = get_camera([GrabSim_pb2.CameraName.Head_Color])
+
+        im_segment = img_data_segment.images[0]
+        im_depth = img_data_depth.images[0]
+        im_color = img_data_color.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))
+        d_color = np.frombuffer(im_color.data, dtype=im_color.dtype).reshape(
+            (im_color.height, im_color.width, im_color.channels))
+
+        items = img_data_segment.info.split(";")
+        objs_id = {}
+        for item in items:
+            key, value = item.split(":")
+            objs_id[int(key)] = value
+        objs_id[251] = "walker"
+        # 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.axis("off")
+        # plt.title("相机分割")
+        # 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])
+                if d_segment[i][j][0] not in not_key_objs_id:
+                    # 首先检查键是否存在
+                    if d_segment[i][j][0] in object_pixels:
+                        # 如果键存在，那么添加元组(i, j)到对应的值中
+                        object_pixels[d_segment[i][j][0]].append([i, j])
+                    else:
+                        # 如果键不存在，那么创建一个新的键值对，其中键是d_segment[i][j][0]，值是一个包含元组(i, j)的列表
+                        object_pixels[d_segment[i][j][0]] = [[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]]}")
+        # plt.subplot(2, 2, 2)
+        plt.imshow(d_color, cmap="gray" if "depth" in im_depth.name.lower() else None)
+        # plt.axis('off')
+        plt.title("目标检测")
+
+
+        for key, value in object_pixels.items():
+            if key == 0:
+                continue
+            if key not in objs_id.keys():
+                continue
+            if key in [91, 84, 96, 87, 102, 106, 120, 85, 113, 101, 83, 251]:
+                X = np.array(value)
+                db.fit(X)
+                labels = db.labels_
+                # 将数据按照聚类标签分组，并打印每个分组的数据
+                for i in range(max(labels) + 1):  # 从0到最大聚类标签的值
+                    group_data = X[labels == i]  # 获取当前标签的数据
+                    x_max = max(p[0] for p in group_data)
+                    y_max = max(p[1] for p in group_data)
+                    x_min = min(p[0] for p in group_data)
+                    y_min = min(p[1] for p in group_data)
+                    if x_max - x_min < 10 or y_max - y_min < 10:
+                        continue
+                    # 在指定的位置绘制方框
+                    # 创建矩形框
+                    rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1,
+                                             edgecolor=colors[key % 10],
+                                             facecolor='none')
+                    plt.text(x_min, y_min, f'{objs_id[key]}',
+                             fontdict={'family': 'serif', 'size': 10, 'color': 'green'}, ha='center',
+                             va='center')
+                    plt.gca().add_patch(rect)
+            else:
+                x_max = max(p[0] for p in value)
+                y_max = max(p[1] for p in value)
+                x_min = min(p[0] for p in value)
+                y_min = min(p[1] for p in value)
+                # 在指定的位置绘制方框
+                # 创建矩形框
+                rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1,
+                                         edgecolor=colors[key % 10],
+                                         facecolor='none')
+
+                plt.text(x_min, y_min, f'{objs_id[key]}',
+                             fontdict={'family': 'serif', 'size': 10, 'color': 'green'},
+                             ha='center',
+                             va='center')
+                plt.gca().add_patch(rect)
+            # point1 = min(value, key=lambda x: (x[0], x[1]))
+            # point2 = max(value, key=lambda x: (x[0], x[1]))
+            # width = point2[1] - point1[1]
+            # height = point2[0] - point1[0]
+            # rect = patches.Rectangle((0, 255), 15, 30, linewidth=1, edgecolor='g',
+            #                          facecolor='none')
+
+            # 将矩形框添加到图像中
+            # plt.gca().add_patch(rect)
+
+        plt.show()
+        # return cur_obstacle_world_points

robowaiter/scene/tasks/AEM.py

@@ -11,6 +11,8 @@ import pickle
 
 import numpy as np
 from matplotlib import pyplot as plt
+plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
+plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
 from sklearn.cluster import DBSCAN
 
 from robowaiter.scene.scene import Scene
@@ -21,21 +23,20 @@ class SceneAEM(Scene):
     def _reset(self):
         pass
     def _run(self):
+        print(len(self.status.objects))
         # 创建一个从白色（1）到灰色（0）的 colormap
+        objs = self.status.objects
         cur_objs = []
         cur_obstacle_world_points = []
-        objs_name_set = set()
         visited_obstacle = set()
         obj_json_data = []
-        db = DBSCAN(eps=4, min_samples=2)
+        obj_count = 0
+        added_info = 0
+
+
 
         map_ratio = 3
-        # # 创建一个颜色映射，其中0表示黑色，1表示白色
+        db = DBSCAN(eps=map_ratio, min_samples=int(map_ratio / 2))
-        # 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):
@@ -51,11 +52,15 @@ class SceneAEM(Scene):
         # navigation_test(i,map_id)
         map_map = np.zeros((math.ceil(950 / map_ratio), math.ceil(1850 / map_ratio)))
 
+        # self.add_walker(0, 30, 520, )
+        # self.add_walker(10, 30, 420)
         while True:
-            goal = self.explore(map, 120)  # cur_pos 指的是当前机器人的位置，场景中应该也有接口可以获取
+            walker_count = 0
+            fig = plt.figure()
+            goal = self.explore(map, 120)
             if goal is None:
                 break
-            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, db,0, 11)
+            cur_obstacle_world_points, cur_objs_id= self.navigation_move(plt, cur_objs, cur_obstacle_world_points, [[goal[0], goal[1]]], map_ratio, db,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:
@@ -66,34 +71,66 @@ class SceneAEM(Scene):
             #            extent=(-400 / map_ratio, 1450 / map_ratio,
             #                    -350 / map_ratio, 600 / map_ratio))
 
-            # 使用imshow函数绘制图像，其中cmap参数设置颜色映射
+            for i in range(len(cur_objs_id)):
+                if cur_objs_id[i] == "walker":
+                    walker_count += 1
+                for obj in objs:
+                    if obj.name == cur_objs_id[i] and obj not in cur_objs:
+                        cur_objs.append(obj)
+                        break
+
+            plt.subplot(2, 1, 2)  # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
             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.title("地图构建过程")
+
+            plt.subplot(2, 7, 14)  # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
+
+            # plt.text(0, 0.7, f'检测行人数量：{walker_count}', fontsize=10)
+
+            new_add_info = len(cur_objs) - added_info + walker_count
+            plt.text(0, 0.5, f'新增语义信息：{new_add_info}', fontsize=10)  # 在图中添加文字，x和y坐标是在这个图片大小内的相对位置，fontsize是字体大小
+            added_info += new_add_info
+            plt.text(0, 0.3, f'已存语义信息：{added_info}', fontsize=10)  # 在图中添加文字，x和y坐标是在这个图片大小内的相对位置，fontsize是字体大小
+            plt.axis("off")
             plt.show()
+            print("------------当前检测到的物品信息--------------")
+            print(cur_objs)
             time.sleep(1)
-        print("------------物品信息--------------")
+
-        print(cur_objs)
 
         for i in range(len(cur_objs)):
-            obj_json_data.append({"id":f"{i}", "name":f"{cur_objs[i].name}", "location":f"{cur_objs[i].location}", "height":f"{cur_objs[i].location.Z * 2}"})
+            if cur_objs[i].name == "Desk" or cur_objs[i].name == "Chair":
+                obj_json_data.append({"id":f"{i}", "name":f"{cur_objs[i].name}", "location":f"{cur_objs[i].location}", "height":f"{cur_objs[i].location.Z * 2}"})
+
+            else:
+                obj_json_data.append(
+                    {"id": f"{i}", "name": f"{cur_objs[i].name}", "location": f"{cur_objs[i].location}",
+                     "height": f"{cur_objs[i].location.Z}"})
 
         with open('../../proto/objs.json', 'w') as file:
             json.dump(obj_json_data, file)
+
         # 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',
+        # plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower',
-                   extent=(-400 / map_ratio, 1450 / map_ratio,
+        #            extent=(-400 / map_ratio, 1450 / map_ratio,
-                           -350 / map_ratio, 600 / map_ratio))
+        #                    -350 / map_ratio, 600 / map_ratio))
         # plt.axis('off')
-        plt.show()
+        # plt.show()
         print("已绘制完成地图！！！")
+        print("------------检测到的所有物品信息--------------")
+        print(obj_json_data)
+        # self.add_walker(0, 30, 520, )
+        # self.add_walker(10, 30, 420)
+        # Scene.get_obstacle_point(db, Scene.status, map_ratio)
 
 
 if __name__ == '__main__':

robowaiter/scene/tasks/CafeDailyOperations/VLN_all.py

@@ -0,0 +1,150 @@
+"""
+视觉语言操作
+机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
+"""
+
+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 = self.st2 + 65
+        self.st2 = 3
+        self.st3 = 3
+        self.st4 = 3
+
+        self.signal_event_list = [
+
+            # 场景1：带小女孩找阳光下的空位
+            (3, self.add_walker, (5, 230, 1200)), # 0号"Girl02_C_3"
+            (1, self.control_walker, (0, False, 200, 60, 520, 0)),
+            (9, self.customer_say, (0, "早上好呀，我想找个能晒太阳的地方。")),
+            (-1, self.customer_say, (0,"可以带我过去嘛？")),
+            (0, self.control_walker, (0, False, 50, 140, 1200, 180)), # 小女孩站在了 BrightTable1 旁边就餐啦
+
+
+            # # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
+            # (3, self.add_walker, (5, 230, 1200)), # 小女孩
+            # # # 上述准备
+            (10, self.add_walker, (26, -28, -150, 90)),
+            (0, self.add_walker, (10, -70, -200, -45)),
+            (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
+            (8, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])), # 20 胖胖男到了 BrightTable6
+            (2, self.customer_say, (1, "咖啡有哪些呢？")),# 10
+            (2, self.customer_say, (1,"来杯卡布奇诺吧。")), # 15
+
+            # # 场景3：有位女士要杯水和冰红茶
+            (30 ,self.add_walkers,([[[21, 65, 1000, -90],[32, -80, 850, 135],[1, 60, 420, 135]]])),
+            (0, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走
+            (0, self.add_walkers, ([[[48, 60, 520,0], [31, 60, 600, -90], [20, 60, 680, -90],[9, 60, 760, -90],[29, -290, 400, 180]]])),
+
+            (5, self.customer_say, (6, "哎呦，今天这么多人，还有空位吗？")),# 女士问
+            (15, self.customer_say, (6, "我带着孩子呢，想要宽敞亮堂的地方。")),  # 女士问
+            # 好的，我明白了，那么您可以选择我们的家庭亲子座，这样可以容纳您的孩子，并且更加宽敞舒适。
+            # 这里可以加一下自主导航和探索，找到一个位置
+            # 好的，我明白了，那么我们推荐您到大厅的桌子，那里的空间比较宽敞，环境也比较明亮，适合带着孩子一起用餐。
+            (8, self.customer_say, (6, "大厅的桌子好啊，快带我去呀！")),
+            (15, self.control_walker, (6, False, 50,-250, 480, 0)), #  #290, 400
+            (3, self.customer_say, (6, "我想来杯水，帮我孩子拿个酸奶吧。")),
+            # ### 9号灰色男 排队排着排着，不排了
+            (0, self.control_walker, (9, False, 100, 100, 760, 180)),
+            (0, self.control_walker, (9, True, 100, 0, 0, 180)),
+            # # ### 增加场景，孩子说热要开空调 或者9号随机游走和说
+            (90, self.customer_say, (6, "谢谢！不用了。")), #倒水+取放酸奶 90s
+
+
+
+            # # # 场景4：三人排队点单，女士要保温杯
+            (6, self.control_walkers_and_say, ([[[7, False, 100, 60, 520, 180, "我昨天保温杯好像落在你们咖啡厅了，你看到了吗？"]]])),
+            (5, self.customer_say, (7,"你可以帮我拿来吗，我在前门的桌子前等你。")),
+            (1, self.control_walker,(7, False, 80, -10, 520, 90)),# 红女士在吧台前后退一步
+            (1, self.control_walker, (7, False, 80, 240, 1000, -45)), # 红女士走到Table1前
+            (1, self.control_walker, (8, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
+            (2, self.control_walker, (9, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
+            (6, self.customer_say, (7,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
+            (5, self.customer_say, (7, "不用了。")),  # 红女士在Table1前
+
+
+
+            # # 场景5：三人排队点单，一人要冰红茶，一个要点心，一个没座位了赠送保温杯
+            # # 场景6：大胖男点了外卖，离开啦
+            (9, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 0, "给我来份午餐套餐。"]]])), #原来写了26s
+            (0, self.animation_reset,()), #重置任务
+            (6, self.customer_say, (8, "打包吧，快点！")),
+            (2, self.control_walker, (9, False, 100, 60, 620, -90)),  # 男灰黑色排队往前走一步
+            (100, self.customer_say, (8, "谢啦，我赶时间！")), #100这个时间很合适
+            (2, self.control_walker, (8, False, 250, 20, 520, -90)), # 大胖男着急得离开啦
+            (2, self.control_walker, (8, False, 250, 240, -150, -90)),
+            (5, self.remove_walkers, ([[0,7,8]])),
+
+
+            # 场景7：最后排队那个随机游走 9号变为6号，随机游走。
+            # 机器人自主发现任务，走一圈去擦桌子/拖地，碰到灰色男问好，灰色男说“太阳大，要关窗帘和空调调低”
+            # 开了空调
+            (2, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
+            (2, self.customer_say, (6, "好热呀！太阳也好大！")),
+            (10, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
+
+            # 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
+            (3, self.clean_walkers, ()),
+            (1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
+            (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),
+            (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "不用了。"]]])),
+
+        ]
+
+    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,"谢谢！"]])
+                self.scene_flag += 1
+
+        # 获得所有顾客的名字
+        # print("=================")
+        # for cus in self.status.walkers:
+        #     print(cus)
+        # print("=================")
+        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.py

@@ -34,33 +34,32 @@ class SceneVLM(Scene):
 
 
             # # 场景2：有个胖胖男人点单交流并要咖啡，帮他送去角落的桌子
-            # (3, self.add_walker, (5, 230, 1200)), # 小女孩
+            (3, self.add_walker, (5, 230, 1200)), # 小女孩
             # # # 上述准备
-            # (10, self.add_walker, (26, -28, -150, 90)),
+            (10, self.add_walker, (26, -28, -150, 90)),
-            # (0, self.add_walker, (10, -70, -200, -45)),
+            (0, self.add_walker, (10, -70, -200, -45)),
-            # (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
+            (6, self.customer_say, (1, "嘿，RoboWaiter，过来一下！")),
-            # (8, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])), # 20 胖胖男到了 BrightTable6
+            (8, self.control_walkers_and_say, ([[[1, False, 100, -18, -200, -90, "你们这有什么饮料嘛？"]]])), # 20 胖胖男到了 BrightTable6
-            # (2, self.customer_say, (1, "咖啡有哪些呢？")),# 10
+            (2, self.customer_say, (1, "咖啡有哪些呢？")),# 10
-            # (2, self.customer_say, (1,"来杯卡布奇诺吧。")), # 15
+            (2, self.customer_say, (1,"来杯卡布奇诺吧。")), # 15
-            # (2, self.customer_say, (1, "来杯卡布奇诺吧。")),
 
 
             # # 场景3：有位女士要杯水和冰红茶
-            # (3, self.add_walker, (5, 230, 1200)),
+            # (30, self.add_walker, (5, 230, 1200)),
             # (0, self.add_walker, (26, -30, -200, -90)),
             # (0, self.add_walker, (10, -80, -180, -45)),
             # # # # 上述准备
-            # # (0, self.add_walker, (21, 65, 1000, -90)),  # 男 'BrightTable2': (65, 1000, 135),
+            # (0, self.add_walker, (21, 65, 1000, -90)),  # 男 'BrightTable2': (65, 1000, 135),
-            # # (0, self.add_walker, (32, -80, 850, 135)),  # 女 'BrightTable3': (-80, 850, 135),
+            # (0, self.add_walker, (32, -80, 850, 135)),  # 女 'BrightTable3': (-80, 850, 135),
-            # # (0, self.add_walker, (1, 60, 420, 135)), # 生成小男孩随机游走
+            # (0, self.add_walker, (1, 60, 420, 135)), # 生成小男孩随机游走
-            # (0,self.add_walkers,([[[21, 65, 1000, -90],[32, -80, 850, 135],[1, 60, 420, 135]]])),
+            (0,self.add_walkers,([[[21, 65, 1000, -90],[32, -80, 850, 135],[1, 60, 420, 135]]])),
-            # (0, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走
+            (0, self.control_walker, (5, True, 50, 250, 1200, 180)), #设置id=4 的2小男孩随机游走红随机游走
-            # # (0, self.add_walker, (48, 60, 520,0)),# 生成他妈妈
+            (0, self.add_walker, (48, 60, 520,0)),# 生成他妈妈
-            # # (0, self.add_walker, (31, 60, 600, -90)),  # 女红色排队 7号
+            # (0, self.add_walker, (31, 60, 600, -90)),  # 女红色排队 7号
-            # # (0, self.add_walker, (20, 60, 680, -90)),  # 大胖男色排队 8号
+            # (0, self.add_walker, (20, 60, 680, -90)),  # 大胖男色排队 8号
-            # # (0, self.add_walker, (9, 60, 760, -90)),  # 男灰黑排队 9号
+            # (0, self.add_walker, (9, 60, 760, -90)),  # 男灰黑排队 9号
-            # # (0, self.add_walker, (29, -290, 400, 180)),  # 青色女人占了位置 BrightTable5
+            # (0, self.add_walker, (29, -290, 400, 180)),  # 青色女人占了位置 BrightTable5
-            # (0, self.add_walkers, ([[[48, 60, 520,0], [31, 60, 600, -90], [20, 60, 680, -90],[9, 60, 760, -90],[29, -290, 400, 180]]])),
+            (0, self.add_walkers, ([[[48, 60, 520,0], [31, 60, 600, -90], [20, 60, 680, -90],[9, 60, 760, -90],[29, -290, 400, 180]]])),
             #
             # # (5, self.customer_say, (6, "哎呦，今天这么多人，还有空位吗？")),# 女士问
             # # (15, self.customer_say, (6, "我带着孩子呢，想要宽敞亮堂的地方。")),  # 女士问
@@ -100,47 +99,48 @@ class SceneVLM(Scene):
             # (1, self.control_walker, (8, False, 100, 60, 600, -90)), # 大胖男排队往前走一步
             # (2, self.control_walker, (9, False, 100, 60, 680, -90)), # 男灰黑色排队往前走一步
             # (6, self.customer_say, (7,"就是这个杯子！找到啦，好开心！")), # 红女士在Table1前
-            # (5, self.customer_say, (7, "不用了")),  # 红女士在Table1前
+            # (5, self.customer_say, (7, "不用了。")),  # 红女士在Table1前
 
 
 
-            # # 场景5：三人排队点单，一人要冰红茶，一个要点心，一个没座位了赠送保温杯
+            # # # 场景5：三人排队点单，一人要冰红茶，一个要点心，一个没座位了赠送保温杯
-            (3, self.add_walker, (5, 230, 1200)),
+            # (3, self.add_walker, (5, 230, 1200)),
-            (0, self.add_walker, (26, -30, -200, -90)),
+            # (0, self.add_walker, (26, -30, -200, -90)),
-            (0, self.add_walker, (10, -80, -180, -45)),
+            # (0, self.add_walker, (10, -80, -180, -45)),
-            (0, self.add_walker, (21, 65, 1000, -90)),  # 男 'BrightTable2': (65, 1000, 135),
+            # (0, self.add_walker, (21, 65, 1000, -90)),  # 男 'BrightTable2': (65, 1000, 135),
-            (0, self.add_walker, (32, -80, 850, 135)),
+            # (0, self.add_walker, (32, -80, 850, 135)),
-            (0, self.add_walker, (1, 60, 220, 135)),
+            # (0, self.add_walker, (1, 60, 220, 135)),
-            (0, self.add_walker, (48, 60, 320, 0)), # 生成他妈妈
+            # (0, self.add_walker, (48, 60, 320, 0)), # 生成他妈妈
-            (0, self.add_walker, (31, 280, 1200, -45)), # # 女红色排队 7号找保温杯的顾客
+            # (0, self.add_walker, (31, 280, 1200, -45)), # # 女红色排队 7号找保温杯的顾客
-            (0, self.add_walker, (20, 60, 680, -90)),  # 大胖男排队
+            # (0, self.add_walker, (20, 60, 680, -90)),  # 大胖男排队
-            (0, self.add_walker, (9, 60, 760, -90)), # 男灰黑色排队
+            # (0, self.add_walker, (9, 60, 760, -90)), # 男灰黑色排队
-            (0, self.add_walker, (29, -290, 400, 180)), # # 青色女人占了位置 BrightTable5
+            # (0, self.add_walker, (29, -290, 400, 180)), # # 青色女人占了位置 BrightTable5
-            # # 上述准备
+            # # # 上述准备
-            # # 场景6：大胖男点了外卖，离开啦
+            # # # 场景6：大胖男点了外卖，离开啦
-            # (9, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 0, "给我来份午餐套餐。"]]])), #原来写了26s
+            # # (9, self.control_walkers_and_say, ([[[8, False, 100, 60, 520, 0, "给我来份午餐套餐。"]]])), #原来写了26s
-            (0, self.animation_reset,()), #重置任务
+            # (0, self.animation_reset,()), #重置任务
-            # (6, self.customer_say, (8, "打包吧，快点！")),
+            # # (6, self.customer_say, (8, "打包吧，快点！")),
-            # (2, self.control_walker, (9, False, 100, 60, 620, -90)),  # 男灰黑色排队往前走一步
+            # # (2, self.control_walker, (9, False, 100, 60, 620, -90)),  # 男灰黑色排队往前走一步
-            # # (100, self.customer_say, (8, "谢啦，我赶时间！")), #100这个时间很合适
+            # # # (100, self.customer_say, (8, "谢啦，我赶时间！")), #100这个时间很合适
-            # (2, self.control_walker, (8, False, 250, 20, 520, -90)), # 大胖男着急得离开啦
+            # # (2, self.control_walker, (8, False, 250, 20, 520, -90)), # 大胖男着急得离开啦
-            # (2, self.control_walker, (8, False, 250, 240, -150, -90)),
+            # # (2, self.control_walker, (8, False, 250, 240, -150, -90)),
-            (5, self.remove_walkers, ([[0,7,8]])),
+            # (5, self.remove_walkers, ([[0,7,8]])),
-            # (2, self.control_walker, (6, False, 100, 60, 520, 0)),  # 9号变7号 男灰黑色排队往前,轮到他
+            # # (2, self.control_walker, (6, False, 100, 60, 520, 0)),  # 9号变7号 男灰黑色排队往前,轮到他
-
+            #
-
+            #
-            # 场景7：最后排队那个随机游走 9号变为6号，随机游走。
+            # # 场景7：最后排队那个随机游走 9号变为6号，随机游走。
-            # 机器人自主发现任务，走一圈去擦桌子/拖地，碰到灰色男问好，灰色男说“太阳大，要关窗帘和空调调低”
+            # # 机器人自主发现任务，走一圈去擦桌子/拖地，碰到灰色男问好，灰色男说“太阳大，要关窗帘和空调调低”
-            # 开了空调
+            # # 开了空调
-            (2, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
+            # (2, self.control_walker, (6, False, 100, 60, 520, 0)), # 10号变7号 男灰黑色排队往前,轮到他
-            # (2, self.customer_say, (6, "好热呀！太阳也好大！")),
+            # # (2, self.customer_say, (6, "好热呀！太阳也好大！")),
-            # (10, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
+            # # (10, self.control_walkers_and_say, ([[[6, True, 100, 60, 520, 0, "谢谢，这下凉快了"]]])),
-
+            #
-
+            #
-            # 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
+            # # 场景8 结束了，删除所有顾客。此处增加自主探索发现空间比较暗，打开大厅灯
-            (3, self.clean_walkers, ()),
+            # (3, self.clean_walkers, ()),
-            (1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
+            # (1, self.add_walker, (17, 60, 1000)),# 增加警察，提醒下班啦
-            (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),
+            # (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "下班啦！别忘了打扫卫生。"]]])),
+            # (3, self.control_walkers_and_say, ([[[0, False, 150, 60, 520, 0, "不用了。"]]])),
 
 
 

robowaiter/scene/tasks/VLM/VLM_1_order.py

@@ -0,0 +1,45 @@
+"""
+视觉语言操作
+机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
+"""
+
+import time
+from robowaiter.scene.scene import Scene
+
+class SceneVLM(Scene):
+    def __init__(self, robot):
+        super().__init__(robot)
+        # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
+        self.signal_event_list = [
+            (3, self.add_walker,  (20,0,700)),
+            (1, self.control_walker, (6, False,100, 60, 520,0)),
+            (1, self.customer_say, (6, "给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。")),
+            (5, self.control_walker, (6, False, 100, -250, 480, 0)),
+        ]
+
+    def _reset(self):
+        self.gen_obj()
+
+        self.add_walkers([[4,1, 880], [31,250, 1200],[6,-55, 750],[10,70, -200],[27,-290, 400, 180],[26, 60,-320,90]])
+        self.control_walkers(walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]],is_autowalk = True)
+
+        pass
+
+    def _run(self, op_type=10):
+
+        pass
+    
+    def _step(self):
+        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/VLM/VLM_2_AC.py

@@ -0,0 +1,43 @@
+"""
+视觉语言操作
+机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
+"""
+
+import time
+from robowaiter.scene.scene import Scene
+
+class SceneVLM(Scene):
+    def __init__(self, robot):
+        super().__init__(robot)
+        # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
+        self.signal_event_list = [
+            (3, self.add_walker,  (0,0,700)),
+            (1, self.control_walker, (6, False,100, 60, 520,0)), #[walkerID,autowalk,speed,X,Y,Yaw]
+            (1, self.customer_say, (6, "好热呀，想开空调，想要温度调低点！")),
+            (5, self.control_walker, (6, False, 200, 60, 80, 0)),
+            (-1, self.customer_say, (6, "谢谢！这下凉快了！")),  #(-100,600)
+        ]
+
+    def _reset(self):
+        self.gen_obj()
+        self.add_walkers([[4,1, 880], [31,250, 1200],[6,-55, 750],[10,70, -200],[27,-290, 400, 180],[26, 60,-320,90]])
+        self.control_walkers(walker_loc=[[-55, 750], [70, -200]],is_autowalk = True)
+        pass
+
+    def _run(self, op_type=10):
+        pass
+    
+    def _step(self):
+        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/VLM/VLM_AC.py

@@ -0,0 +1,142 @@
+"""
+视觉语言操作
+机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板
+"""
+
+import time
+from robowaiter.scene.scene import Scene
+
+class SceneVLM(Scene):
+    def __init__(self, robot):
+        super().__init__(robot)
+        # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
+        self.signal_event_list = [
+            (3, self.add_walker,  (0,0,700)),
+            (1, self.control_walker, (6, False,100, 60, 520,0)), #[walkerID,autowalk,speed,X,Y,Yaw]
+            (3, self.customer_say, (6, "好热呀，想开空调，想要温度调低点！")),
+            (5, self.control_walker, (6, False, 200, 60, 80, 0)),
+            (-1, self.customer_say, (6, "谢谢！这下凉快了！")),  #(-100,600)
+
+            # 有人提出要开空调和关窗帘
+            # bar (60, 520)
+            # (28, self.add_walker, (0, 0, 0)),
+            # (33, self.control_walker, (7, False, 100, 60, 520, 180)),
+            # (35, self.customer_say, (7,"好热呀！太阳也好大！")),
+            # (45, 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):
+        self.gen_obj()
+        # self.state["condition_set"] = {'At(Robot,Bar)', 'Is(AC,Off)',
+        #  'Holding(Nothing)','Exist(Yogurt)','Exist(Softdrink)','On(Yogurt,Bar)','On(Softdrink,Table1)',
+        #  'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
+        #  'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
+        # # 随机生成4个自由行走，一个在 BrightTable4,BrightTable5(-20,220)
+        self.add_walkers([[4,1, 880], [31,250, 1200],[6,-55, 750],[10,70, -200],[27,-290, 400, 180],[26, 60,-320,90]])
+        # [3,1, 880] 1号桌旁边小女孩
+        # [31,250, 1200] 最角落QuietTable1女红色
+        # [6,-55, 750] 1号桌附近小男孩
+        # [10,70, -200]  另一边角落 QuietTable2 男黄色
+        # [27,-290, 400, 180] 中间 BrightTable4 女灰
+        # [26, 60,-320,90] 另一边角落 BrightTable5 红胖男
+        self.control_walkers(walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]],is_autowalk = True)
+
+        # 0-3男孩  4-7女孩 8-26男
+        # 3男孩 31女红  32女灰  10黄色衣服男瘦  9男灰瘦 26红胖男
+        # 17 是员工 police
+        # [0, -150,180]
+
+
+        # self.control_walkers(walker_loc=[[-55, 750]],is_autowalk = False)
+        # 在场景中随机增加一堆行人。
+        # walker_loc = [[-55, 750], [70, -200], [250, 1200], [0, 880]]
+        # controls = []
+        # for i in range(len(s.walkers)):
+        #     loc = walker_loc[i]
+        #     is_autowalk = False
+        #     pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180)
+        #     controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=200, pose=pose))
+        # scene = sim_client.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=scene_id))
+
+        # 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)
+        pass
+
+    def _run(self, op_type=10):
+        # 共17个操作
+        # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子",    # 1-7
+        # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘",            # 8-12
+        # "调整空调开关","调高空调温度","调低空调温度",                   # 13-15
+        # "抓握物体","放置物体"                                       # 16-17
+        #
+        # self.gen_obj()
+        # if op_type <=15:
+        #     self.move_task_area(op_type)
+        #     self.op_task_execute(op_type)
+        # if op_type == 16:   # 16: 抓操作需要传入物品id
+        #     self.move_task_area(op_type, obj_id=0)
+        #     self.op_task_execute(op_type, obj_id=0)
+        # # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 100.0], 水杯桌:[-70.0, 500.0, 107]
+        # # 桌子1:[-55.0, 0.0, 107],抹布桌:[340.0, 900.0, 99.0]   # 桌子2:[-55.0, 150.0, 107],
+        # if op_type == 17:   # 17: 放操作需要传入放置位置周围的可达区域
+        #     pos = [240.0, 40.0, 100.0]
+        #     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
+    
+    def _step(self):
+        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/VLM/VLM_order.py

@@ -12,10 +12,11 @@ class SceneVLM(Scene):
         # 在这里加入场景中发生的事件， (事件发生的时间，事件函数)
         self.signal_event_list = [
             (3, self.add_walker,  (20,0,700)),
-            (1, self.control_walker, (4, False,100, 60, 520,0)), #[walkerID,autowalk,speed,X,Y,Yaw]
+            (1, self.control_walker, (6, False,100, 60, 520,0)), #[walkerID,autowalk,speed,X,Y,Yaw]
             # (10, self.customer_say, (6,"给我来杯酸奶和咖啡，哦对，再倒一杯水。")),
-            (-1, self.customer_say, (4, "来杯酸奶吧。")),
+            # (-1, self.customer_say, (6, "来杯酸奶吧。")),
-            (-1, self.control_walker, (4, False, 100, -250, 480, 0)),  #(-100,600)
+            (3, self.customer_say, (6, "给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。")),
+            (5, self.control_walker, (6, False, 100, -250, 480, 0)),  #(-100,600)
 
             # 有人提出要开空调和关窗帘
             # bar (60, 520)
@@ -24,19 +25,19 @@ class SceneVLM(Scene):
             # (35, self.customer_say, (7,"好热呀！太阳也好大！")),
             # (45, self.control_walkers_and_say, ([[[7, False, 100, 270, -240, -65, "谢谢，这下凉快了！"]]])),
 
-            (3, self.add_walker, (0, 0, 0)),
+            # (3, self.add_walker, (0, 0, 0)),
-            (1, self.control_walker, (5, False, 100, 60, 520, 180)),
+            # (1, self.control_walker, (5, False, 100, 60, 520, 180)),
-            (1, self.customer_say, (5,"好热呀！太阳也好大！")),
+            # (1, self.customer_say, (5,"好热呀！太阳也好大！")),
-            (-1, self.control_walkers_and_say, ([[[5, False, 100, 270, -240, -65, "谢谢，这下凉快了！"]]])),
+            # (-1, self.control_walkers_and_say, ([[[5, False, 100, 270, -240, -65, "谢谢，这下凉快了！"]]])),
         ]
 
     def _reset(self):
         self.gen_obj()
-        self.state["condition_set"] = {'At(Robot,Bar)', 'Is(AC,Off)',
+        # self.state["condition_set"] = {'At(Robot,Bar)', 'Is(AC,Off)',
-         'Holding(Nothing)','Exist(Yogurt)','Exist(Softdrink)','On(Yogurt,Bar)','On(Softdrink,Table1)',
+        #  'Holding(Nothing)','Exist(Yogurt)','Exist(Softdrink)','On(Yogurt,Bar)','On(Softdrink,Table1)',
-         'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
+        #  'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
-         'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
+        #  'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
-        # 随机生成4个自由行走，一个在 BrightTable4,BrightTable5(-20,220)
+        # # 随机生成4个自由行走，一个在 BrightTable4,BrightTable5(-20,220)
         self.add_walkers([[4,1, 880], [31,250, 1200],[6,-55, 750],[10,70, -200],[27,-290, 400, 180],[26, 60,-320,90]])
         # [3,1, 880] 1号桌旁边小女孩
         # [31,250, 1200] 最角落QuietTable1女红色

robowaiter/utils/bt/load.py

@@ -13,6 +13,7 @@ def load_bt_from_ptml(scene, ptml_path, behavior_lib_path):
 
     print(f'BT loaded:')
     print_tree_from_root(bt.root)
+    # print("行为树子树加载完毕")
     # print(ptree.display.unicode_tree(root=bt.root, show_status=True))
     return bt