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()