AEM

robowaiter/behavior_lib/_base/Behavior.py

@@ -6,6 +6,7 @@ from py_trees.common import Status
 
 # _base Behavior
 class Bahavior(ptree.behaviour.Behaviour):
+
     can_be_expanded = False
     num_params = 0
     valid_params='''

robowaiter/behavior_lib/act/ResolveAnomaly.py

@@ -0,0 +1,14 @@
+import py_trees as ptree
+from typing import Any
+from robowaiter.behavior_lib._base.Act import Act
+
+class ResolveAnomaly(Act):
+
+    def __init__(self, *args):
+        super().__init__(*args)
+
+    def _update(self) -> ptree.common.Status:
+        # explore algorithm
+        self.scene.state["chat_list"].insert(0,("Goal",'Is(HallLight,On)'))
+
+        return ptree.common.Status.RUNNING

robowaiter/behavior_lib/cond/AnomalyDetected.py

@@ -0,0 +1,23 @@
+import py_trees as ptree
+from typing import Any
+from robowaiter.behavior_lib._base.Cond import Cond
+
+class AnomalyDetected(Cond):
+
+
+    def __init__(self):
+        super().__init__()
+
+
+    def _update(self) -> ptree.common.Status:
+        # if self.scene.status?
+
+        light_set = {'Is(HallLight,Off)', 'Is(TubeLight,Off)', 'Is(Curtain,Off)'}
+        if light_set.issubset(self.scene.state["condition_set"]):
+            self.scene.chat_bubble("太暗了，开灯")
+            self.scene.state["anomaly"] = "TooDark"
+            return ptree.common.Status.SUCCESS
+
+
+
+        return  ptree.common.Status.FAILURE

robowaiter/proto/camera.py

@@ -370,7 +370,7 @@ def get_id_object_pixels(id, scene):
 
 
 
-def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
+def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
     cur_obstacle_pixel_points = []
     object_pixels = {}
     colors = [
@@ -439,13 +439,16 @@ 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, 1, 1)
     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:
             continue
-        if key in [91, 84]:
+        if key in [91, 84, 96, 87, 102, 106, 120, 85,113, 101, 83]:
             X = np.array(value)
             db.fit(X)
             labels = db.labels_
@@ -487,7 +490,8 @@ def get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio):
 
         # 将矩形框添加到图像中
         # plt.gca().add_patch(rect)
-    plt.show()
+
+    # plt.show()
     return cur_obstacle_world_points
 
 

robowaiter/scene/scene.py

@@ -76,7 +76,8 @@ class Scene:
         "attention":{},
         "serve_state":{},
         "chat_history":{},
-        "wait_history":set()
+        "wait_history":set(),
+        "anomaly": None
     }
     """
     status:
@@ -766,7 +767,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, objs_name_set, 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]
@@ -782,7 +783,7 @@ class Scene:
             cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
                                                               objs_name_set)
 
-            cur_obstacle_world_points = camera.get_obstacle_point(db, scene, cur_obstacle_world_points,map_ratio)
+            cur_obstacle_world_points = camera.get_obstacle_point(plt, db, scene, cur_obstacle_world_points,map_ratio)
 
 
             # if scene.info == "Unreachable":
@@ -804,7 +805,7 @@ class Scene:
                 cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
                                                                   objs_name_set)
 
-                cur_obstacle_world_points = camera.get_obstacle_point(db, scene, cur_obstacle_world_points, map_ratio)
+                cur_obstacle_world_points = camera.get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio)
 
 
                 # if scene.info == "Unreachable":

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
@@ -51,11 +53,13 @@ class SceneAEM(Scene):
         # navigation_test(i,map_id)
         map_map = np.zeros((math.ceil(950 / map_ratio), math.ceil(1850 / map_ratio)))
 
+
         while True:
+            fig = plt.figure()
             goal = self.explore(map, 120)  # cur_pos 指的是当前机器人的位置，场景中应该也有接口可以获取
             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_objs, objs_name_set, cur_obstacle_world_points= self.navigation_move(plt, cur_objs, objs_name_set, 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:
@@ -67,33 +71,46 @@ class SceneAEM(Scene):
             #                    -350 / map_ratio, 600 / map_ratio))
 
             # 使用imshow函数绘制图像，其中cmap参数设置颜色映射
+
+            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.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)
 
 
 if __name__ == '__main__':