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