def run_astar(episode):
MAX_RUNS = 2000000
TURN_ANGLE = 30
FORWARD_STEP_SIZE = 0.25
AGENT_HEIGHT = 0.88
AGENT_RADIUS = 0.18
house_floor_threshold = {
'2azQ1b91cZZ': [1.5],
'8194nk5LbLH': [1.2],
'EU6Fwq7SyZv': [-1.3, 2.0],
'oLBMNvg9in8': [-0.8, 1.8, 4.6],
'pLe4wQe7qrG': [],
'QUCTc6BB5sX': [-1.5],
'TbHJrupSAjP': [-1.5, 1.7],
'X7HyMhZNoso': [1.8],
'x8F5xyUWy9e': [],
'Z6MFQCViBuw': [],
'zsNo4HB9uLZ': [],
}
# -- folders
data_dir = '../data/ObjectNav/objectnav_mp3d_v1/val/'
folder_pred = '../data/ObjectNav/semmap/'
floormap_dir = '../data/ObjectNav/freespace_map/'
info = json.load(open('../data/ObjectNav/semmap_objnav_info.json', 'r'))
# -- setup naming bindings
read_tsv = csv.reader(open("mpcat40.tsv"), delimiter="\t")
mpcat40 = {line[0]: line[1] for line in read_tsv}
jsonfile = json.load(open(os.path.join(data_dir, 'val.json'), 'r'))
category_to_mp3d_category_id = jsonfile['category_to_mp3d_category_id']
house = episode['scene_id'].split('/')[1]
# -- sample object category target
obj_semantic_id = category_to_mp3d_category_id[episode['object_category']]
obj_semantic_name = mpcat40[str(obj_semantic_id)]
if obj_semantic_name in object_whitelist:
sid = object_whitelist.index(obj_semantic_name) + 1
else:
return (episode['episode_id'], [], [])
# -- select floor -> env
if house_floor_threshold[house]:
level = bisect.bisect(house_floor_threshold[house],
episode['start_position'][1])
else:
level = 0
env = house + '_' + str(level)
map_world_shift = np.array(info[env]['map_world_shift'])
# -- load maps
floormap = imread(os.path.join(floormap_dir, env + '.png'))
floormap = floormap.astype(np.float)
floormap /= 255
floormap = floormap.astype(np.bool)
if not os.path.isfile(os.path.join(folder_pred, env + '.h5')):
return (episode['episode_id'], [], [])
h5file = h5py.File(os.path.join(folder_pred, env + '.h5'), 'r')
map_semantic = np.array(h5file['semmap'])
observed_map = np.array(h5file['observed_map'])
observed_map = observed_map.astype(np.float)
h5file.close()
map_semantic = np.multiply(map_semantic, observed_map)
map_semantic = map_semantic.astype(np.int)
#goals = None
goals = all_goals[house][episode['episode_id']]
goals = np.array(goals)
goals -= map_world_shift
goals = goals[:, [0, 2]]
# -- get init position
start_pos = episode['start_position']
start_pos -= map_world_shift
start_x = start_pos[0]
start_y = start_pos[2]
start_row = int(np.round(start_y / resolution))
start_col = int(np.round(start_x / resolution))
start_rot = np.array(episode['start_rotation'])
r = R.from_quat(start_rot)
_, start_heading, _ = r.as_rotvec()
start_heading = (start_heading * 180 / np.pi)
start_heading = -(start_heading + 90)
start = Node(x=start_x,
y=start_y,
heading=start_heading,
row=start_row,
col=start_col)
# -- get maps for Astar
goal_mask = map_semantic == sid
goal_mask = binary_opening(goal_mask.astype(int),
structure=np.ones((3, 3))).astype(np.bool)
floormap = binary_closing(floormap.astype(int),
structure=np.ones((10, 10))).astype(np.bool)
navmap = floormap & (map_semantic == 0)
# compute Heuristic
# -- Euclidean distance
non_object_mask = ~goal_mask
non_object_mask = non_object_mask.astype(np.uint8)
distance_map = cv2.distanceTransform(non_object_mask.copy(), cv2.DIST_L2,
3)
# -- Geodesic distance
#os.system('pip install scikit-fmm')
#import skfmm
#import numpy.ma as ma
#mask = ~navmap&~goal_mask
#mask = ~mask
#mask = binary_dilation(mask.astype(int), structure=np.ones((10,10))).astype(np.bool)
#mask = ~mask
#map = np.ones(navmap.shape)
#map = map - 2*goal_mask.astype(np.float)
#map = ma.masked_array(map, mask)
#distance_map = skfmm.distance(map)
pathfinder = Astar(navmap,
observed_map,
heuristic=distance_map,
init_heading=start_heading,
goals=goals)
path, runs = pathfinder.run(start, goal_mask, max_runs=MAX_RUNS)
if len(path) == 0:
actions = []
else:
path = path[::-1]
# -- convert path to actions
actions = []
prev_p = path[0]
for p in path[1:]:
# -- rotate
pre_h = prev_p[2]
new_h = p[2]
delta_h = (new_h - pre_h + 360) % 360
if delta_h == 0:
pass
elif delta_h <= 180:
#trun right
num_rotations = int(delta_h / TURN_ANGLE)
actions += [3] * abs(num_rotations)
elif delta_h > 180:
#trun left
delta_h = 360 - delta_h
num_rotations = int(delta_h / TURN_ANGLE)
actions += [2] * abs(num_rotations)
# move forward
actions.append(1)
prev_p = p
actions.append(0)
return (episode['episode_id'], actions, path)
node7 = Node('Node 7', x_position=90, y_position=90)
node8 = Node('Node 8', x_position=120, y_position=120)
# create connections
node1.add_connection(node2, 7)
node1.add_connection(node3, 2)
node2.add_connection(node4, 5)
node3.add_connection(node5, 3)
node4.add_connection(node6, 1)
node5.add_connection(node7, 3)
node5.add_connection(node8, 4)
node7.add_connection(node6, 3)
node8.add_connection(node7, 1)
# do search
ucs = Ucs()
print_result_info(ucs.run(start=node1, goal=node6))
Node.reset_nodes()
astar = Astar(heuristic_factor=0.05, heuristic=Astar.MANHATTAN_DISTANCE)
print_result_info(astar.run(start=node1, goal=node6))
