code: # Copyright (c) 2020 Horizon Robotics. All Rights Reserved.
  #
  # Licensed under the Apache License, Version 2.0 (the "License");
  # you may not use this file except in compliance with the License.
  # You may obtain a copy of the License at
  #
  #      http://www.apache.org/licenses/LICENSE-2.0
  #
  # Unless required by applicable law or agreed to in writing, software
  # distributed under the License is distributed on an "AS IS" BASIS,
  # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  # See the License for the specific language governing permissions and
  # limitations under the License.
  """CarlaEnvironment suite.
  
  To use this, there are two ways:
  
  1. Run the code within docker image horizonrobotics/alf:0.0.3-carla
     Both `Docker <https://docs.docker.com/engine/install/ubuntu/>`_ and
     `Nvidia-Docker2 <https://github.com/NVIDIA/nvidia-docker>`_ need to be installed.
  
  2. Install carla:
  
  .. code-block:: bash
  
      wget https://carla-releases.s3.eu-west-3.amazonaws.com/Linux/CARLA_0.9.9.tar.gz
      mkdir carla
      tar zxf CARLA_0.9.9.tar.gz -C carla
      cd carla/Import
      wget https://carla-releases.s3.eu-west-3.amazonaws.com/Linux/AdditionalMaps_0.9.9.tar.gz
      cd ..
      ./ImportAssert.sh
      easy_install PythonAPI/carla/dist/carla-0.9.9-py3.7-linux-x86_64.egg
  
  Make sure you are using python3.7
  
  """
  
  from collections import OrderedDict
  from absl import logging
  import gin
  import math
  import numpy as np
  import os
  import random
  import subprocess
  import sys
  import time
  import torch
  
  try:
      import carla
  except ImportError:
      carla = None
  
  import alf
  import alf.data_structures as ds
  from alf.utils import common
  from .suite_socialbot import _get_unused_port
  from .alf_environment import AlfEnvironment
  from .carla_sensors import (CameraSensor, CollisionSensor, GnssSensor,
                              IMUSensor, LaneInvasionSensor, NavigationSensor,
                              RadarSensor, World, MINIMUM_RENDER_WIDTH,
                              MINIMUM_RENDER_HEIGHT)
  
  
  def is_available():
      return carla is not None
  
  
  def geo_distance(loc1, loc2):
      """
      Args:
          loc1 (np.array): [latitude, longitude, altitude]. The units for altitude
              is meter.
          loc2 (np.array):
      Returns:
          float: distance in meters
      """
      earth_radius = 6371 * 1000
      d2r = math.pi / 180
  
      d = loc1 - loc2
      dlat = d[0] * d2r
      dlon = d[1] * d2r
      lat1 = loc1[0] * d2r
      lat2 = loc2[0] * d2r
      a = np.sin(
          0.5 * dlat)**2 + np.sin(0.5 * dlon)**2 * np.cos(lat1) * np.cos(lat2)
      c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a))
      c = earth_radius * c
      return np.sqrt(c * c + d[2] * d[2])
  
  
  def _calculate_relative_position(self_transform, location):
      """
      Args:
          self_transform (carla.Transform): transform of self actor
          location (np.ndarray): shape is [3] or [N, 3]
      Returns:
          np.ndarray: shape is same as location
      """
      trans = self_transform
      self_loc = trans.location
      yaw = math.radians(trans.rotation.yaw)
  
      self_loc = np.array([self_loc.x, self_loc.y, self_loc.z])
      cos, sin = np.cos(yaw), np.sin(yaw)
      rot = np.array([[cos, -sin, 0.], [sin, cos, 0.], [0., 0., 1.]])
      return np.matmul(location - self_loc, rot).astype(np.float32)
  
  
  def _calculate_relative_velocity(self_transform, velocity):
      """
      Args:
          self_transform (carla.Transform): transform of self actor
          velocity (np.ndarray): shape is [3] or [N, 3]
      Returns:
          np.ndarray: shape is same as location
      """
      trans = self_transform
      yaw = math.radians(trans.rotation.yaw)
  
      cos, sin = np.cos(yaw), np.sin(yaw)
      rot = np.array([[cos, -sin, 0.], [sin, cos, 0.], [0., 0., 1.]])
      return np.matmul(velocity, rot).astype(np.float32)
  
  
  def _to_numpy_loc(loc: carla.Location):
      return np.array([loc.x, loc.y, loc.z])
  
  
  @gin.configurable(blacklist=['actor', 'alf_world'])
  class Player(object):
      """Player is a vehicle with some sensors.
  
      An episode terminates if it reaches one of the following situations:
      1. the vehicle arrives at the goal.
      2. the time exceeds ``route_length / min_speed``.
      3. it get stuck because of a collision.
  
      At each step, the reward is given based on the following components:
      1. Arriving goal:  ``success_reward``
      2. Moving in the navigation direction: the number of meters moved
         This moving reward can be either dense of sparse depending on the argument
         ``sparse_reward``.
      3. Negative reward caused by collision: ``-min(max_collision_reward, max(epside_reward, 0))``
  
      Currently, the player has the these sensors: ``CollisionSensor``, ``GnssSensor``,
      ``IMUSensor``, ``CameraSensor``, ``LaneInvasionSensor`` , ``RadarSensor``,
      ``NavigationSensor``. See the documentation for these class for the definition
      the data generated by these sensors.
      """
  
      # over all reward
      REWARD_OVERALL = 0
  
      # distance in meter for moving along route
      # If using sparse reward (`sparse_reward` is True), this reward is only given
      # about every `sparse_reward_interval` meters
      # If not using sparse reward, this reward is given every steps.
      REWARD_DISTANCE = 1
  
      # 0/1 valued indicating whether there is collision
      REWARD_COLLISION = 2
  
      # 0/1 valued indicating reaching goal
      REWARD_SUCCESS = 3
  
      # dimension of the reward vector
      REWARD_DIMENSION = 4
  
      def __init__(self,
                   actor,
                   alf_world,
                   success_reward=100.,
                   success_distance_thresh=5.0,
                   max_collision_penalty=100.,
                   max_stuck_at_collision_seconds=5.0,
                   stuck_at_collision_distance=1.0,
                   sparse_reward=False,
                   sparse_reward_interval=10.,
                   allow_negative_distance_reward=True,
                   min_speed=5.,
                   with_gnss_sensor=True,
                   with_imu_sensor=True,
                   with_camera_sensor=True,
                   with_radar_sensor=True):
          """
          Args:
              actor (carla.Actor): the carla actor object
              alf_world (Wolrd): the world containing the player
              success_reward (float): the reward for arriving the goal location.
              success_distance_thresh (float): success is achieved if the current
                  location is with such distance of the goal
              max_collision_penalty (float): the maximum penalty (i.e. negative reward)
                  for collision. We don't want the collision penalty to be too large
                  if the player cannot even get enough positive moving reward. So the
                  panalty is capped at ``max(0., episode_reward))``. Note that this
                  reward is only given once at the first step of contiguous collisions.
              max_stuck_at_collision_seconds (float): the episode will end and is
                  considerred as failure if the car is stuck at the collision for
                  so many seconds,
              stuck_at_collision_distance (float): the car is considerred as being
                  stuck at the collision if it is within such distance of the first
                  collision location.
              sparse_reward (bool): If False, the distance reward is given at every
                  step based on how much it moves along the navigation route. If
                  True, the distance reward is only given after moving ``sparse_reward_distance``.
              sparse_reward_interval (float): the sparse reward is given after
                  approximately every such distance along the route has been driven.
              allow_negative_distance_reward (True): whether to allow negative distance
                  reward. If True, the agent will receive positive reward for moving
                  ahead along the route, and negative rewad for moving back along
                  the route. If False, the agent still receives positive reward for
                  moving ahead along the route, but will not receive negative rewad
                  for moving back along the route. Instead, the negative distance
                  will be accumulated to the future distance reward. This may ease
                  the learning if the right behavior is to temporarily go back along
                  the route in order, for examle, to avoid obstacle.
              min_speed (float): unit is m/s. Failure if initial_distance / min_speed
                  seconds passed
              with_gnss_sensor (bool): whether to use ``GnssSensor``.
              with_imu_sensor (bool): whether to use ``IMUSensor``.
              with_camera_sensor (bool): whether to use ``CameraSensor``.
              with_radar_sensor (bool): whether to use ``RadarSensor``.
          """
          self._actor = actor
          self._alf_world = alf_world
          self._observation_sensors = {}
  
          self._collision_sensor = CollisionSensor(actor)
          self._observation_sensors['collision'] = self._collision_sensor
  
          if with_gnss_sensor:
              self._gnss_sensor = GnssSensor(actor)
              self._observation_sensors['gnss'] = self._gnss_sensor
          else:
              self._gnss_sensor = None
  
          if with_imu_sensor:
              self._imu_sensor = IMUSensor(actor)
              self._observation_sensors['imu'] = self._imu_sensor
          else:
              self._imu_sensor = None
  
          if with_camera_sensor:
              self._camera_sensor = CameraSensor(actor)
              self._observation_sensors['camera'] = self._camera_sensor
          else:
              self._camera_sensor = None
  
          self._lane_invasion_sensor = LaneInvasionSensor(actor)
  
          if with_radar_sensor:
              self._radar_sensor = RadarSensor(actor)
              self._observation_sensors['radar'] = self._radar_sensor
          else:
              self._radar_sensor = None
  
          self._navigation = NavigationSensor(actor, alf_world)
          self._observation_sensors['navigation'] = self._navigation
  
          self._success_reward = success_reward
          self._success_distance_thresh = success_distance_thresh
          self._min_speed = min_speed
          self._delta_seconds = actor.get_world().get_settings(
          ).fixed_delta_seconds
          self._max_collision_penalty = max_collision_penalty
          self._max_stuck_at_collision_frames = max_stuck_at_collision_seconds / self._delta_seconds
          self._stuck_at_collision_distance = stuck_at_collision_distance
          self._sparse_reward = sparse_reward
          self._sparse_reward_index_interval = int(
              max(1, sparse_reward_interval // self._alf_world.route_resolution))
          self._allow_negative_distance_reward = allow_negative_distance_reward
  
          self._observation_spec = dict()
          self._observation_desc = dict()
          for sensor_name, sensor in self._observation_sensors.items():
              self._observation_spec[sensor_name] = sensor.observation_spec()
              self._observation_desc[sensor_name] = sensor.observation_desc()
          self._observation_spec['goal'] = alf.TensorSpec([3])
          self._observation_spec['velocity'] = alf.TensorSpec([3])
  
          # UE4 coordinate system is right handed:
          # https://forums.unrealengine.com/development-discussion/c-gameplay-programming/103787-ue4-coordinate-system-not-right-handed
          self._observation_desc['goal'] = (
              "Target location relative to the vehicle coordinate system in "
              "meters. X axis: front, Y axis: right, Z axis: up. Only the "
              "rotation around Z axis is taken into account when calculating the "
              "vehicle's coordinate system.")
          self._observation_desc['navigation'] = (
              'Relative positions of the future waypoints in the route')
          self._observation_desc[
              'velocity'] = "3D Velocity relative to self coordinate in m/s"
          self._info_spec = OrderedDict(
              success=alf.TensorSpec(()), collision=alf.TensorSpec(()))
  
          self._control = carla.VehicleControl()
          self.reset()
  
          # for rendering
          self._surface = None
          self._font = None
          self._clock = None
  
      def reset(self):
          """Reset the player location and goal.
  
          Use ``carla.Client.apply_batch_sync()`` to actually reset.
  
          Returns:
              list[carla.command]:
          """
  
          wp = random.choice(self._alf_world.get_waypoints())
          goal_loc = wp.transform.location
          self._goal_location = np.array([goal_loc.x, goal_loc.y, goal_loc.z],
                                         dtype=np.float32)
  
          forbidden_locations = []
          for v in self._alf_world.get_actors():
              if v.id == self._actor.id:
                  continue
              forbidden_locations.append(
                  self._alf_world.get_actor_location(v.id))
  
          # find a waypoint far enough from other vehicles
          ok = False
          i = 0
          while not ok and i < 100:
              wp = random.choice(self._alf_world.get_waypoints())
              loc = wp.transform.location
              ok = True
              for other_loc in forbidden_locations:
                  if loc.distance(other_loc) < 10.:
                      ok = False
                      break
              i += 1
          assert ok, "Fail to find new position"
          # loc.z + 0.27531 to avoid Z-collision, see Carla documentation for
          # carla.Map.get_spawn_points(). The value used by carla is slightly
          # smaller: 0.27530714869499207
          loc = carla.Location(loc.x, loc.y, loc.z + 0.3)
  
          commands = [
              carla.command.ApplyTransform(
                  self._actor, carla.Transform(loc, wp.transform.rotation)),
              carla.command.ApplyVelocity(self._actor, carla.Vector3D()),
              carla.command.ApplyAngularVelocity(self._actor, carla.Vector3D())
          ]
  
          self._max_frame = None
          self._done = False
          self._prev_location = loc
          self._prev_action = np.zeros(
              self.action_spec().shape, dtype=np.float32)
          self._alf_world.update_actor_location(self._actor.id, loc)
  
          self._route_length = self._navigation.set_destination(goal_loc)
  
          self._prev_collision = False  # whether there is collision in the previous frame
          self._collision = False  # whether there is colliion in the current frame
          self._collision_loc = None  # the location of the car when it starts to have collition
  
          # The intermediate goal for sparse reward
          self._intermediate_goal_index = min(self._sparse_reward_index_interval,
                                              self._navigation.num_waypoints - 1)
  
          # The location of the car when the intermediate goal is set
          self._intermediate_start = _to_numpy_loc(loc)
  
          self._episode_reward = 0.
          self._unrecorded_distance_reward = 0.
          self._is_first_step = True
  
          return commands
  
      def destroy(self):
          """Get the commands for destroying the player.
  
          Use carla.Client.apply_batch_sync() to actually destroy the sensor.
  
          Returns:
              list[carla.command]:
          """
          commands = []
          for sensor in self._observation_sensors.values():
              commands.extend(sensor.destroy())
          commands.extend(self._lane_invasion_sensor.destroy())
          commands.append(carla.command.DestroyActor(self._actor))
          if self._surface is not None:
              import pygame
              pygame.quit()
  
          return commands
  
      def observation_spec(self):
          """Get the observation spec.
  
          Returns:
              nested TensorSpec:
          """
          return self._observation_spec
  
      def observation_desc(self):
          """Get the description about the observation.
  
          Returns:
              nested str: each str corresponds to one TensorSpec from
              ``observatin_spec()``.
          """
          return self._observation_desc
  
      def action_spec(self):
          """Get the action spec.
  
          The action is a 4-D vector of [throttle, steer, brake, reverse], where
          throttle is in [-1.0, 1.0] (negative value is same as zero), steer is in
          [-1.0, 1.0], brake is in [-1.0, 1.0] (negative value is same as zero),
          and reverse is interpreted as a boolean value with values greater than
          0.5 corrsponding to True.
  
          Returns:
              nested BoundedTensorSpec:
          """
          return alf.BoundedTensorSpec([4],
                                       minimum=[-1., -1., -1., 0.],
                                       maximum=[1., 1., 1., 1.])
  
      def info_spec(self):
          """Get the info spec."""
          return self._info_spec
  
      def action_desc(self):
          """Get the description about the action.
  
          Returns:
              nested str: each str corresponds to one TensorSpec from
              ``action_spec()``.
          """
          return (
              "4-D vector of [throttle, steer, brake, reverse], where "
              "throttle is in [-1.0, 1.0] (negative value is same as zero), "
              "steer is in [-1.0, 1.0], brake is in [-1.0, 1.0] (negative value "
              "is same as zero), and reverse is interpreted as a boolean value "
              "with values greater than 0.5 corrsponding to True.")
  
      def reward_spec(self):
          """Get the reward spec."""
          return alf.TensorSpec([Player.REWARD_DIMENSION])
  
      def _get_goal(self):
          return _calculate_relative_position(self._actor.get_transform(),
                                              self._goal_location)
  
      def get_current_time_step(self, current_frame):
          """Get the current time step for the player.
  
          Args:
              current_frame (int): current simulation frame no.
          Returns:
              TimeStep: all elements are ``np.ndarray`` or ``np.number``.
          """
          obs = dict()
          for sensor_name, sensor in self._observation_sensors.items():
              obs[sensor_name] = sensor.get_current_observation(current_frame)
          obs['goal'] = self._get_goal()
          self._alf_world.update_actor_location(self._actor.id,
                                                self._actor.get_location())
          v = self._actor.get_velocity()
          obs['velocity'] = _calculate_relative_velocity(
              self._actor.get_transform(), _to_numpy_loc(v))
          self._current_distance = np.linalg.norm(obs['goal'])
  
          prev_loc = _to_numpy_loc(self._prev_location)
          curr_loc = _to_numpy_loc(self._actor.get_location())
  
          reward_vector = np.zeros(Player.REWARD_DIMENSION, np.float32)
          reward = 0.
          discount = 1.0
          info = OrderedDict(success=np.float32(0.0), collision=np.float32(0.0))
  
          # When the previous episode ends because of stucking at a collision with
          # another vehicle, it may get an additional collision event in the new frame
          # because the relocation of the car may happen after the simulation of the
          # moving. So we ignore the collision at the first step.
          self._collision = not np.all(
              obs['collision'] == 0) and not self._is_first_step
          if self._collision and not self._prev_collision:
              # We only report the first collision event among contiguous collision
              # events.
              info['collision'] = np.float32(1.0)
              logging.info("actor=%d frame=%d COLLISION" % (self._actor.id,
                                                            current_frame))
              self._collision_loc = curr_loc
              self._collision_frame = current_frame
              # We don't want the collision penalty to be too large if the player
              # cannot even get enough positive moving reward. So we cap the penalty
              # at ``max(0., self._episode_reward)``
              reward -= min(self._max_collision_penalty,
                            max(0., self._episode_reward))
              reward_vector[Player.REWARD_COLLISION] = 1.
  
          if self._max_frame is None:
              step_type = ds.StepType.FIRST
              max_frames = math.ceil(
                  self._route_length / self._min_speed / self._delta_seconds)
              self._max_frame = current_frame + max_frames
          elif (self._current_distance < self._success_distance_thresh
                and self._actor.get_velocity() == carla.Location(0., 0., 0.)):
              # TODO: include waypoint orientation as success critiria
              step_type = ds.StepType.LAST
              reward += self._success_reward
              reward_vector[Player.REWARD_SUCCESS] = 1.
              discount = 0.0
              info['success'] = np.float32(1.0)
              logging.info(
                  "actor=%d frame=%d SUCCESS" % (self._actor.id, current_frame))
          elif current_frame >= self._max_frame:
              logging.info("actor=%d frame=%d FAILURE: out of time" %
                           (self._actor.id, current_frame))
              step_type = ds.StepType.LAST
          elif (self._collision_loc is not None
                and current_frame - self._collision_frame >
                self._max_stuck_at_collision_frames
                and np.linalg.norm(curr_loc - self._collision_loc) <
                self._stuck_at_collision_distance):
              logging.info("actor=%d frame=%d FAILURE: stuck at collision" %
                           (self._actor.id, current_frame))
              step_type = ds.StepType.LAST
          else:
              step_type = ds.StepType.MID
  
          if self._sparse_reward:
              current_index = self._navigation.get_next_waypoint_index()
              if step_type == ds.StepType.LAST and info['success'] == 1.0:
                  # Since the episode is finished, we need to incorporate the final
                  # progress towards the goal as reward to encourage stopping near the goal.
                  distance_reward = (
                      np.linalg.norm(self._intermediate_start -
                                     self._goal_location) -
                      np.linalg.norm(curr_loc - self._goal_location))
              elif self._intermediate_goal_index < current_index:
                  # This means that the car has passed the intermediate goal.
                  # And we give it a reward which is equal to the distance it
                  # travels.
                  intermediate_goal = self._navigation.get_waypoint(
                      self._intermediate_goal_index)
                  distance_reward = np.linalg.norm(intermediate_goal -
                                                   self._intermediate_start)
                  self._intermediate_start = intermediate_goal
                  self._intermediate_goal_index = min(
                      self._intermediate_goal_index +
                      self._sparse_reward_index_interval,
                      self._navigation.num_waypoints - 1)
          else:
              goal0 = obs['navigation'][2]  # This is about 10m ahead
              distance_reward = (np.linalg.norm(prev_loc - goal0) -
                                 np.linalg.norm(curr_loc - goal0))
  
          reward_vector[Player.REWARD_DISTANCE] = distance_reward
          if not self._allow_negative_distance_reward:
              distance_reward += self._unrecorded_distance_reward
              if distance_reward < 0:
                  self._unrecorded_distance_reward = distance_reward
                  distance_reward = 0
              else:
                  self._unrecorded_distance_reward = 0
          reward += distance_reward
  
          obs['navigation'] = _calculate_relative_position(
              self._actor.get_transform(), obs['navigation'])
  
          self._done = step_type == ds.StepType.LAST
          self._episode_reward += reward
  
          reward_vector[Player.REWARD_OVERALL] = reward
  
          self._current_time_step = ds.TimeStep(
              step_type=step_type,
              reward=reward_vector,
              discount=np.float32(discount),
              observation=obs,
              prev_action=self._prev_action,
              env_info=info)
          return self._current_time_step
  
      def act(self, action):
          """Generate the carla command for taking the given action.
  
          Use ``carla.Client.apply_batch_sync()`` to actually destroy the sensor.
  
          Args:
              action (nested np.ndarray):
          Returns:
              list[carla.command]:
          """
          self._prev_collision = self._collision
          self._prev_location = self._actor.get_location()
          self._is_first_step = False
          if self._done:
              return self.reset()
          self._control.throttle = max(float(action[0]), 0.0)
          self._control.steer = float(action[1])
          self._control.brake = max(float(action[2]), 0.0)
          self._control.reverse = bool(action[3] > 0.5)
          self._prev_action = action
  
          return [carla.command.ApplyVehicleControl(self._actor, self._control)]
  
      def render(self, mode):
          """Render the simulation.
  
          Args:
              mode (str): one of ['rgb_array', 'human']
          Returns:
              one of the following:
                  - None: if mode is 'human'
                  - np.ndarray: the image of shape [height, width, channeles] if
                      mode is 'rgb_array'
          """
          import pygame
          if self._surface is None:
              pygame.init()
              pygame.font.init()
              self._clock = pygame.time.Clock()
              if self._camera_sensor:
                  height, width = self._camera_sensor.observation_spec(
                  ).shape[1:3]
                  height = max(height, MINIMUM_RENDER_HEIGHT)
                  width = max(width, MINIMUM_RENDER_WIDTH)
              else:
                  height = MINIMUM_RENDER_HEIGHT
                  width = MINIMUM_RENDER_WIDTH
              if mode == 'human':
                  self._surface = pygame.display.set_mode(
                      (width, height), pygame.HWSURFACE | pygame.DOUBLEBUF)
              else:
                  self._surface = pygame.Surface((width, height))
  
          if mode == 'human':
              self._clock.tick_busy_loop(1000)
  
          if self._camera_sensor:
              self._camera_sensor.render(self._surface)
          obs = self._current_time_step.observation
          np_precision = np.get_printoptions()['precision']
          np.set_printoptions(precision=1)
          info_text = [
              'FPS: %6.2f' % self._clock.get_fps(),
              'GPS:  (%7.4f, %8.4f, %5.2f)' % tuple(obs['gnss'].tolist()),
              'Goal: (%7.1f, %8.1f, %5.1f)' % tuple(obs['goal'].tolist()),
              'Ahead: (%7.1f, %8.1f, %5.1f)' % tuple(
                  obs['navigation'][2].tolist()),
              'Distance: %7.2f' % np.linalg.norm(obs['goal']),
              'Velocity: (%4.1f, %4.1f, %4.1f) km/h' % tuple(
                  (3.6 * obs['velocity']).tolist()),
              'Acceleration: (%4.1f, %4.1f, %4.1f)' % tuple(
                  obs['imu'][0:3].tolist()),
              'Compass: %5.1f' % math.degrees(float(obs['imu'][6])),
              'Throttle: %4.2f' % self._control.throttle,
              'Brake:    %4.2f' % self._control.brake,
              'Steer:    %4.2f' % self._control.steer,
              'Reverse:  %4s' % self._control.reverse,
              'Reward: (%s)' % self._current_time_step.reward,
          ]
          np.set_printoptions(precision=np_precision)
          self._draw_text(info_text)
  
          if mode == 'human':
              pygame.display.flip()
          elif mode == 'rgb_array':
              # (x, y, c) => (y, x, c)
              return np.transpose(
                  pygame.surfarray.array3d(self._surface), (1, 0, 2))
          else:
              raise ValueError("Unsupported render mode: %s" % mode)
  
      def _draw_text(self, texts):
          import os
          import pygame
          if self._font is None:
              font_name = 'courier' if os.name == 'nt' else 'mono'
              fonts = [x for x in pygame.font.get_fonts() if font_name in x]
              default_font = 'ubuntumono'
              mono = default_font if default_font in fonts else fonts[0]
              mono = pygame.font.match_font(mono)
              self._font = pygame.font.Font(mono, 12 if os.name == 'nt' else 14)
          info_surface = pygame.Surface((240, 240))
          info_surface.set_alpha(100)
          self._surface.blit(info_surface, (0, 0))
          v_offset = 4
          for item in texts:
              surface = self._font.render(item, True, (255, 255, 255))
              self._surface.blit(surface, (8, v_offset))
              v_offset += 18
  
  
  def _exec(command):
      stream = os.popen(command)
      ret = stream.read()
      stream.close()
      return ret
  
  
  gin.constant('CarlaEnvironment.REWARD_DIMENSION', Player.REWARD_DIMENSION)
  
  
  @gin.configurable
  class CarlaServer(object):
      """CarlaServer for doing the simulation."""
  
      def __init__(self,
                   rpc_port=2000,
                   streaming_port=2001,
                   docker_image="horizonrobotics/alf:0.0.3-carla",
                   quality_level="Low",
                   carla_root="/home/carla",
                   use_opengl=True):
          """
  
          Args:
              rpc_port (int): port for RPC
              streaming_port (int): port for data streaming
              docker_image (str): If provided, will use the docker image to start
                  the Carla server. Some valid images are "carlasim/carla:0.9.9"
                  and "horionrobotics/alf:0.0.3-carla"
              quality_level (str): one of ['Low', 'Epic']. See the explanation at
                  `<https://carla.readthedocs.io/en/latest/adv_rendering_options/#graphics-quality>`_
              carla_root (str): directorcy where CarlaUE4.sh is in. The default
                  value is correct for using docker image. If not using docker
                  image, make sure you provide the correct path. This is the directory
                  where you unzipped the file you downloaded from
                  `<https://github.com/carla-simulator/carla/releases/tag/0.9.9>`_.
              use_opengl (bool): the default graphics engine of Carla is Vulkan,
                  which is supposed to be better than OpenGL. However, Vulkan is not
                  always available. It may not be installed or the nvidia driver does
                  not support vulkan.
          """
          assert quality_level in ['Low', 'Epic'], "Unknown quality level"
          use_docker = (not alf.utils.common.is_inside_docker_container()
                        and docker_image)
          opengl = "-opengl" if use_opengl else ""
          if use_docker:
              dev = os.environ.get('CUDA_VISIBLE_DEVICES')
              if not dev:
                  dev = 'all'
              command = ("docker run -d "
                         "-p {rpc_port}:{rpc_port} "
                         "-p {streaming_port}:{streaming_port} "
                         "-u carla "
                         "--rm --gpus device=" + dev + " " + docker_image +
                         " {carla_root}/CarlaUE4.sh "
                         "--carla-rpc-port={rpc_port} "
                         "--carla-streaming-port={streaming_port} "
                         "--quality-level={quality_level} {opengl}")
          else:
              assert os.path.exists(carla_root + "/CarlaUE4.sh"), (
                  "%s/CarlaUE4.sh "
                  "does not exist. Please provide correct value for `carla_root`"
                  % carla_root)
              # We do not use CarlaUE4.sh here in order to get the actual Carla
              # server processs so that we can kill it.
              command = (
                  "{carla_root}/CarlaUE4/Binaries/Linux/CarlaUE4-Linux-Shipping "
                  "CarlaUE4 "  # perhaps most system does not have vulkan support, so we use opengl
                  "-carla-rpc-port={rpc_port} "
                  "-carla-streaming-port={streaming_port} "
                  "-quality-level={quality_level} {opengl}")
  
          command = command.format(
              rpc_port=rpc_port,
              streaming_port=streaming_port,
              quality_level=quality_level,
              carla_root=carla_root,
              opengl=opengl)
  
          logging.info("Starting Carla server: %s" % command)
          self._container_id = None
          self._process = None
          if use_docker:
              self._container_id = _exec(command)
              assert self._container_id, "Fail to start container"
              logging.info("Starting carla in container %s" % self._container_id)
          else:
              new_env = os.environ.copy()
              new_env['SDL_VIDEODRIVER'] = 'offscreen'
              self._process = subprocess.Popen(
                  command.split(),
                  stdout=sys.stdout,
                  stderr=sys.stderr,
                  env=new_env)
  
      def stop(self):
          """Stop the carla server."""
          if self._container_id:
              command = "docker kill %s" % self._container_id
              logging.info("Stopping Carla server: %s" % command)
              _exec(command)
              self._container_id = None
          if self._process:
              self._process.kill()
              self._process.communicate()
              self._process = None
  
      def __del__(self):
          self.stop()
  
  
  @gin.configurable
  class CarlaEnvironment(AlfEnvironment):
      """Carla simulation environment.
  
      In order to use it, you need to either download a valid docker image or
      a Carla package.
      """
  
      def __init__(self,
                   batch_size,
                   map_name,
                   vehicle_filter='vehicle.*',
                   walker_filter='walker.pedestrian.*',
                   num_other_vehicles=0,
                   num_walkers=0,
                   percentage_walkers_running=0.1,
                   percentage_walkers_crossing=0.1,
                   global_distance_to_leading_vehicle=2.0,
                   use_hybrid_physics_mode=True,
                   safe=True,
                   step_time=0.05):
          """
          Args:
              batch_size (int): the number of learning vehicles.
              map_name (str): the name of the map (e.g. "Town01")
              vehicle_filter (str): the filter for getting vehicle blueprints.
              walker_filter (str): the filter for getting walker blueprints.
              num_other_vehicles (int): the number of autopilot vehicles
              num_walkers (int): the number of walkers
              global_distance_to_leading_vehicle (str): the autopiloted vehicles
                  will try to keep such distance from other vehicles.
              percentage_walkers_running (float): percent of running walkers
              percentage_walkers_crossing (float): percent of walkers walking
                  across the road.
              use_hybrid_physics_mode (bool): If true, the autopiloted vehicle will
                  not use physics for simulation if it is far from other vehicles.
              safe (bool): avoid spawning vehicles prone to accidents.
              step_time (float): how many seconds does each step of simulation represents.
          """
          super().__init__()
  
          with _get_unused_port(2000, n=2) as (rpc_port, streaming_port):
              self._server = CarlaServer(rpc_port, streaming_port)
  
          self._batch_size = batch_size
          self._num_other_vehicles = num_other_vehicles
          self._num_walkers = num_walkers
          self._percentage_walkers_running = percentage_walkers_running
          self._percentage_walkers_crossing = percentage_walkers_crossing
  
          self._world = None
          try:
              for i in range(20):
                  try:
                      logging.info(
                          "Waiting for server to start. Try %d" % (i + 1))
                      self._client = carla.Client("localhost", rpc_port)
                      self._world = self._client.load_world(map_name)
                      break
                  except RuntimeError:
                      continue
          finally:
              if self._world is None:
                  self._server.stop()
                  assert self._world is not None, "Fail to start server."
  
          logging.info("Server started.")
  
          self._traffic_manager = None
          if self._num_other_vehicles + self._num_walkers > 0:
              with _get_unused_port(8000, n=1) as tm_port:
                  self._traffic_manager = self._client.get_trafficmanager(
                      tm_port)
              self._traffic_manager.set_hybrid_physics_mode(
                  use_hybrid_physics_mode)
              self._traffic_manager.set_global_distance_to_leading_vehicle(
                  global_distance_to_leading_vehicle)
  
          self._client.set_timeout(20)
          self._alf_world = World(self._world)
          self._safe = safe
          self._vehicle_filter = vehicle_filter
          self._walker_filter = walker_filter
  
          settings = self._world.get_settings()
          settings.synchronous_mode = True
          settings.fixed_delta_seconds = step_time
  
          self._world.apply_settings(settings)
          self._map_name = map_name
  
          self._spawn_vehicles()
          self._spawn_walkers()
  
          self._observation_spec = self._players[0].observation_spec()
          self._action_spec = self._players[0].action_spec()
          self._env_info_spec = self._players[0].info_spec()
          self._reward_spec = self._players[0].reward_spec()
  
          # metadata property is required by video recording
          self.metadata = {
              'render.modes': ['human', 'rgb_array'],
              'video.frames_per_second': 1 / step_time
          }
  
      def _spawn_vehicles(self):
          blueprints = self._world.get_blueprint_library().filter(
              self._vehicle_filter)
          assert len(
              blueprints) > 0, "Cannot find vehicle '%s'" % self._vehicle_filter
          if self._safe:
              blueprints = [
                  x for x in blueprints
                  if int(x.get_attribute('number_of_wheels')) == 4
              ]
              blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
              blueprints = [
                  x for x in blueprints if not x.id.endswith('carlacola')
              ]
              blueprints = [
                  x for x in blueprints if not x.id.endswith('cybertruck')
              ]
              blueprints = [x for x in blueprints if not x.id.endswith('t2')]
          assert len(
              blueprints
          ) > 0, "Cannot find safe vehicle '%s'" % self._vehicle_filter
  
          spawn_points = self._world.get_map().get_spawn_points()
          number_of_spawn_points = len(spawn_points)
  
          num_vehicles = self._batch_size + self._num_other_vehicles
          if num_vehicles <= number_of_spawn_points:
              random.shuffle(spawn_points)
          else:
              raise ValueError(
                  "requested %d vehicles, but could only find %d spawn points" %
                  (self._batch_size, number_of_spawn_points))
  
          commands = []
          for i, transform in enumerate(spawn_points[:num_vehicles]):
              blueprint = random.choice(blueprints)
              if blueprint.has_attribute('color'):
                  color = random.choice(
                      blueprint.get_attribute('color').recommended_values)
                  blueprint.set_attribute('color', color)
              if blueprint.has_attribute('driver_id'):
                  driver_id = random.choice(
                      blueprint.get_attribute('driver_id').recommended_values)
                  blueprint.set_attribute('driver_id', driver_id)
              if i < self._batch_size:
                  blueprint.set_attribute('role_name', 'hero')
              else:
                  blueprint.set_attribute('role_name', 'autopilot')
              command = carla.command.SpawnActor(blueprint, transform)
              if i >= self._batch_size:
                  # managed by traffic manager
                  command = command.then(
                      carla.command.SetAutopilot(
                          carla.command.FutureActor, True,
                          self._traffic_manager.get_port()))
              commands.append(command)
  
          self._players = []
          self._other_vehicles = []
          responses = self._client.apply_batch_sync(commands, True)
          for i, response in enumerate(responses):
              if response.error:
                  logging.error(response.error)
                  continue
              vehicle = self._world.get_actor(response.actor_id)
              if i < self._batch_size:
                  self._players.append(Player(vehicle, self._alf_world))
              else:
                  self._other_vehicles.append(vehicle)
              self._alf_world.add_actor(vehicle)
              self._alf_world.update_actor_location(vehicle.id,
                                                    spawn_points[i].location)
  
          assert len(self._players) + len(
              self._other_vehicles) == num_vehicles, (
                  "Fail to create %s vehicles" % num_vehicles)
  
      def _spawn_walkers(self):
          walker_blueprints = self._world.get_blueprint_library().filter(
              self._walker_filter)
  
          # 1. take all the random locations to spawn
          spawn_points = []
          for _ in range(self._num_walkers):
              spawn_point = carla.Transform()
              loc = self._world.get_random_location_from_navigation()
              if loc != None:
                  spawn_point.location = loc
                  spawn_points.append(spawn_point)
  
          # 2. we spawn the walker object
          commands = []
          walker_speeds = []
          for spawn_point in spawn_points:
              walker_bp = random.choice(walker_blueprints)
              # set as not invincible
              if walker_bp.has_attribute('is_invincible'):
                  walker_bp.set_attribute('is_invincible', 'false')
              # set the max speed
              if walker_bp.has_attribute('speed'):
                  if (random.random() > self._percentage_walkers_running):
                      # walking
                      walker_speeds.append(
                          walker_bp.get_attribute('speed').recommended_values[1])
                  else:
                      # running
                      walker_speeds.append(
                          walker_bp.get_attribute('speed').recommended_values[2])
              else:
                  logging.info("Walker has no speed")
                  walker_speeds.append(0.0)
              commands.append(carla.command.SpawnActor(walker_bp, spawn_point))
          responses = self._client.apply_batch_sync(commands, True)
          walker_speeds2 = []
          self._walkers = []
          for response, walker_speed, spawn_point in zip(
                  responses, walker_speeds, spawn_points):
              if response.error:
                  logging.error(
                      "%s: %s" % (response.error, spawn_point.location))
                  continue
              walker = self._world.get_actor(response.actor_id)
              self._walkers.append({"walker": walker})
              walker_speeds2.append(walker_speed)
          walker_speeds = walker_speeds2
  
          # 3. we spawn the walker controller
          commands = []
          walker_controller_bp = self._world.get_blueprint_library().find(
              'controller.ai.walker')
          for walker in self._walkers:
              commands.append(
                  carla.command.SpawnActor(walker_controller_bp,
                                           carla.Transform(),
                                           walker["walker"].id))
          responses = self._client.apply_batch_sync(commands, True)
          for response, walker in zip(responses, self._walkers):
              if response.error:
                  logging.error(response.error)
                  continue
              walker["controller"] = self._world.get_actor(response.actor_id)
  
          # wait for a tick to ensure client receives the last transform of the walkers we have just created
          self._world.tick()
  
          # 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...])
          # set how many pedestrians can cross the road
          self._world.set_pedestrians_cross_factor(
              self._percentage_walkers_crossing)
          for walker, walker_speed in zip(self._walkers, walker_speeds):
              # start walker
              walker['controller'].start()
              # set walk to random point
              location = self._world.get_random_location_from_navigation()
              walker['controller'].go_to_location(location)
              # max speed
              walker['controller'].set_max_speed(float(walker_speed))
              self._alf_world.add_actor(walker['walker'])
              self._alf_world.update_actor_location(walker['walker'].id,
                                                    location)
  
      def _clear(self):
          if self._world is None:
              return
          if self._players:
              commands = []
              for player in self._players:
                  commands.extend(player.destroy())
              for response in self._client.apply_batch_sync(commands, True):
                  if response.error:
                      logging.error(response.error)
              self._players.clear()
          commands = []
          for vehicle in self._other_vehicles:
              commands.append(carla.command.DestroyActor(vehicle))
          for walker in self._walkers:
              walker['controller'].stop()
              commands.append(carla.command.DestroyActor(walker['controller']))
              commands.append(carla.command.DestroyActor(walker['walker']))
  
          if commands:
              for response in self._client.apply_batch_sync(commands, True):
                  if response.error:
                      logging.error(response.error)
          self._other_vehicles.clear()
          self._walkers.clear()
  
      @property
      def batched(self):
          return True
  
      @property
      def batch_size(self):
          return self._batch_size
  
      def env_info_spec(self):
          return self._env_info_spec
  
      def observation_spec(self):
          return self._observation_spec
  
      def observation_desc(self):
          return self._players[0].observation_desc()
  
      def action_spec(self):
          return self._action_spec
  
      def action_desc(self):
          return self._players[0].action_desc()
  
      def reward_spec(self):
          return self._reward_spec
  
      def close(self):
          self._clear()
          self._server.stop()
  
      def __del__(self):
          self.close()
  
      @property
      def players(self):
          """Get all the players in the environment.
  
          Returns:
               list[Player]:
          """
          return self._players
  
      def render(self, mode):
          return self._players[0].render(mode)
  
      def _step(self, action):
          action = alf.nest.map_structure(lambda x: x.cpu().numpy(), action)
          commands = []
          for player, act in zip(self._players, action):
              commands.extend(player.act(act))
          for response in self._client.apply_batch_sync(commands):
              if response.error:
                  logging.error(response.error)
          self._current_frame = self._world.tick()
          for vehicle in self._other_vehicles:
              self._alf_world.update_actor_location(vehicle.id,
                                                    vehicle.get_location())
          for walker in self._walkers:
              actor = walker['walker']
              self._alf_world.update_actor_location(actor.id,
                                                    actor.get_location())
  
          return self._get_current_time_step()
  
      def _get_current_time_step(self):
          time_step = [
              player.get_current_time_step(self._current_frame)
              for player in self._players
          ]
          time_step = alf.nest.map_structure(lambda *a: np.stack(a), *time_step)
          time_step = alf.nest.map_structure(torch.as_tensor, time_step)
  
          common.check_numerics(time_step)
  
          return time_step._replace(env_id=torch.arange(self._batch_size))
  
      def _reset(self):
          commands = []
          for player in self._players:
              commands.extend(player.reset())
          for response in self._client.apply_batch_sync(commands):
              if response.error:
                  logging.error(response.error)
          self._current_frame = self._world.tick()
          return self._get_current_time_step()
  
  
  @gin.configurable(whitelist=['wrappers'])
  def load(map_name, batch_size, wrappers=[]):
      """Load CarlaEnvironment
  
      Args:
          map_name (str): name of the map. Currently available maps are:
              'Town01, Town02', 'Town03', 'Town04', 'Town05', 'Town06', 'Town07',
              and 'Town10HD'
          batch_size (int): the number of vehicles in the simulation.
          wrappers (list[AlfEnvironmentBaseWrapper]): environment wrappers
      Returns:
          CarlaEnvironment
      """
      env = CarlaEnvironment(batch_size, map_name)
      for wrapper in wrappers:
          env = wrapper(env)
      return env
  
  
  load.batched = True
  
apis: numpy.matmul numpy.get_printoptions numpy.array numpy.zeros numpy.stack numpy.set_printoptions numpy.cos numpy.float32 numpy.all torch.arange numpy.sqrt numpy.sin numpy.linalg.norm