EmbodiedAI
/
RoboWaiter
mirror of https://github.com/HPCL-EI/RoboWaiter.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

优化AEM

This commit is contained in:
liwang_zhang 2023-11-20 14:06:03 +08:00
parent 66ce119e12
commit dd943e39af
4 changed files with 54 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
robowaiter/proto/camera.py
View File

@ -29,7 +29,7 @@ objects_dic = {}
obstacle_objs_id = [114, 115, 122, 96, 102, 83, 121, 105, 108, 89, 100, 90, 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, 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, 106, 120, 97, 86, 104, 78, 85, 81, 82, 84, 91, 93, 94,
99, 107, 116, 117, 118, 119, 255] 99, 107, 116, 117, 118, 119, 255, 251]
not_key_objs_id = {255,254,253,107,81} not_key_objs_id = {255,254,253,107,81}
''' '''
@ -367,12 +367,11 @@ def get_id_object_pixels(id, scene):
world_points.append(transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene)) world_points.append(transform_co(img_data_depth, pixel[0], pixel[1], d_depth[pixel[0]][pixel[1]][0], scene))
return world_points return world_points
def get_obstacle_point(plt, 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 = [] cur_obstacle_pixel_points = []
object_pixels = {} object_pixels = {}
obj_detect_count = 0
cur_objs_id = []
colors = [ colors = [
'red', 'red',
'pink', 'pink',
@ -403,6 +402,7 @@ def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
for item in items: for item in items:
key, value = item.split(":") key, value = item.split(":")
objs_id[int(key)] = value objs_id[int(key)] = value
objs_id[251] = "walker"
# plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None) # plt.imshow(d_depth, cmap="gray" if "depth" in im_depth.name.lower() else None)
# plt.show() # plt.show()
plt.subplot(2, 2, 1) plt.subplot(2, 2, 1)
@ -448,12 +448,14 @@ def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
# plt.tight_layout() # plt.tight_layout()
for key, value in object_pixels.items(): for key, value in object_pixels.items():
if key == 0: if key == 0 or key not in objs_id.keys():
continue continue
if key in [91, 84, 96, 87, 102, 106, 120, 85,113, 101, 83]:
if key in [91, 84, 96, 87, 102, 106, 120, 85,113, 101, 83, 251]:
X = np.array(value) X = np.array(value)
db.fit(X) db.fit(X)
labels = db.labels_ labels = db.labels_
# 将数据按照聚类标签分组，并打印每个分组的数据 # 将数据按照聚类标签分组，并打印每个分组的数据
for i in range(max(labels) + 1): # 从0到最大聚类标签的值 for i in range(max(labels) + 1): # 从0到最大聚类标签的值
group_data = X[labels == i] # 获取当前标签的数据 group_data = X[labels == i] # 获取当前标签的数据
@ -463,6 +465,9 @@ def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
y_min = min(p[1] for p in group_data) y_min = min(p[1] for p in group_data)
if x_max - x_min < 10 or y_max - y_min < 10: if x_max - x_min < 10 or y_max - y_min < 10:
continue continue
if key != 251:
obj_detect_count += 1
cur_objs_id.append(objs_id[key])
# 在指定的位置绘制方框 # 在指定的位置绘制方框
# 创建矩形框 # 创建矩形框
rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1, edgecolor=colors[key % 10], rect = patches.Rectangle((x_min, y_min), (x_max - x_min), (y_max - y_min), linewidth=1, edgecolor=colors[key % 10],
@ -470,7 +475,11 @@ def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
plt.text(x_min, y_min, f'{objs_id[key]}', fontdict={'family': 'serif', 'size': 10, 'color': 'green'}, ha='center', plt.text(x_min, y_min, f'{objs_id[key]}', fontdict={'family': 'serif', 'size': 10, 'color': 'green'}, ha='center',
va='center') va='center')
plt.gca().add_patch(rect) plt.gca().add_patch(rect)
else: else:
if key != 251:
obj_detect_count += 1
cur_objs_id.append(objs_id[key])
x_max = max(p[0] for p in value) x_max = max(p[0] for p in value)
y_max = max(p[1] for p in value) y_max = max(p[1] for p in value)
x_min = min(p[0] for p in value) x_min = min(p[0] for p in value)
@ -489,12 +498,12 @@ def get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio):
# rect = patches.Rectangle((0, 255), 15, 30, linewidth=1, edgecolor='g', # rect = patches.Rectangle((0, 255), 15, 30, linewidth=1, edgecolor='g',
# facecolor='none') # facecolor='none')
plt.subplot(2, 7, 14) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
# 将矩形框添加到图像中 plt.text(0, 0.7, f'检测物体数量：{obj_detect_count}', fontsize=10)
# plt.gca().add_patch(rect)
# plt.show() # plt.show()
return cur_obstacle_world_points return cur_obstacle_world_points, cur_objs_id

2
robowaiter/proto/objs.json
View File

File diff suppressed because one or more lines are too long

16
robowaiter/scene/scene.py
View File

@ -774,7 +774,7 @@ class Scene:
scene = stub.Do(action) scene = stub.Do(action)
print(scene.info) print(scene.info)
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): def navigation_move(self, plt, cur_objs, cur_obstacle_world_points, v_list, map_ratio, db, scene_id=0, map_id=11):
print('------------------navigation_move----------------------') print('------------------navigation_move----------------------')
scene = stub.Observe(GrabSim_pb2.SceneID(value=scene_id)) scene = stub.Observe(GrabSim_pb2.SceneID(value=scene_id))
walk_value = [scene.location.X, scene.location.Y] walk_value = [scene.location.X, scene.location.Y]
@ -787,10 +787,10 @@ class Scene:
print("walk_v", walk_v) print("walk_v", walk_v)
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
scene = stub.Do(action) scene = stub.Do(action)
cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, # cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
objs_name_set) # objs_name_set)
cur_obstacle_world_points = camera.get_obstacle_point(plt, db, scene, cur_obstacle_world_points,map_ratio) cur_obstacle_world_points, cur_objs_id= camera.get_obstacle_point(plt, db, scene, cur_obstacle_world_points,map_ratio)
# if scene.info == "Unreachable": # if scene.info == "Unreachable":
@ -809,15 +809,15 @@ class Scene:
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
scene = stub.Do(action) scene = stub.Do(action)
cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, # cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs,
objs_name_set) # objs_name_set)
cur_obstacle_world_points = camera.get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio) cur_obstacle_world_points, cur_objs_id= camera.get_obstacle_point(plt, db, scene, cur_obstacle_world_points, map_ratio)
# if scene.info == "Unreachable": # if scene.info == "Unreachable":
print(scene.info) print(scene.info)
return cur_objs, objs_name_set, cur_obstacle_world_points return cur_obstacle_world_points, cur_objs_id
def isOutMap(self, pos, min_x=-200, max_x=600, min_y=-250, max_y=1300): 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: if pos[0] <= min_x or pos[0] >= max_x or pos[1] <= min_y or pos[1] >= max_y:

