diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index d20804f..e23ab17 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/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=''' diff --git a/robowaiter/behavior_lib/act/ResolveAnomaly.py b/robowaiter/behavior_lib/act/ResolveAnomaly.py new file mode 100644 index 0000000..dfce25c --- /dev/null +++ b/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 diff --git a/robowaiter/behavior_lib/cond/AnomalyDetected.py b/robowaiter/behavior_lib/cond/AnomalyDetected.py new file mode 100644 index 0000000..728b87e --- /dev/null +++ b/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 diff --git a/robowaiter/proto/camera.py b/robowaiter/proto/camera.py index c310598..688efa8 100644 --- a/robowaiter/proto/camera.py +++ b/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 diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 1a0d231..1d1f43e 100644 --- a/robowaiter/scene/scene.py +++ b/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": diff --git a/robowaiter/scene/tasks/AEM.py b/robowaiter/scene/tasks/AEM.py index 5dc7603..1e9b2e4 100644 --- a/robowaiter/scene/tasks/AEM.py +++ b/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', - extent=(-400 / map_ratio, 1450 / map_ratio, - -350 / map_ratio, 600 / 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)) # plt.axis('off') - plt.show() + # plt.show() print("已绘制完成地图！！！") + print("------------检测到的所有物品信息--------------") + print(obj_json_data) if __name__ == '__main__':