def plan_path(self, overwrite=False):
t = time.time()
if ((not self.prev_plan_is_not_valid()) and (not overwrite)
and (len(self.planned_waypoints) > 0)):
return self.planned2Dpath, self.planned_waypoints
self.waypointPose6D = None
current_pos = self.get_position_on_map()
start_map = torch.zeros_like(self.map2DObstacles).to(self.device)
start_map[0, 0, current_pos[0, 0].long(), current_pos[0,
1].long()] = 1.0
goal_map = torch.zeros_like(self.map2DObstacles).to(self.device)
goal_map[0, 0, self.estimatedGoalPos2D[0, 0].long(),
self.estimatedGoalPos2D[0, 1].long(), ] = 1.0
path, cost = self.planner(
self.rawmap2_planner_ready(self.map2DObstacles, start_map,
goal_map).to(self.device),
self.coordinatesGrid.to(self.device),
goal_map.to(self.device),
start_map.to(self.device),
)
if len(path) == 0:
return path, []
if self.timing:
print(time.time() - t, " s, Planning")
t = time.time()
planned_waypoints = planned_path2tps(path, self.map_cell_size,
self.map_size_meters, 1.0,
False).to(self.device)
return path, planned_waypoints
def set_offset_to_goal(self, observation):
self.offset_to_goal = (torch.from_numpy(
observation["pointgoal"]).float().to(self.device))
self.estimatedGoalPos2D = habitat_goalpos_to_mapgoal_pos(
self.offset_to_goal,
self.pose6D.squeeze(),
self.map_cell_size,
self.map_size_meters,
)
self.estimatedGoalPos6D = planned_path2tps(
[self.estimatedGoalPos2D],
self.map_cell_size,
self.map_size_meters,
1.0,
).to(self.device)[0]
return
def set_offset_to_goal(self, observation):
""" ID mappings"""
obj_to_id = {
'chair': 0,
'table': 1,
'picture': 2,
'cabinet': 3,
'cushion': 4,
'sofa': 5,
'bed': 6,
'chest_of_drawers': 7,
'plant': 8,
'sink': 9,
'toilet': 10,
'stool': 11,
'towel': 12,
'tv_monitor': 13,
'shower': 14,
'bathtub': 15,
'counter': 16,
'fireplace': 17,
'gym_equipment': 18,
'seating': 19,
'clothes': 20
}
id_to_obj = {obj_to_id[key]: key for key in obj_to_id}
# [distance to goal in metres, angle to goal in radians]
if GOAL_SENSOR_UUID == 'objectgoal':
self.offset_to_goal = (
torch.from_numpy(np.array([1.0, 3
])) #observation[GOAL_SENSOR_UUID])
.float().to(self.device))
print('class observation from goal',
id_to_obj[observation[GOAL_SENSOR_UUID][0]])
else:
self.offset_to_goal = (torch.from_numpy(
observation[GOAL_SENSOR_UUID]).float().to(self.device))
print('substituted observation from goal', self.offset_to_goal)
self.estimatedGoalPos2D = habitat_goalpos_to_mapgoal_pos(
self.offset_to_goal,
self.pose6D.squeeze(),
self.map_cell_size,
self.map_size_meters,
)
self.estimatedGoalPos6D = planned_path2tps(
[self.estimatedGoalPos2D],
self.map_cell_size,
self.map_size_meters,
1.0,
).to(self.device)[0]
# {'STOP': 0, 'MOVE_FORWARD': 1, 'TURN_LEFT': 2, 'TURN_RIGHT': 3, 'LOOK_UP': 4, 'LOOK_DOWN': 5}
# https://github.com/facebookresearch/habitat-api/blob/master/habitat/sims/habitat_simulator/actions.py
log_attrs = [
'pose6D', 'estimatedGoalPos6D', 'action_history', 'cur_time',
'position_history', 'steps'
]
for attr in log_attrs:
print(attr, getattr(self, attr))
print('num obstacles:', self.map2DObstacles.sum())
if self.map2DObstacles.sum() > 0:
np.savetxt(
'obstacle_map_' + str(self.map2DObstacles.sum()) + '.csv',
self.map2DObstacles[0, 0].cpu().numpy())
return
def set_offset_to_goal(self, observation):
""" ID mappings"""
obj_to_id = {
'chair': 0,
'table': 1,
'picture': 2,
'cabinet': 3,
'cushion': 4,
'sofa': 5,
'bed': 6,
'chest_of_drawers': 7,
'plant': 8,
'sink': 9,
'toilet': 10,
'stool': 11,
'towel': 12,
'tv_monitor': 13,
'shower': 14,
'bathtub': 15,
'counter': 16,
'fireplace': 17,
'gym_equipment': 18,
'seating': 19,
'clothes': 20
}
id_to_obj = {obj_to_id[key]: key for key in obj_to_id}
# if GOAL_SENSOR_UUID == 'objectgoal':
# # [distance to goal in metres, angle to goal in radians]
# # Take the existing goal and find your offset from it
# self.obs["pointgoal_with_gps_compass"] = self.find_object_goal(observation)
# if self.obs["pointgoal_with_gps_compass"] is None:
# self.obs["explorationgoal"] = self.plan_exploration(observation)
# # doesn't see viable rays. this is not good
# if self.obs["explorationgoal"] is None:
# print("\n\nWarning: no viable rays found\n\n")
# self.obs["explorationgoal"] = HabitatSimActions.TURN_RIGHT
# print('class observation from goal', id_to_obj[observation[GOAL_SENSOR_UUID][0]])
if self.obs["pointgoal_with_gps_compass"] is not None:
self.located_true_goal = True
self.offset_to_goal = (torch.from_numpy(
self.obs["pointgoal_with_gps_compass"]).float().to(
self.device))
else:
self.offset_to_goal = (torch.from_numpy(
self.obs["explorationgoal"]).float().to(self.device))
#print('ostensible observation from goal', self.offset_to_goal)
self.estimatedGoalPos2D = habitat_goalpos_to_mapgoal_pos(
self.offset_to_goal,
self.pose6D.squeeze(),
self.map_cell_size,
self.map_size_meters,
)
self.estimatedGoalPos6D = planned_path2tps(
[self.estimatedGoalPos2D],
self.map_cell_size,
self.map_size_meters,
1.0,
).to(self.device)[0]
# {'STOP': 0, 'MOVE_FORWARD': 1, 'TURN_LEFT': 2, 'TURN_RIGHT': 3, 'LOOK_UP': 4, 'LOOK_DOWN': 5}
# https://github.com/facebookresearch/habitat-api/blob/master/habitat/sims/habitat_simulator/actions.py
log_attrs = [
'pose6D', 'estimatedGoalPos2D', 'estimatedGoalPos6D',
'action_history', 'cur_time', 'steps'
]
# for attr in log_attrs:
# print(attr, getattr(self, attr))
# if len(self.position_history) > 0:
# print("most recent position:", self.position_history[-1])
# print('num obstacles:', self.map2DObstacles.sum())
# print('gps and compass:'******'gps'], observation['compass'])
# Log the 2D obstacles map and observations
if self.map2DObstacles.sum() > 0:
map_dir = 'maps/' + GOAL_SENSOR_UUID + '_' + str(self.init_time)
if not os.path.exists(map_dir):
os.mkdir(map_dir)
suffix = str(int(time.time())) + '_' + str(
int(self.map2DObstacles.sum()))
np.savez_compressed(map_dir + '/obstacle_map_' + suffix + '.npz',
self.map2DObstacles[0, 0].cpu().numpy())
if self.obstacle_ray_map is not None:
np.savez_compressed(
map_dir + '/obstacle_ray_map_' + suffix + '.npz',
self.obstacle_ray_map)
PIL.Image.fromarray(observation['rgb']).save(map_dir + '/rgb_' +
suffix + '.png')
PIL.Image.fromarray(
(observation['depth'] * 255).astype(np.uint8).squeeze(2),
'L').save(map_dir + '/depth_' + suffix + '.png')
return
