在操作任务中增加了 视觉图像分割

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

@@ -41,6 +41,8 @@ class PutDown(Act):
 
         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

@@ -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/NewCustomerComing.py

@@ -14,6 +14,8 @@ class NewCustomerComing(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/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

@@ -15,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(
@@ -35,7 +41,7 @@ def init_world(scene_num=1, mapID=11):
     time.sleep(3)  # wait for the map to load
 
 def get_camera(part, scene_id=0):
-    print('------------------get_camera----------------------')
+    # print('------------------get_camera----------------------')
     action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id)
     return stub.Capture(action)
 
@@ -107,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:
@@ -576,7 +586,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
@@ -595,6 +615,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:  # 开关窗帘不需要移动
@@ -642,6 +672,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
@@ -672,6 +711,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":
@@ -721,6 +773,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)
@@ -1093,6 +1158,8 @@ class Scene:
         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)

robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py

@@ -42,7 +42,6 @@ class SceneVLM(Scene):
             (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
-            (2, self.customer_say, (1, "来杯卡布奇诺吧。")),
 
 
             # # 场景3：有位女士要杯水和冰红茶

robowaiter/scene/tasks/VLM/VLM_1_order.py

@@ -13,7 +13,7 @@ class SceneVLM(Scene):
         self.signal_event_list = [
             (3, self.add_walker,  (20,0,700)),
             (1, self.control_walker, (6, False,100, 60, 520,0)),
-            (3, self.customer_say, (6, "给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。")),
+            (1, self.customer_say, (6, "给我来杯酸奶和冰红茶，我坐在对面的桌子那儿。")),
             (5, self.control_walker, (6, False, 100, -250, 480, 0)),
         ]
 

robowaiter/scene/tasks/VLM/VLM_2_AC.py

@@ -13,7 +13,7 @@ class SceneVLM(Scene):
         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, "好热呀，想开空调，想要温度调低点！")),
+            (1, self.customer_say, (6, "好热呀，想开空调，想要温度调低点！")),
             (5, self.control_walker, (6, False, 200, 60, 80, 0)),
             (-1, self.customer_say, (6, "谢谢！这下凉快了！")),  #(-100,600)
         ]
@@ -21,7 +21,7 @@ class SceneVLM(Scene):
     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)
+        self.control_walkers(walker_loc=[[-55, 750], [70, -200]],is_autowalk = True)
         pass
 
     def _run(self, op_type=10):

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