38
robowaiter/scene/tasks/AEM.py
View File

@ -23,22 +23,20 @@ class SceneAEM(Scene):
def _reset(self): def _reset(self):
pass pass
def _run(self): def _run(self):
print(len(self.status.objects))
# 创建一个从白色（1）到灰色（0）的 colormap # 创建一个从白色（1）到灰色（0）的 colormap
objs = self.status.objects
cur_objs = [] cur_objs = []
cur_obstacle_world_points = [] cur_obstacle_world_points = []
objs_name_set = set()
visited_obstacle = set() visited_obstacle = set()
obj_json_data = [] obj_json_data = []
obj_count = 0
added_info = 0
map_ratio = 3 map_ratio = 3
db = DBSCAN(eps=map_ratio, min_samples=int(map_ratio / 2)) db = DBSCAN(eps=map_ratio, min_samples=int(map_ratio / 2))
# # 创建一个颜色映射，其中0表示黑色，1表示白色
# cmap = plt.cm.get_cmap('gray')
# cmap.set_under('black')
# cmap.set_over('white')
file_name = '../../proto/map_1.pkl' file_name = '../../proto/map_1.pkl'
if os.path.exists(file_name): if os.path.exists(file_name):
@ -54,13 +52,15 @@ class SceneAEM(Scene):
# navigation_test(i,map_id) # navigation_test(i,map_id)
map_map = np.zeros((math.ceil(950 / map_ratio), math.ceil(1850 / map_ratio))) map_map = np.zeros((math.ceil(950 / map_ratio), math.ceil(1850 / map_ratio)))
# self.add_walker(0, 30, 520, )
# self.add_walker(10, 30, 420)
while True: while True:
walker_count = 0
fig = plt.figure() fig = plt.figure()
goal = self.explore(map, 120) # cur_pos 指的是当前机器人的位置，场景中应该也有接口可以获取 goal = self.explore(map, 120)
if goal is None: if goal is None:
break break
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) cur_obstacle_world_points, cur_objs_id= self.navigation_move(plt, cur_objs, cur_obstacle_world_points, [[goal[0], goal[1]]], map_ratio, db,0, 11)
for point in cur_obstacle_world_points: for point in cur_obstacle_world_points:
if point[0] < -350 or point[0] > 600 or point[1] < -400 or point[1] > 1450: if point[0] < -350 or point[0] > 600 or point[1] < -400 or point[1] > 1450:
@ -71,7 +71,13 @@ class SceneAEM(Scene):
# 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))
# 使用imshow函数绘制图像，其中cmap参数设置颜色映射 for i in range(len(cur_objs_id)):
if cur_objs_id[i] == "walker":
walker_count += 1
for obj in objs:
if obj.name == cur_objs_id[i] and obj not in cur_objs:
cur_objs.append(obj)
break
plt.subplot(2, 1, 2) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图 plt.subplot(2, 1, 2) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower', plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower',
@ -80,6 +86,16 @@ class SceneAEM(Scene):
# plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower') # plt.imshow(map_map, cmap='binary', alpha=0.5, origin='lower')
# plt.axis('off') # plt.axis('off')
plt.title("地图构建过程") plt.title("地图构建过程")
plt.subplot(2, 7, 14) # 这里的2,1表示总共2行，1列，2表示这个位置是第2个子图
# plt.text(0, 0.7, f'检测行人数量：{walker_count}', fontsize=10)
new_add_info = len(cur_objs) - added_info + walker_count
plt.text(0, 0.5, f'新增语义信息：{new_add_info}', fontsize=10) # 在图中添加文字，x和y坐标是在这个图片大小内的相对位置，fontsize是字体大小
added_info += new_add_info
plt.text(0, 0.3, f'已存语义信息：{added_info}', fontsize=10) # 在图中添加文字，x和y坐标是在这个图片大小内的相对位置，fontsize是字体大小
plt.axis("off")
plt.show() plt.show()
print("------------当前检测到的物品信息--------------") print("------------当前检测到的物品信息--------------")
print(cur_objs) print(cur_objs)