AEM
This commit is contained in:
Caiyishuai
commit
37871c6d77
|
|
@ -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='''
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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":
|
||||
|
|
|
|||
|
|
@ -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__':
|
||||
|
|
|
|||
Loading…
Reference in New Issue