def reset_metric(self, episode): self._step_count = 0 self._metric = None self._top_down_map = self.get_original_map() agent_position = self._sim.get_agent_state().position a_x, a_y = maps.to_grid( agent_position[0], agent_position[2], self._coordinate_min, self._coordinate_max, self._map_resolution, ) self._previous_xy_location = (a_y, a_x) if self._config.DRAW_SHORTEST_PATH: # draw shortest path self._shortest_path_points = self._sim.get_straight_shortest_path_points( agent_position, episode.goals[0].position) self._shortest_path_points = [ maps.to_grid( p[0], p[2], self._coordinate_min, self._coordinate_max, self._map_resolution, )[::-1] for p in self._shortest_path_points ] maps.draw_path( self._top_down_map, self._shortest_path_points, maps.MAP_SHORTEST_PATH_COLOR, self.line_thickness, ) # draw source and target points last to avoid overlap if self._config.DRAW_SOURCE_AND_TARGET: self.draw_source_and_target(episode)
def _draw_goals_aabb(self, episode): if self._config.DRAW_GOAL_AABBS: for goal in episode.goals: try: sem_scene = self._sim.semantic_annotations() object_id = goal.object_id assert int( sem_scene.objects[object_id].id.split("_")[-1] ) == int( goal.object_id ), f"Object_id doesn't correspond to id in semantic scene objects dictionary for episode: {episode}" center = sem_scene.objects[object_id].aabb.center x_len, _, z_len = ( sem_scene.objects[object_id].aabb.sizes / 2.0 ) # Nodes to draw rectangle corners = [ center + np.array([x, 0, z]) for x, z in [ (-x_len, -z_len), (-x_len, z_len), (x_len, z_len), (x_len, -z_len), (-x_len, -z_len), ] ] map_corners = [ maps.to_grid( p[0], p[2], self._coordinate_min, self._coordinate_max, self._map_resolution, ) for p in corners ] maps.draw_path( self._top_down_map, map_corners, maps.MAP_TARGET_BOUNDING_BOX, self.line_thickness, ) except AttributeError: pass
def _draw_shortest_path(self, episode: Episode, agent_position: AgentState): if self._config.DRAW_SHORTEST_PATH: self._shortest_path_points = self._sim.get_straight_shortest_path_points( agent_position, episode.goals[0].position) self._shortest_path_points = [ maps.to_grid(p[2], p[0], self._top_down_map.shape[0:2], sim=self._sim) for p in self._shortest_path_points ] maps.draw_path( self._top_down_map, self._shortest_path_points, maps.MAP_SHORTEST_PATH_COLOR, self.line_thickness, )
def reset_metric(self, *args: Any, episode, **kwargs: Any): self._step_count = 0 self._metric = None self._top_down_map = self.get_original_map() self._valid_map = self._top_down_map != MAP_INVALID_POINT self._explored_map = np.zeros_like(self._valid_map) agent_position = self._sim.get_agent_state().position a_x, a_y = maps.to_grid( agent_position[0], agent_position[2], self._coordinate_min, self._coordinate_max, self._map_resolution, ) goal_idx = getattr(episode, "goal_idx", 0) self._previous_xy_location = (a_y, a_x) if self._config.DRAW_SHORTEST_PATH: # draw shortest path self._shortest_path_points = self._sim.get_straight_shortest_path_points( agent_position, episode.goals[goal_idx].position) self._shortest_path_points = [ maps.to_grid( p[0], p[2], self._coordinate_min, self._coordinate_max, self._map_resolution, )[::-1] for p in self._shortest_path_points ] maps.draw_path( self._top_down_map, self._shortest_path_points, maps.MAP_SHORTEST_PATH_COLOR, self.line_thickness, ) self.update_fog_of_war_mask(np.array([a_x, a_y])) # draw source and target points last to avoid overlap if self._config.DRAW_SOURCE_AND_TARGET: self.draw_source_and_target(episode)
trajectory = [ maps.to_grid( path_point[2], path_point[0], grid_dimensions, pathfinder=sim.pathfinder, ) for path_point in path_points ] grid_tangent = mn.Vector2( trajectory[1][1] - trajectory[0][1], trajectory[1][0] - trajectory[0][0] ) path_initial_tangent = grid_tangent / grid_tangent.length() initial_angle = math.atan2(path_initial_tangent[0], path_initial_tangent[1]) # draw the agent and trajectory on the map maps.draw_path(top_down_map, trajectory) maps.draw_agent( top_down_map, trajectory[0], initial_angle, agent_radius_px=8 ) print("\nDisplay the map with agent and path overlay:") display_map(top_down_map) # @markdown 4. (optional) Place agent and render images at trajectory points (if found). display_path_agent_renders = True # @param{type:"boolean"} if display_path_agent_renders: print("Rendering observations at path points:") tangent = path_points[1] - path_points[0] agent_state = habitat_sim.AgentState() for ix, point in enumerate(path_points): if ix < len(path_points) - 1: tangent = path_points[ix + 1] - point