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
