From 3859c810d1904a8158657586f019fc5cf38c57af Mon Sep 17 00:00:00 2001 From: liwang_zhang <2638950452@qq.com> Date: Sat, 18 Nov 2023 22:42:54 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9AEM=EF=BC=8C=E9=80=9A?= =?UTF-8?q?=E8=BF=87=E8=A7=86=E8=A7=89=E6=9E=84=E5=BB=BA=E6=9B=B4=E5=8A=A0?= =?UTF-8?q?=E7=B2=BE=E7=BB=86=E5=8C=96=E7=9A=842D=E5=9C=B0=E5=9B=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 完善了AEM，障碍物识别，２D地图构建 --- robowaiter/proto/camera.py | 155 +++++++++++++++++++++++++++++++++- robowaiter/scene/scene.py | 35 ++++---- robowaiter/scene/tasks/AEM.py | 51 ++++++++++- 3 files changed, 220 insertions(+), 21 deletions(-) diff --git a/robowaiter/proto/camera.py b/robowaiter/proto/camera.py index 8780bbb..27f8fa7 100644 --- a/robowaiter/proto/camera.py +++ b/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]: - img_data = get_camera([camera_name], i) - show_image(img_data, scene) + # for camera_name in [GrabSim_pb2.CameraName.Head_Depth]: + # img_data = get_camera([camera_name], i) + # 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) diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index b0eb1b7..bbe38f3 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -666,7 +666,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] @@ -679,6 +679,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": @@ -696,11 +698,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: @@ -741,21 +746,21 @@ class Scene: free_array = np.array(free_list) print(f"主动探索完成！以下是场景中可以到达的点：{free_array}；其余点均是障碍物不可达") - # 画地图: X行Y列，第一行在下面 - plt.clf() - plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower', - extent=(-250, 1300, - -200, 600)) - plt.show() - print("已绘制完成地图！！！") + # # 画地图: X行Y列，第一行在下面 + # plt.clf() + # plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower', + # extent=(-250, 1300, + # -200, 600)) + # plt.show() + # print("已绘制完成地图！！！") return None - # 画地图: X行Y列，第一行在下面 - plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower', - extent=(-250, 1300, - -200, 600)) - plt.show() - print("已绘制部分地图！") + # # 画地图: X行Y列，第一行在下面 + # plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower', + # extent=(-250, 1300, + # -200, 600)) + # plt.show() + # print("已绘制部分地图！") return self.getNearestFrontier(cur_pos, self.all_frontier_list) def isNewFrontier(self, pos, map): diff --git a/robowaiter/scene/tasks/AEM.py b/robowaiter/scene/tasks/AEM.py index fb04a0f..fab39ad 100644 --- a/robowaiter/scene/tasks/AEM.py +++ b/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) - - pass + # 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.axis('off') + plt.show() + print("已绘制完成地图！！！") if __name__ == '__main__':