def _set_destination(
self,
destination: carla.Location, # pylint: disable=no-member
) -> None:
"""Generates the global plan for the agent.
Args:
destination: The location of the new destination.
"""
# Set vehicle's current location as start for the plan.
origin = self._vehicle.get_location()
start_waypoint = self._map.get_waypoint(origin).transform.location
end_waypoint = self._map.get_waypoint(destination).transform.location
# Calculate the plan.
waypoints, roadoptions, _ = cutil.global_plan(
world=self._world,
origin=start_waypoint,
destination=end_waypoint,
)
# Mutate the local planner's global plan.
self._local_planner.set_global_plan(list(zip(waypoints, roadoptions)))
def plot_benchmark(
self,
output_dir: str,
) -> None:
"""Visualizes all the tasks in a benchmark (A -> B).
Args:
output_dir: The full path to the output directory.
"""
import signal
import matplotlib.pyplot as plt
import numpy as np
import tqdm
from oatomobile.utils import carla as cutil
def world_to_pixel(
location: np.ndarray,
town: str,
) -> np.ndarray:
"""Converts CARLA world coordinates to pixel coordinates."""
assert town in [
"Town01",
"Town02",
"Town03",
"Town04",
"Town05",
]
offset = {
"Town01": (-52.059906005859375, -52.04995942115784),
"Town02": (-57.459808349609375, 55.3907470703125),
"Town03": (-207.43186950683594, -259.27125549316406),
"Town04": (-565.26904296875, -446.1461181640625),
"Town05": (-326.0448913574219, -257.8750915527344)
}[town]
scale = {
"Town01": 12.0,
"Town02": 12.0,
"Town03": 12.0,
"Town04": 9.6,
"Town05": 12.0,
}[town]
return (location - offset) * scale
# Creates the necessary output directory.
os.makedirs(output_dir, exist_ok=True)
for task_id in tqdm.tqdm(self.tasks):
try:
# Initialize environment and fetches origin->destination.
env = self.load(task_id)
town = env.simulator._town
world = env.simulator.world
origin = env.simulator.hero.get_transform()
destination = env.unwrapped.simulator.destination
# Gets global plan.
waypoints, _, distances = cutil.global_plan(
world,
origin.location,
destination.location,
)
# Converts locations to ego coordinates.
pixel_coordinates = list()
for waypoint in waypoints:
coordinates = cutil.carla_xyz_to_ndarray(
waypoint.transform.location)[:2]
pixel_coordinates.append(
world_to_pixel(coordinates, town=town))
pixel_coordinates = np.asarray(pixel_coordinates)
# Visualizes optimal task on CARLA map.
fig, ax = plt.subplots(figsize=(15.0, 15.0))
ax.imshow(
plt.imread(
os.path.join(
os.path.abspath(os.path.dirname(__file__)),
os.pardir,
os.pardir,
os.pardir,
"assets",
"maps",
"{}.png".format(town),
)))
cb = ax.scatter(
pixel_coordinates[..., 0],
pixel_coordinates[..., 1],
cmap="RdYlBu_r",
linewidth=0.1,
marker=".",
s=300,
c=np.linspace(0, 1, len(pixel_coordinates)),
)
# Box around the task.
center = np.mean(pixel_coordinates, axis=0)
ax.set(
title="{} | distance: {:.2f}".format(
task_id,
sum(distances),
),
frame_on=False,
xlim=[center[0] - 1000, center[0] + 1000],
ylim=[center[1] - 1000, center[1] + 1000],
)
ax.get_xaxis().set_visible(False)
ax.get_yaxis().set_visible(False)
# fig.colorbar(cb, ax=ax)
fig.savefig(
os.path.join(output_dir, "{}.png".format(task_id)),
bbox_inches="tight",
pad_inches=0,
transparent=True,
)
finally:
env.close()