def update_map(self, agent_position):
a_x, a_y = maps.to_grid(
agent_position[0],
agent_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
# Don't draw over the source point
if self._top_down_map[a_x, a_y] != maps.MAP_SOURCE_POINT_INDICATOR:
color = 10 + min(
self._step_count * 245 // self._config.MAX_EPISODE_STEPS, 245)
thickness = int(
np.round(self._map_resolution[0] * 2 / MAP_THICKNESS_SCALAR))
cv2.line(
self._top_down_map,
self._previous_xy_location,
(a_y, a_x),
color,
thickness=thickness,
)
self.update_fog_of_war_mask(np.array([a_x, a_y]))
self._previous_xy_location = (a_y, a_x)
return self._top_down_map, a_x, a_y
def reset_metric(self, episode, *args: Any, **kwargs: Any):
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)
self.update_fog_of_war_mask(np.array([a_x, a_y]))
# draw source and target parts last to avoid overlap
self._draw_goals_view_points(episode)
self._draw_goals_aabb(episode)
self._draw_goals_positions(episode)
self._draw_shortest_path(episode, agent_position)
if self._config.DRAW_SOURCE:
self._draw_point(episode.start_position,
maps.MAP_SOURCE_POINT_INDICATOR)
def _draw_point(self, position, point_type):
t_x, t_y = maps.to_grid(
position[0],
position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
self._top_down_map[t_x - self.point_padding:t_x + self.point_padding +
1, t_y - self.point_padding:t_y +
self.point_padding + 1, ] = point_type
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[0],
p[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
) 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 _get_mesh_occupancy(self):
agent_position = self.current_episode.start_position
agent_rotation = quaternion_xyzw_to_wxyz(
self.current_episode.start_rotation)
a_x, a_y = maps.to_grid(
agent_position[0],
agent_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
# The map size here represents size around the agent, not infront.
mrange = int(self.map_size * 1.5 / 2.0)
# Add extra padding if map range is coordinates go out of bounds
y_start = a_y - mrange
y_end = a_y + mrange
x_start = a_x - mrange
x_end = a_x + mrange
x_l_pad, y_l_pad, x_r_pad, y_r_pad = 0, 0, 0, 0
H, W = self._top_down_map.shape
if x_start < 0:
x_l_pad = int(-x_start)
x_start += x_l_pad
x_end += x_l_pad
if x_end >= W:
x_r_pad = int(x_end - W + 1)
if y_start < 0:
y_l_pad = int(-y_start)
y_start += y_l_pad
y_end += y_l_pad
if y_end >= H:
y_r_pad = int(y_end - H + 1)
ego_map = np.pad(self._top_down_map,
((y_l_pad, y_r_pad), (x_l_pad, x_r_pad)))
ego_map = ego_map[y_start:(y_end + 1), x_start:(x_end + 1)]
if ego_map.shape[0] == 0 or ego_map.shape[1] == 0:
ego_map = np.zeros((2 * mrange + 1, 2 * mrange + 1),
dtype=np.uint8)
# Rotate to get egocentric map
# Negative since the value returned is clockwise rotation about Y,
# but we need anti-clockwise rotation
agent_heading = -compute_heading_from_quaternion(agent_rotation)
agent_heading = math.degrees(agent_heading)
half_size = ego_map.shape[0] // 2
center = (half_size, half_size)
M = cv2.getRotationMatrix2D(center, agent_heading, scale=1.0)
ego_map = cv2.warpAffine(
ego_map,
M,
(ego_map.shape[1], ego_map.shape[0]),
flags=cv2.INTER_NEAREST,
borderMode=cv2.BORDER_CONSTANT,
borderValue=(1, ),
)
ego_map = ego_map.astype(np.float32)
mrange = int(self.map_size / 2.0)
start_coor = half_size - mrange
end_coor = int(start_coor + self.map_size - 1)
ego_map = ego_map[start_coor:(end_coor + 1), start_coor:(end_coor + 1)]
# This map is currently 0 if occupied and 1 if unoccupied. Flip it.
ego_map = 1.0 - ego_map
# Flip the x axis because to_grid() flips the conventions
ego_map = np.flip(ego_map, axis=1)
# Append explored status in the 2nd channel
ego_map = np.stack([ego_map, np.ones_like(ego_map)], axis=2)
return ego_map