From ee25917bc2f9ece276fb19b16e1f8f9709c33c72 Mon Sep 17 00:00:00 2001 From: ChenXL97 <908926798@qq.com> Date: Wed, 15 Nov 2023 14:39:25 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=BB=E5=8A=A8=E6=8E=A2=E7=B4=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/algos/explore/AEM.py | 180 +++++++++++++-- robowaiter/algos/explore/camera.py | 281 +++++++++++++++++++++++ robowaiter/algos/explore/explore.py | 91 -------- robowaiter/algos/explore/map_5.pkl | Bin 0 -> 562562 bytes robowaiter/algos/explore/semantic_map.py | 146 ++++++++++++ 5 files changed, 585 insertions(+), 113 deletions(-) create mode 100644 robowaiter/algos/explore/camera.py delete mode 100644 robowaiter/algos/explore/explore.py create mode 100644 robowaiter/algos/explore/map_5.pkl create mode 100644 robowaiter/algos/explore/semantic_map.py diff --git a/robowaiter/algos/explore/AEM.py b/robowaiter/algos/explore/AEM.py index 9b00980..60c5948 100644 --- a/robowaiter/algos/explore/AEM.py +++ b/robowaiter/algos/explore/AEM.py @@ -1,11 +1,15 @@ #!/usr/bin/env python3 # -*- encoding: utf-8 -*- # enconding = utf8 +import math +import os +import pickle import sys import time import grpc -from explore import Explore +import camera +import semantic_map sys.path.append('./') sys.path.append('../') @@ -14,8 +18,8 @@ import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable -from robowaiter.proto import GrabSim_pb2 -from robowaiter.proto import GrabSim_pb2_grpc +import GrabSim_pb2_grpc +import GrabSim_pb2 channel = grpc.insecure_channel('localhost:30001', options=[ ('grpc.max_send_message_length', 1024 * 1024 * 1024), @@ -24,6 +28,10 @@ channel = grpc.insecure_channel('localhost:30001', options=[ sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) +visited = set() +all_frontier_list = set() + + ''' 初始化，卸载已经加载的关卡，清除所有机器人 ''' @@ -68,12 +76,13 @@ def SetWorld(map_id=0, scene_num=1): def Observe(scene_id=0): print('------------------show_env_info----------------------') scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - print( - f"location:{[scene.location]}, rotation:{scene.rotation}\n", - f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", - f"objects number: {len(scene.objects)}, walkers number: {len(scene.walkers)}\n" - f"timestep:{scene.timestep}, timestamp:{scene.timestamp}\n" - f"collision:{scene.collision}, info:{scene.info}") + # print( + # f"location:{[scene.location]}, rotation:{scene.rotation}\n", + # f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", + # f"objects number: {len(scene.objects)}, walkers number: {len(scene.walkers)}\n" + # f"timestep:{scene.timestep}, timestamp:{scene.timestamp}\n" + # f"collision:{scene.collision}, info:{scene.info}") + return scene ''' @@ -100,33 +109,152 @@ def Reset(scene_id=0): yaw:机器人朝向; velocity:速度,>0代表移动,<0代表瞬移,=0代表只查询; dis:最终达到的位置距离目标点最远距离,如果超过此距离则目标位置不可达 + """ -def navigation_move(v_list, scene_id=0, map_id=11): +# def navigation_test(scene_id=0, map_id=11): +# print('------------------navigation_move----------------------') +# scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) +# walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw] +# print("position:", walk_value) +# +# if map_id == 11: # coffee +# v_list = [[237, 495], [232, 495], [227, 490]] +# else: +# v_list = [[0.0, 0.0]] +# +# for walk_v in v_list: +# walk_v = walk_v + [scene.rotation.Yaw - 90, 60, 0] +# print("walk_v", walk_v) +# action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) +# scene = sim_client.Do(action) +# print(scene.info) + + +def navigation_move(cur_objs, objs_name_set, v_list, scene_id=0, map_id=11): print('------------------navigation_move----------------------') scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw] print("position:", walk_value) + # if map_id == 11: # coffee # v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]] # else: # v_list = [[0.0, 0.0]] for walk_v in v_list: - walk_v = walk_v + [scene.rotation.Yaw - 90, 60, 0] + walk_v = walk_v + [scene.rotation.Yaw - 90, 250, 60] print("walk_v", walk_v) action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = sim_client.Do(action) + 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 + + +def isOutMap(pos, min_x=-350, max_x=600, min_y=-400, max_y=1450): + if pos[0] <= min_x or pos[0] >= max_x or pos[1] <= min_y or pos[1] >= \ + max_y: + return True + return False + + +def real2map(x, y): + ''' + 实际坐标->地图坐标 (向下取整) + ''' + # x = round((x - self.min_x) / self.scale_ratio) + # y = round((y - self.min_y) / self.scale_ratio) + x = math.floor((x + 350) / 5) + y = math.floor((y + 400) / 5) + return x, y + + +def explore(map, cur_pos, explore_range): + for i in range(cur_pos[0] - explore_range, cur_pos[0] + explore_range + 1): + for j in range(cur_pos[1] - explore_range, cur_pos[1] + explore_range + 1): + if isOutMap((i, j)): + continue + x, y = real2map(i, j) + if map[x, y] == 0: + visited.add((i, j)) + for i in range(cur_pos[0] - explore_range, cur_pos[0] + explore_range + 1): + for j in range(cur_pos[1] - explore_range, cur_pos[1] + explore_range + 1): + if isOutMap((i, j)): + continue + x, y = real2map(i, j) + if map[x, y] == 0: + if isNewFrontier((i, j), map): + all_frontier_list.add((i, j)) + if len(all_frontier_list) <= 400: + free_list = list(visited) + free_array = np.array(free_list) + print(f"探索完成！以下是场景中可以到达的点：{free_array}；其余点均是障碍物不可达") + return None + return getNearestFrontier(cur_pos, all_frontier_list) + + +def isNewFrontier(pos, map): + around_nodes = [(pos[0], pos[1] + 1), (pos[0], pos[1] - 1), (pos[0] - 1, pos[1]), (pos[0] + 1, pos[1])] + + for node in around_nodes: + x, y = real2map(node[0], node[1]) + if node not in visited and map[x, y] == 0: + return True + if (pos[0], pos[1]) in all_frontier_list: + all_frontier_list.remove((pos[0], pos[1])) + return False + + +# def reachable(pos, map): +# # scene = sim_client.Observe(GrabSim_pb2.SceneID(value=0)) +# # # x, y = real2map(self.area_range[0], self.area_range[2], self.scale_ratio, pos[0], pos[1]) +# # # if self.map[x, y] == 1: +# # # return True +# # # else: +# # # return False +# # loc = pos + [0, 0, 5] +# # action = GrabSim_pb2.Action(scene=0, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc) +# # scene = sim_client.Do(action) +# # # print(scene.info) +# # if (str(scene.info).find('Unreachable') > -1): +# # return False +# # else: +# # return True +# if map[pos[0], pos[1]] == 0: +# return True +# else: +# return False +def getDistance(pos1, pos2): + return math.sqrt((pos1[0] - pos2[0]) ** 2 + (pos1[1] - pos2[1]) ** 2) + + +def getNearestFrontier(cur_pos, frontiers): + dis_min = sys.maxsize + frontier_best = None + for frontier in frontiers: + dis = getDistance(frontier, cur_pos) + if dis <= dis_min: + dis_min = dis + frontier_best = frontier + return frontier_best if __name__ == '__main__': map_id = 11 # 地图编号 scene_num = 1 # 场景数量 node_list = [] # 探索点 - explorer = Explore(_explore_range=100) + explore_range = 240 # 机器人感知范围（以机器人为中心，边长为2 * _explore_range的正方形） + cur_objs = [] + objs_name_set = set() + + file_name = 'map_5.pkl' + if os.path.exists(file_name): + with open(file_name, 'rb') as file: + map = pickle.load(file) print('------------ 初始化加载场景 ------------') Init() @@ -136,20 +264,28 @@ if __name__ == '__main__': for i in range(scene_num): print('------------ 场景操作 ------------') - Observe(i) + scene = Observe(i) Reset(i) print('------------ 自主探索 ------------') - while True: + cur_objs = semantic_map.navigation_move(cur_objs,i, map_id) + print("物品列表如下：") + print(cur_objs) - # scene = sim_client.Observe(GrabSim_pb2.SceneID(value=0)) - # cur_pos =[int(scene.location.X), int(scene.location.Y), int(scene.rotation.Yaw)] - goal = explorer.explore(cur_pos) # cur_pos 指的是当前机器人的位置，场景中应该也有接口可以获取 - if goal is None: - break - node_list.append(goal) - cur_pos = goal - navigation_move(i, map_id, node_list) + + # cur_pos = [int(scene.location.X), int(scene.location.Y)] + # # print(reachable([237,490])) + # # navigation_move([[237,490]], i, map_id) + # # navigation_test(i,map_id) + # while True: + # goal = explore(map, cur_pos, explore_range) # cur_pos 指的是当前机器人的位置，场景中应该也有接口可以获取 + # if goal is None: + # break + # node_list.append(goal) + # # print(goal) + # cur_objs, objs_name_set= navigation_move(cur_objs, objs_name_set, [[goal[0], goal[1]]], i, map_id) + # print(cur_objs) + # cur_pos = goal # TODO：node_list就是机器人拍照的点，目前没有设置拍照角度，需要机器人到达一个拍照点后，前后左右各拍一张照片然后获取语义信息 diff --git a/robowaiter/algos/explore/camera.py b/robowaiter/algos/explore/camera.py new file mode 100644 index 0000000..8cdb8b1 --- /dev/null +++ b/robowaiter/algos/explore/camera.py @@ -0,0 +1,281 @@ +#!/usr/bin/env python3 +# -*- encoding: utf-8 -*- +# enconding = utf8 +import json +import string +import sys +import time +import grpc + +sys.path.append('./') +sys.path.append('../') + +import matplotlib.pyplot as plt +import numpy as np +from mpl_toolkits.axes_grid1 import make_axes_locatable + +import GrabSim_pb2_grpc +import GrabSim_pb2 + +channel = grpc.insecure_channel('localhost:30001', options=[ + ('grpc.max_send_message_length', 1024 * 1024 * 1024), + ('grpc.max_receive_message_length', 1024 * 1024 * 1024) +]) + +sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) +objects_dic = {} + +''' +初始化，卸载已经加载的关卡，清除所有机器人 +''' + + +def Init(): + sim_client.Init(GrabSim_pb2.NUL()) + + +''' +获取当前可加载的地图信息(地图名字、地图尺寸) +''' + + +def AcquireAvailableMaps(): + AvailableMaps = sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()) + print(AvailableMaps) + + +''' +1、根据mapID加载指定地图 +2、如果scene_num>1,则根据地图尺寸偏移后加载多个相同地图 +3、这样就可以在一个关卡中训练多个地图 +''' + + +def SetWorld(map_id=0, scene_num=1): + print('------------------SetWorld----------------------') + world = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) + + +''' +返回场景的状态信息 +1、返回机器人的位置和旋转 +2、返回各个关节的名字和旋转 +3、返回场景中标记的物品信息(名字、类型、位置、旋转) +4、返回场景中行人的信息(名字、位置、旋转、速度) +5、返回机器人手指和双臂的碰撞信息 +''' + + +def Observe(scene_id=0): + print('------------------show_env_info----------------------') + scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) + # print( + # f"location:{[scene.location]}, rotation:{scene.rotation}\n", + # f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", + # f"objects number: {len(scene.objects)}, walkers number: {len(scene.walkers)}\n" + # f"timestep:{scene.timestep}, timestamp:{scene.timestamp}\n" + # f"collision:{scene.collision}, info:{scene.info}") + return scene + + +''' +重置场景 +1、重置桌子的宽度和高度 +2、清除生成的行人和物品 +3、重置关节角度、位置旋转 +4、清除碰撞信息 +5、重置场景中标记的物品 +''' + + +def Reset(scene_id=0): + print('------------------Reset----------------------') + scene = sim_client.Reset(GrabSim_pb2.ResetParams(scene=scene_id)) + print(scene) + + # 如果场景支持调整桌子 + # sim_client.Reset(GrabSim_pb2.ResetParams(scene = scene_id, adjust = True, height = 100.0, width = 100.0))" + + +''' +根据传入的部位名字，获取相机数据 +''' + + +def get_camera(part, scene_id=0): + print('------------------get_camera----------------------') + action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id) + return sim_client.Capture(action) + + +''' +显示相机画面 +''' + + +def show_image(img_data, scene): + print('------------------show_image----------------------') + im = img_data.images[0] + + # 相机内参矩阵 + in_matrix = np.array( + [[im.parameters.fx, 0, im.parameters.cx], [0, im.parameters.fy, im.parameters.cy], [0, 0, 1]]) + + # 相机外参矩阵 + out_matrix = np.array(im.parameters.matrix).reshape((4, 4)) + + # # 旋转矩阵 + # rotation_matrix = out_matrix[0:3, 0:3] + # + # # 平移矩阵 + # translation_matrix = out_matrix[0:3, -1].reshape(3, 1) + + # 像素坐标 + # pixel_point = np.array([403, 212, 1]).reshape(3, 1) + pixel_x = 404 + pixel_y = 212 + depth = 369 + + # 将像素坐标转换为归一化设备坐标 + 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] + # print("物体的世界坐标：", world_coordinates) + + # 世界偏移后的坐标 + world_offest_coordinates = [world_coordinates[0] + 700, world_coordinates[1] + 1400, world_coordinates[2]] + # print("物体世界偏移的坐标: ", world_offest_coordinates) + # world_point = world_coordinates + np.array([]) + # print("物体的世界坐标为：", ) + + # # 相对机器人的世界坐标 + # world_point = rotation_matrix.T @ (in_matrix.T * 369 @ pixel_point - translation_matrix) + + + # print(world_point) + + # print(in_matrix @ out_matrix @ obj_world) + # + d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels)) + plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None) + plt.show() + + +def save_obj_info(img_data, objs_name): + items = img_data.info.split(";") + dictionary = {} + for item in items: + key, value = item.split(":") + dictionary[int(key)] = value + + im = img_data.images[0] + d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels)) + arr_flat = d.ravel() + for id in arr_flat: + if id not in dictionary: + print(id) + else: + objs_name.add(dictionary[id]) + return objs_name + + +def get_semantic_map(camera, cur_objs, objs_name): + scene = Observe(0) + objs = scene.objects._values + img_data = get_camera([camera]) + show_image(img_data, scene) + objs_name = save_obj_info(img_data, objs_name) + for obj_name in list(objs_name): + for obj in objs: + if obj.name == obj_name and obj not in cur_objs: + cur_objs.append(obj) + break + return cur_objs, objs_name + + +if __name__ == '__main__': + map_id = 11 # 地图编号 + scene_num = 1 # 场景数量 + cur_objs = [] + + print('------------ 初始化加载场景 ------------') + Init() + AcquireAvailableMaps() + SetWorld(map_id, scene_num) + time.sleep(5.0) + + for i in range(scene_num): + print('------------ 场景操作 ------------') + scene = Observe(i) + + Reset(i) + + print('------------ 相机捕获 ------------') + Reset(i) + time.sleep(1.0) + + # 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.Waist_Color, GrabSim_pb2.CameraName.Waist_Depth]: + # img_data = get_camera([camera_name], i) + # show_image(img_data, 2) diff --git a/robowaiter/algos/explore/explore.py b/robowaiter/algos/explore/explore.py deleted file mode 100644 index 021237a..0000000 --- a/robowaiter/algos/explore/explore.py +++ /dev/null @@ -1,91 +0,0 @@ -import math -import sys - -import numpy as np - - -# from rrt import RRT -# from rrt_star import RRTStar - - -# def real2map(min_x, min_y, scale_ratio, x, y): -# ''' -# 实际坐标->地图坐标 (向下取整) -# ''' -# # x = round((x - self.min_x) / self.scale_ratio) -# # y = round((y - self.min_y) / self.scale_ratio) -# x = math.floor((x - min_x) / scale_ratio) -# y = math.floor((y - min_y) / scale_ratio) -# return x, y - - -def getDistance(pos1, pos2): - return math.sqrt((pos1[0] - pos2[0]) ** 2 + (pos1[1] - pos2[1]) ** 2) - - -def getNearestFrontier(cur_pos, frontiers): - dis_min = sys.maxsize - frontier_best = None - for frontier in frontiers: - dis = getDistance(frontier, cur_pos) - if dis <= dis_min: - dis_min = dis - frontier_best = frontier - return frontier_best - - -class Explore: - def __init__(self, _explore_range): - self.explore_range = _explore_range # 机器人感知范围（以机器人为中心，边长为2 * _explore_range的正方形） - self.visited = set() - self.all_frontier_list = set() - - def explore(self, cur_pos): - for i in range(cur_pos[0] - self.explore_range, cur_pos[0] + self.explore_range + 1): - for j in range(cur_pos[1] - self.explore_range, cur_pos[1] + self.explore_range + 1): - # if self.isOutMap((i, j)): - # continue - # x, y = real2map(self.area_range[0], self.area_range[2], self.scale_ratio, i, j) - # if self.map[x, y] == 0: - if self.reachable((i, j)): - self.visited.add((i, j)) - for i in range(cur_pos[0] - self.explore_range, cur_pos[0] + self.explore_range + 1): - for j in range(cur_pos[1] - self.explore_range, cur_pos[1] + self.explore_range + 1): - # if self.isOutMap((i, j)): - # continue - # x, y = real2map(self.area_range[0], self.area_range[2], self.scale_ratio, i, j) - if self.reachable((i, j)): - if self.isNewFrontier((i, j)): - self.all_frontier_list.add((i, j)) - if len(self.all_frontier_list) == 0: - free_list = list(self.visited) - free_array = np.array(free_list) - print(f"探索完成！以下是场景中可以到达的点：{free_array}；其余点均是障碍物不可达") - return None - return getNearestFrontier(cur_pos, self.all_frontier_list) - - def isNewFrontier(self, pos): - around_nodes = [(pos[0], pos[1] + 1), (pos[0], pos[1] - 1), (pos[0] - 1, pos[1]), (pos[0] + 1, pos[1])] - - for node in around_nodes: - if node not in self.visited and self.reachable(node): - return True - if (pos[0], pos[1]) in self.all_frontier_list: - self.all_frontier_list.remove((pos[0], pos[1])) - return False - - def reachable(self, pos): - # x, y = real2map(self.area_range[0], self.area_range[2], self.scale_ratio, pos[0], pos[1]) - # if self.map[x, y] == 1: - # return True - # else: - # return False - - # TODO: 调用reachable_check函数 - pass - - # def isOutMap(self, pos): - # if pos[0] <= self.area_range[0] or pos[0] >= self.area_range[1] or pos[1] <= self.area_range[2] or pos[1] >= \ - # self.area_range[3]: - # return True - # return False diff --git a/robowaiter/algos/explore/map_5.pkl b/robowaiter/algos/explore/map_5.pkl new file mode 100644 index 0000000000000000000000000000000000000000..7b49370226f1770a974c60a3578f1c44c62dea35 GIT binary patch literal 562562 zcmeI!&8{p*QowPW!GZ-_-hq|ZERc|3hlLls02+G+fy8)57kl9y3%*A3ZqB?8&%z_{ z20%_wx;}kxpUSGr%*v17U1Z zKmWyd|M=}c{Pf+spML-H!yongB=kXt}I3xl*-~kUl?1Ql4 z!_W1Le$g-b!W=LM%mH)2954sWf%1JRB9YcKJN!~8Tq%};Z{954sW0dv3{m}w3~HTPHE z^L{4p`OIfN`#on4m;>g3Indi2h^~CT_7<;r%u#dH95n~b0dv3{FbB+mndU%L^ZCkm zzR%=4pZUyZzvIjSbHE%h2h0I;z#K3K%mH)2954sW0dv3{FbB*5bHE%h2h0I;z#K3K z%mH)299VA-g!Sv?ZQhx8=AFJb2h0I;z#K3K%z+-}KvZFWw<>o*=d z;b?sJ2VWd;NCbGm10LuDbHE%h2g=WZu&Uge+o$DjJaY7u&Dg{tUt@&wC0J= z`1(A2zyXIufCoI_fj%$?%z+W+Kvb#x`sjc88=RO3JaYCvyubkm{b4?s1LlA^U=Ea? z1JPCeuch<8>%9I+zmD)OPVS95;DAFSzylueKp&U`_soH~b=`Fp??$QcD)o@(J@XEC z+!FyF@PG&UAa@Q#w^&OP$mKulzS2Bvoi{6gd4(}J;E)LLfCoI#2WRI%bdPlu0%z;I z%5%NeaTWgZ30f$6@2Wxr|S+`q9_pCOlyZO}4Z`Si4 z>o#hhk9l|>l|0L#7Z1+H@40LB#}B_mfCoIt@gOL*TSxV>Hlg0R)Xx9z^Gox;JD;=n zm*%kgd1vcT>$%nIA^$K72OJUs9`xZsL~-9biU+j`jl$tsoVEGnp8qVqyFbs3zn6V? z^P2U3^$h~E)>mHYn+WiL2WLD8YWJZ2!&*M5cinp9i*F*p10Hs0#r)%eRd48j42M1TivJ&4=JSzGzh zib$(`+|#zNa(TD%?CL$a`Pf&+v#a$g{k*!CsVj~+CIUR*!N)uZD&wxDy>3OKbllF1 z%8kaMm2X$?8I9}c@m{T0t>;JQD*iA72OJUs9`xZsL^02L)vHz{uEy#4KKiG09a{N! z^`6ps*FJAA-`e+IJ%2C$*56;v$MaD!9B@bkc))`oF!!44c`FLJ@z}R!f41_l^gVa^ zI!hk8{7UD0^}O7;_x5dZ?CtY?kir3nM1Ti#co12vvp(~-6@h2>`+SamDpki;KGweH zDv#RnJv;wu+_j&77Wdts=jOc^XYM)n1xXxmNCbE=wg(ZtwG{#VGIqbTu3sxJTivtq zYjxk%d$xFceZ87rt>-shXV2fB_ZkFoz#$Rf!JHmMR@PPo^wFGsRJ*RN{B+OT1Zvm$ z?D=m;J?^(>d3Y{rjsp&f01w9SAfmL^CZHe2=!ahGRNGJIA0yCMS!<; zyj`7ZEnl5`j=)_$o{M(er|#nH-f#v791;N@)b$`vx0WKn*Sfy0&bO4O&M8Zvm6v6s z9_O@Hd{@6`Gh%fP@(&YmxQD~mhhDD{;O{;DuEzbErJk-upq7WV;vMDOT3nqQ25cOj z`LKQO^))X%@JIw|c@T81M+wxbhjXJ6IK1X?)KB}{5UAy8o9O$$r50c3M(sBadnRoA zyuRj!2Ofz)X%B*`b&o)4?#>J1IK0ha&r9FmOQ4j$_h!-G?WMRnCu;kQ!_mq=JICj5 zbH)dsL?G9Lpk;j|kju}$AcsR84v$`X-xqba<8E{EL8=bmTZG5&br zl?Xg~5LB#>T)Yn_aJYwqjvRx)(f$}CtNWuo%GLg;SuTg%3g@0@-!cAp;gtwH^C0M~ zw?5-`9p$iB;iKo-f36O=;g$$&9s~{R#=~p4 zfWti;bRq%>yeD7|1&h)g-V=WPzwMsw)9wBCx_!RAZl8M%A~@iX2%PmG=vns&oaO7e zAdAD>9QOS4JpwBeFh8RDS2;X$=<(cb-?PuR?;l?u_j@0NaKIrE*z+JLS+5e<QxY|#iFH1n*&E0opZE+d`2%I7C zY|b7%>)AQIef})3+&G^-U+eSSxa^Ax;4lY=TK(kQIt29HSbbN=0;eK?K+gnvnbXf0 z9gXkVIjufF8n69P863vq(5j!@(}sZl>!bhLRNE~GATR=fyXNvUHo5UUd%l;?bMvq- zDuzSf9D3<1akL?zAM5sGn<}~m0R%=SP z{dufE`>2{I5I|rC0$1m6E`!{Aa?k5^Uv8fEMFnxVheNOZBd)#)=-0mMBRT{Sn1{g8 zd7GPg?zy?=^}a7RU;CnxIJD)^d;f@UOaim_@t9Q+H3A5XMBwNg&CTuTx!eBS^LyX7 z<>NIfwK|8**Sk4vad<@l0R#|00D=1n9Q|F88}ZR|ANS|pGs?cl{Jf9Kt;S*V^{%|P z`@JH700IagfIvS4a(@>bjX3w*XZw!cH_HBJ`T9I6I4g%oKXdbZd~SQ6`~2g+?Rl>V zAbV=kGn|djt?b z009ILXis4KyItz$?YvU2rQ+)KoKl>e6BX`-!=7)saqgYtdjt?b009ILKwu+~`}^YP zzU}NnRveD{nH$&9bL~d}0R#|00D<)gR> zx^F$Qk?~b-p0CUCte3fUIeWh65I_I{1Q0-A9Rj((D@yHKhwST2OYw6~a4p5*b(Qkp z<=&rvqMZmJfB*srAh0Td+}{9p!xWKF=e700IagfWW*2j{dHw zwSQj1AJ427N9P9bXB<9WjpLs_JOA9;d#&3Co?-ll*2+dY2APxJ%=2q1s}0tl>1 zVDEQDFW;|HpI&&+7Ds7+p7AMtkMj^f009ILKwy3X&wf|*`uY47i41$?KWbd1z1cXF zKG%5&Ab_NAb_oLTk*7!&DN!)7^n6(e(6+YvDLn5%c2VucH>*)2IrxLF-8NDvE#{arX-XYM2 zz^wfwukZ&491;N@obw=_nrEHGgLx`)&dXWq*n1wcc_yE>1bX*deCBT=zyltPE-CnbGSpYy7i$CZCTJ znAKl-MZIyrAraug*d9dm^RA=&Ia_5$ug7fh&*qnWUK5zjSNTMJalj!F;K5uTL^kuU zvv@m8rDmzeEP2f0l{{V(n8i#U>uJWHiUufr_y&*GCjUK5zb zPkBULalj!F;KAG;M0V?~qkBC|#YV5g=<(0ukvxLH=st>H9TNc_@L**RqWbHtqxbzR z6&t+{qsKpsN2}*Cx`*NqgK@wi5#Yhv9z^!*t+RMPdgW%R!)SSo?v40^z-ZoySKSf; z9`Jw%ANE05VI6%wj9$Uf`HvR==)SBT|7gC6H;lyrheUt}Jb254pkp1qPe!lc==?{E ze{^3~jej)H#2cpKfI}j{10KBPLC~>o1ZL@%EuT?dXW>3d9;0|6&esG+@k^XhHym(C z1bD!MBOU}T>qcOf{@L>B{dE@JqvX-M58``GU=**!8Fj+}heUt}JUHV)P_sTGFnV7- z%d3~qN9Q|g{Jqp)9B&gC)h}^JU2(u65#Rw2j(8BXtj`FH-cQf+y8H9dxsDqD-F0_= z9)VH)5_i-W2OJUs9`N9d2SLsHjKC~?^(?>EpU=W`v^-kZ*?q?djOLSgqrN!ckO=UA z2PHfRiq<^>v-I0up0&T9h2yAs)UL1d&k-2aCviu;alj!F-~kUxcn}n=xdcYWIc7w_8=n8hP`gf}?gkO=UA2Q@tiy4I@%X6aKLf`EEnHDT@NXYofKbIk+wRL?|! z2Yq@FQCUk7n8ow6JZ8y%mOReZ(R0@b%;Jwc!UG&|NCbGmgBBhHWosz{v-Isz9<$^< zOCCq-Xa6+v+ggjj=>43_W%N8}i$Aw+_O&H2n`iO~e{jGd z5#Rw2N_Y?yt+fc~(;$mO8xFNRcJ93d^iP}q!5Qa7fCoId(}SRItwlh82U#5IaH!?2 zbLSwS@9Oj&uDB)wJm5h|4}zw(HUWJbgmEa#p|-cqUz>n_E!(en;+Y8WfCq1R5Ol1q z2pHtH92$kge8$TN z0R#|00DLz54n->UWmIR{gWjKR&me=k+y#jnnH-`+2v;=QV1A0}hD*5Bl;T zqM3JH#kXE6xbf~Kp5EiwczKOVmEy2h_oMghoj3dUxx9MikQ={!Q9&GVNCbG$mj@Be z`0FZO)vnZ0jzx~C?R|H1aDP-72OJUs9`xx!M0MZF zi?&|eSLwcP=3N=@`l@O_(e&b1FGPCbA&xSAk0XwW01xVT5U1X^s%xFPKI3{{rTe(~ z85f`DdDur4`;DR(kM1Va3kPxJ^)*g7B?3IS*MqqI{*?!9J-EN(Z6n~Ut2T-?yr7-TFTAy|2>u-29lo_icUK<9&}6C5 zWq9nQyYh8!)CmV15&<5J?LkC;-^!o1{@hpTHaGLBtxsopx3Qu8Eobv>^<4Rk^G7@L*04BCGpWUbXS+z6!UwnMZ9rD#f{t73FRzm3OUk%4M2&Qms6k8~ky= zAraugnjS>f_pQ8ZQ95qtMICU!ArasK4?gBWP-(yNv5k-I zD_iD9ezftUHRm#RTtB6C-lgulp84A+l#0_i!5Ie}5&<6Y;D`r7tNqH;x}LVLXqg*% zQP+#R`IfQd_$hbiUF!bhndd!GDn91~Zyaz)1bD!MGadxBb}LWYc-pR_Wp3g{8!ztW zT*j9CQ|`{!{cCuw&gz^9@PG#;JP3;IR^Hb2wp}I5-o%T#Ui8AZtTi8=)=Qq^Si@&^ zR_8>32Rtb0LC|cs^0%(P?J8OJCSKI_q8Gkpt$BZ1FZqdM4Ug4Xof82b@SvmzL9_kJ z<2oLMD-8`&NC`Hxb}L8xP`^_N)5Vsc-vAmc5Y|b-d_> zYbjfL$wwS>`>MX`n+WiL2Q57a+U-_;*Y&$yCF|V8kGg)`&G)P|cgOAixxG|h^-TnL zz=Iwtbxj0#z=IJyh$_`z^+TP0s9(9ZH}Is6CwFswwCL`5+&`yh>Z-1Z01tRDvIkMU z`l~)E+b8uacFzqwDeFluJU5$qiANlBdZw=Gnh5ZK2P1nB)vLeild^qMzhd{?z>~6` z;E87Lvx;+P2VfCoI#2j;+7b0DIvxANhd z5A`b3=gEAz<_muKB?3I)0T1+nIWXoNh^XtWyg24Xy-JL6GGC7Qf)`$i01tS;1ASl) zj6DY;`uZzR@;s?ui7{^AOP(({;gksQfCoI#2j;*Wb0D&)zw+hdzSJi_<_&!LxG%Wi zk_hmC2RzUR=D?hDAhN2z@~1xInQq`qR1*gr5&<6YfCu`(99UxxM3(i|)jg_5cJ7l` z_eTEVHx4)?0zBXW5A?w*b0E6C-nt6M$2{v1UE}0Op58~@alj!F-~kVKpbyM}Ip;uR zRc~Fzn|cJ-Jb4vwBV&`rMo| z2h0I;;KMl(R+L|7>$mcRocI=j+5C}DR2v5z5&<6YfCu`(9QgPgh)>I}v+yoY#)&xu zX7NWJQEeP>NCbGm10LuDbKvbc5Z{(xN9A6ggcHvY7}XzfN40UlArasK4|t#t%zk%$-<*z$pHRGpdaP4v7E{c)$aFU=CcH1MzgZwfB0I%hX9V3H0uf_@eqa z;E)LLfCoI#2j;-FIS@}<5$KbE{*JoffI}j{10L`|AD9Dk&wv1k9hP3l2CW0zBXW z5A=aKF!vmY?5qg%Nx=Mxy5N9ABESP4@IW7!19Q)T$j*vDp9IXGs0$7_Bmz9(0T1+n zIWYGei0rHg^hv<{iMrr`Ln6Qf9`HaPm;-aqfymB^K%WH6pQsBCI3xl*-~kWxfjO|| z9Ehy*SDxJCNq&XK+R3MTe8L^~M1Ti8;DJ6c2l|);5ykUWo%*QLR)^=68ta=aAFqA+ zr5@^$2=IUhJkST`K-)PGx4mESZ_EEE@As7&`+rCCw!d#L)kl320Uq#x2l~JqFb7te z15xd9SN*DASL#>3^PTU0rCy4q`dfeNZ*#yLSa}Xa_4{A- zsXkq~Px;S({`bHE%h2h0I;V1+plRqk)qfBJ8Q{^L2%dG7b7IbaT$1LnXs2cjGLS+}^nnp5VK zIb{x*1LlA^U=ElAE6jnYazCrS(|0TM9nX2rbH6jq0dv3{Fb5vzKy<}@>tj6Ln^WeL zIb{x*1LlA^U=ElAE6stZ_I;~<({C&F8{hfPcfT{u0dv3{FbBq;1Hb(&I{A~AZ(qLp G{C@$1cKMJ1 literal 0 HcmV?d00001 diff --git a/robowaiter/algos/explore/semantic_map.py b/robowaiter/algos/explore/semantic_map.py new file mode 100644 index 0000000..b8a6710 --- /dev/null +++ b/robowaiter/algos/explore/semantic_map.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 +# -*- encoding: utf-8 -*- +# enconding = utf8 +import sys +import time +import grpc + +import camera + +sys.path.append('./') +sys.path.append('../') + +import matplotlib.pyplot as plt +import numpy as np +from mpl_toolkits.axes_grid1 import make_axes_locatable + +import GrabSim_pb2_grpc +import GrabSim_pb2 + +channel = grpc.insecure_channel('localhost:30001', options=[ + ('grpc.max_send_message_length', 1024 * 1024 * 1024), + ('grpc.max_receive_message_length', 1024 * 1024 * 1024) +]) + +sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) + +''' +初始化，卸载已经加载的关卡，清除所有机器人 +''' + + +def Init(): + sim_client.Init(GrabSim_pb2.NUL()) + + +''' +获取当前可加载的地图信息(地图名字、地图尺寸) +''' + + +def AcquireAvailableMaps(): + AvailableMaps = sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()) + print(AvailableMaps) + + +''' +1、根据mapID加载指定地图 +2、如果scene_num>1,则根据地图尺寸偏移后加载多个相同地图 +3、这样就可以在一个关卡中训练多个地图 +''' + + +def SetWorld(map_id=0, scene_num=1): + print('------------------SetWorld----------------------') + world = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) + + +''' +返回场景的状态信息 +1、返回机器人的位置和旋转 +2、返回各个关节的名字和旋转 +3、返回场景中标记的物品信息(名字、类型、位置、旋转) +4、返回场景中行人的信息(名字、位置、旋转、速度) +5、返回机器人手指和双臂的碰撞信息 +''' + + +def Observe(scene_id=0): + print('------------------show_env_info----------------------') + scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) + print( + f"location:{[scene.location]}, rotation:{scene.rotation}\n", + f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", + f"objects number: {len(scene.objects)}, walkers number: {len(scene.walkers)}\n" + f"timestep:{scene.timestep}, timestamp:{scene.timestamp}\n" + f"collision:{scene.collision}, info:{scene.info}") + + +''' +重置场景 +1、重置桌子的宽度和高度 +2、清除生成的行人和物品 +3、重置关节角度、位置旋转 +4、清除碰撞信息 +5、重置场景中标记的物品 +''' + + +def Reset(scene_id=0): + print('------------------Reset----------------------') + scene = sim_client.Reset(GrabSim_pb2.ResetParams(scene=scene_id)) + print(scene) + + # 如果场景支持调整桌子 + # sim_client.Reset(GrabSim_pb2.ResetParams(scene = scene_id, adjust = True, height = 100.0, width = 100.0))" + + +""" +导航移动 +yaw:机器人朝向; +velocity:速度,>0代表移动,<0代表瞬移,=0代表只查询; +dis:最终达到的位置距离目标点最远距离,如果超过此距离则目标位置不可达 +""" + + +def navigation_move(cur_objs, scene_id=0, map_id=0): + print('------------------navigation_move----------------------') + scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) + walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw] + print("position:", walk_value) + objs_name_set = set() + + if map_id == 11: # coffee + v_list = [[247,520], [247, 700], [270, 1100], [55, 940], [30, 900], [30, 520], [160, -165], [247, 0],[247, 520]] + else: + v_list = [[0.0, 0.0]] + + for walk_v in v_list: + walk_v = walk_v + [scene.rotation.Yaw - 90, 200, 10] + print("walk_v", walk_v) + action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) + scene = sim_client.Do(action) + cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, objs_name_set) + print(scene.info) + return cur_objs + + +if __name__ == '__main__': + map_id = 11 # 地图编号 + scene_num = 1 # 场景数量 + cur_objs = [] + + print('------------ 初始化加载场景 ------------') + Init() + AcquireAvailableMaps() + SetWorld(map_id, scene_num) + time.sleep(5.0) + + for i in range(scene_num): + print('------------ 场景操作 ------------') + Observe(i) + Reset(i) + + print('------------ 导航移动 ------------') + cur_objs = navigation_move(cur_objs, i, map_id) + print(cur_objs)