From 04c6c4e8f56f3be5fe219c9ee2808f9e222888de Mon Sep 17 00:00:00 2001
From: Farzad <fnozarian@gmail.com>
Date: Fri, 13 Nov 2020 18:03:43 +0100
Subject: [PATCH 1/2] Initial changes to upgrade oatomobile for the latest
 carla (0.9.10)

---
 .../baselines/rulebased/autopilot/agent.py    | 257 ++----------------
 .../baselines/rulebased/autopilot/run.py      |   4 +-
 oatomobile/core/loop.py                       |   2 +-
 oatomobile/simulators/carla/simulator.py      |  30 +-
 oatomobile/utils/carla.py                     | 248 ++++++++++++-----
 5 files changed, 223 insertions(+), 318 deletions(-)

diff --git a/oatomobile/baselines/rulebased/autopilot/agent.py b/oatomobile/baselines/rulebased/autopilot/agent.py
index d1d5e57..fcaf07b 100644
--- a/oatomobile/baselines/rulebased/autopilot/agent.py
+++ b/oatomobile/baselines/rulebased/autopilot/agent.py
@@ -24,16 +24,12 @@
 
 import carla
 import oatomobile
+from oatomobile import envs
 from oatomobile.simulators.carla import defaults
 from oatomobile.utils import carla as cutil
 
 try:
-  from agents.navigation.local_planner import \
-      LocalPlanner  # pylint: disable=import-error
-  from agents.tools.misc import \
-      compute_magnitude_angle  # pylint: disable=import-error
-  from agents.tools.misc import \
-      is_within_distance_ahead  # pylint: disable=import-error
+  from agents.navigation.behavior_agent import BehaviorAgent
 except ImportError:
   raise ImportError("Missing CARLA installation, "
                     "make sure the environment variable CARLA_ROOT is provided "
@@ -45,19 +41,13 @@ class AutopilotAgent(oatomobile.Agent):
   `carla.PythonAPI.agents.navigation.basic_agent.BasicAgent`"""
 
   def __init__(self,
-               environment: oatomobile.envs.CARLAEnv,
+               environment: envs.CARLAEnv,
                *,
-               proximity_tlight_threshold: float = 5.0,
-               proximity_vehicle_threshold: float = 10.0,
-               noise: float = 0.1) -> None:
+               noise: float = 0.0) -> None:  # TODO(farzad) why should we have noise=0.1 for autopilot?
     """Constructs an autopilot agent.
 
     Args:
       environment: The navigation environment to spawn the agent.
-      proximity_tlight_threshold: The threshold (in metres) to
-        the traffic light.
-      proximity_vehicle_threshold: The threshold (in metres) to
-        the front vehicle.
       noise: The percentage of random actions.
     """
     super(AutopilotAgent, self).__init__(environment=environment)
@@ -66,36 +56,18 @@ def __init__(self,
     self._vehicle = self._environment.simulator.hero
     self._world = self._vehicle.get_world()
     self._map = self._world.get_map()
-
-    # Agent hyperparametres.
-    self._proximity_tlight_threshold = proximity_tlight_threshold
-    self._proximity_vehicle_threshold = proximity_vehicle_threshold
-    self._hop_resolution = 2.0
-    self._path_seperation_hop = 2
-    self._path_seperation_threshold = 0.5
-    self._target_speed = defaults.TARGET_SPEED
+    self._agent = BehaviorAgent(self._vehicle, behavior='normal')
     self._noise = noise
 
-    # The internal state of the agent.
-    self._last_traffic_light = None
+    spawn_points = self._map.get_spawn_points()
+    random.shuffle(spawn_points)
 
-    # Local planner, including the PID controllers.
-    dt = self._vehicle.get_world().get_settings().fixed_delta_seconds
-    lateral_control_dict = defaults.LATERAL_PID_CONTROLLER_CONFIG.copy()
-    lateral_control_dict.update({"dt": dt})
-    # TODO(filangel): tune the parameters for FPS != 20
-    self._local_planner = LocalPlanner(
-        self._vehicle,
-        opt_dict=dict(
-            target_speed=self._target_speed,
-            dt=dt,
-        ),
-    )
+    if spawn_points[0].location != self._agent.vehicle.get_location():
+      destination = spawn_points[0].location
+    else:
+      destination = spawn_points[1].location
 
-    # Set agent's dsestination.
-    if hasattr(self._environment.unwrapped.simulator, "destination"):
-      self._set_destination(
-          self._environment.unwrapped.simulator.destination.location)
+    self._agent.set_destination(self._agent.vehicle.get_location(), destination, clean=True)
 
   def act(
       self,
@@ -123,209 +95,12 @@ def _run_step(
   ) -> carla.VehicleControl:  # pylint: disable=no-member
     """Executes one step of navigation."""
 
-    # is there an obstacle in front of us?
-    hazard_detected = False
-
-    # retrieve relevant elements for safe navigation, i.e.: traffic lights
-    # and other vehicles
-    actor_list = self._world.get_actors()
-    vehicle_list = actor_list.filter("*vehicle*")
-    lights_list = actor_list.filter("*traffic_light*")
-
-    # check possible obstacles
-    vehicle_state, vehicle = self._is_vehicle_hazard(vehicle_list)
-    if vehicle_state:
-      if debug:
-        logging.debug('!!! VEHICLE BLOCKING AHEAD [{}])'.format(vehicle.id))
-
-      hazard_detected = True
-
-    # check for the state of the traffic lights
-    light_state, traffic_light = self._is_light_red(lights_list)
-    if light_state:
-      if debug:
-        logging.debug('=== RED LIGHT AHEAD [{}])'.format(traffic_light.id))
+    self._agent.update_information()
 
-      hazard_detected = True
+    speed_limit = self._vehicle.get_speed_limit()
+    self._agent.get_local_planner().set_speed(speed_limit)
 
-    if hazard_detected:
-      control = carla.VehicleControl()  # pylint: disable=no-member
-      control.steer = 0.0
-      control.throttle = 0.0
-      control.brake = 1.0
-      control.hand_brake = False
-    else:
-      # standard local planner behavior
-      control = self._local_planner.run_step(debug=debug)
+    control = self._agent.run_step()
 
     return control
 
-  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 _is_vehicle_hazard(
-      self,
-      vehicle_list,
-  ) -> Tuple[bool, Optional[carla.Vehicle]]:  # pylint: disable=no-member
-    """It detects if a vehicle in the scene can be dangerous for the ego
-    vehicle's current plan.
-
-    Args:
-      vehicle_list: List of potential vehicles (obstancles) to check.
-
-    Returns:
-      vehicle_ahead: If True a vehicle ahead blocking us and False otherwise.
-      vehicle: The blocker vehicle itself.
-    """
-
-    ego_vehicle_location = self._vehicle.get_location()
-    ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
-
-    for target_vehicle in vehicle_list:
-      # do not account for the ego vehicle.
-      if target_vehicle.id == self._vehicle.id:
-        continue
-
-      # if the object is not in our lane it's not an obstacle.
-      target_vehicle_waypoint = self._map.get_waypoint(
-          target_vehicle.get_location())
-      if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \
-              target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
-        continue
-
-      loc = target_vehicle.get_location()
-      if is_within_distance_ahead(
-          loc,
-          ego_vehicle_location,
-          self._vehicle.get_transform().rotation.yaw,
-          self._proximity_vehicle_threshold,
-      ):
-        return (True, target_vehicle)
-
-    return (False, None)
-
-  def _is_light_red(
-      self,
-      lights_list,
-  ) -> Tuple[bool, Any]:  # pylint: disable=no-member
-    """It detects if the light in the scene is red.
-
-    Args:
-      lights_list: The list containing TrafficLight objects.
-
-    Returns:
-      light_ahead: If True a traffic light ahead is read and False otherwise.
-      traffic_light: The traffic light object ahead itself.
-    """
-    if self._map.name == 'Town01' or self._map.name == 'Town02':
-      return self._is_light_red_europe_style(lights_list)
-    else:
-      return self._is_light_red_us_style(lights_list)
-
-  def _is_light_red_europe_style(self, lights_list):
-    """This method is specialized to check European style traffic lights."""
-    ego_vehicle_location = self._vehicle.get_location()
-    ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
-
-    for traffic_light in lights_list:
-      object_waypoint = self._map.get_waypoint(traffic_light.get_location())
-      if object_waypoint.road_id != ego_vehicle_waypoint.road_id or \
-              object_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
-        continue
-
-      loc = traffic_light.get_location()
-      if is_within_distance_ahead(
-          loc,
-          ego_vehicle_location,
-          self._vehicle.get_transform().rotation.yaw,
-          self._proximity_tlight_threshold,
-      ):
-        if traffic_light.state == carla.TrafficLightState.Red:  # pylint: disable=no-member
-          return (True, traffic_light)
-
-    return (False, None)
-
-  def _is_light_red_us_style(self, lights_list, debug=False):
-    """This method is specialized to check US style traffic lights."""
-    ego_vehicle_location = self._vehicle.get_location()
-    ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
-
-    if ego_vehicle_waypoint.is_junction:
-      # It is too late. Do not block the intersection! Keep going!
-      return (False, None)
-
-    if self._local_planner.target_waypoint is not None:
-      if self._local_planner.target_waypoint.is_junction:
-        min_angle = 180.0
-        sel_magnitude = 0.0
-        sel_traffic_light = None
-        for traffic_light in lights_list:
-          loc = traffic_light.get_location()
-          magnitude, angle = compute_magnitude_angle(
-              loc, ego_vehicle_location,
-              self._vehicle.get_transform().rotation.yaw)
-          if magnitude < 60.0 and angle < min(25.0, min_angle):
-            sel_magnitude = magnitude
-            sel_traffic_light = traffic_light
-            min_angle = angle
-
-        if sel_traffic_light is not None:
-          if debug:
-            logging.debug('=== Magnitude = {} | Angle = {} | ID = {}'.format(
-                sel_magnitude, min_angle, sel_traffic_light.id))
-
-          if self._last_traffic_light is None:
-            self._last_traffic_light = sel_traffic_light
-
-          if self._last_traffic_light.state == carla.TrafficLightState.Red:  # pylint: disable=no-member
-            return (True, self._last_traffic_light)
-        else:
-          self._last_traffic_light = None
-
-    return (False, None)
-
-  def _get_trafficlight_trigger_location(
-      self,
-      traffic_light,
-  ) -> carla.Location:  # pylint: disable=no-member
-    """Calculates the yaw of the waypoint that represents the trigger volume of
-    the traffic light."""
-
-    def rotate_point(point, radians):
-      """Rotates a given point by a given angle."""
-      rotated_x = math.cos(radians) * point.x - math.sin(radians) * point.y
-      rotated_y = math.sin(radians) * point.x - math.cos(radians) * point.y
-
-      return carla.Vector3D(rotated_x, rotated_y, point.z)  # pylint: disable=no-member
-
-    base_transform = traffic_light.get_transform()
-    base_rot = base_transform.rotation.yaw
-    area_loc = base_transform.transform(traffic_light.trigger_volume.location)
-    area_ext = traffic_light.trigger_volume.extent
-
-    point = rotate_point(
-        carla.Vector3D(0, 0, area_ext.z),  # pylint: disable=no-member
-        math.radians(base_rot),
-    )
-    point_location = area_loc + carla.Location(x=point.x, y=point.y)  # pylint: disable=no-member
-
-    return carla.Location(point_location.x, point_location.y, point_location.z)  # pylint: disable=no-member
diff --git a/oatomobile/baselines/rulebased/autopilot/run.py b/oatomobile/baselines/rulebased/autopilot/run.py
index 3a3c637..ae5593e 100644
--- a/oatomobile/baselines/rulebased/autopilot/run.py
+++ b/oatomobile/baselines/rulebased/autopilot/run.py
@@ -95,7 +95,9 @@ def main(argv):
       )
     if output_dir is not None:
       env = oatomobile.SaveToDiskWrapper(env, output_dir=output_dir)
-    env = oatomobile.MonitorWrapper(env, output_fname="tmp/yoo.gif")
+
+    # MonitorWrapper is incompatible with render_mode = human. Either it or --render should be used at the same time.
+    env = oatomobile.MonitorWrapper(env, output_fname="carla_dataset/carla_dataset.gif")
 
     # Runs the environment loop.
     oatomobile.EnvironmentLoop(
diff --git a/oatomobile/core/loop.py b/oatomobile/core/loop.py
index a301348..31482bc 100644
--- a/oatomobile/core/loop.py
+++ b/oatomobile/core/loop.py
@@ -104,7 +104,7 @@ def run(self) -> Optional[Mapping[str, types.Scalar]]:
     finally:
       # Garbage collector.
       try:
-        environment.close()
+        self._environment.close()
       except NameError:
         pass
 
diff --git a/oatomobile/simulators/carla/simulator.py b/oatomobile/simulators/carla/simulator.py
index 7396845..43dc813 100644
--- a/oatomobile/simulators/carla/simulator.py
+++ b/oatomobile/simulators/carla/simulator.py
@@ -87,7 +87,11 @@ def __init__(
     self.config = config
     self.sensor = self._spawn_sensor(hero, self.config)  # pylint: disable=no-member
     self.queue = queue.Queue()
-    self.sensor.listen(self.queue.put)
+    self.sensor.listen(self.put_to_queue)
+
+  def put_to_queue(self, item):
+    # logging.warning(f"frame {item.frame} added.")
+    self.queue.put(item)
 
   @property
   def observation_space(self, *args: Any, **kwargs: Any) -> gym.spaces.Box:
@@ -146,8 +150,11 @@ def get_observation(
     try:
       while True:
         data = self.queue.get(timeout=timeout)
+        # logging.debug(f"frame {data.frame} removed from queue. frame {frame} requested.")
+
         # Confirms synced frames.
         if data.frame == frame:
+          # logging.debug(f"received/removed synced frame {data.frame} from queue.")
           break
       # Processes the raw sensor data to a RGB array.
       return cutil.carla_rgb_image_to_ndarray(data)
@@ -1272,7 +1279,7 @@ def get_observation(self, frame: int,
     # Fetches hero measurements for the coordinate transformations.
     hero_transform = self._hero.get_transform()
 
-    if self._goal is None or self._num_steps % self._replan_every_steps == 0:
+    if self._goal is None or self._num_steps % int(self._replan_every_steps) == 0:
       # References to CARLA objects.
       carla_world = self._hero.get_world()
       carla_map = carla_world.get_map()
@@ -1291,7 +1298,7 @@ def get_observation(self, frame: int,
 
       # Samples goals.
       goals_world = [waypoints[0]]
-      for _ in range(self._num_goals - 1):
+      for _ in range(int(self._num_goals) - 1):
         goals_world.append(goals_world[-1].next(self._sampling_radius)[0])
 
       # Converts goals to `NumPy` arrays.
@@ -1639,6 +1646,7 @@ def __init__(
     self._world = None
     self._frame = None
     self._server = None
+    self._traffic_manager = None
     self._frame0 = None
     self._dt = None
     self._vehicles = None
@@ -1709,7 +1717,7 @@ def reset(self) -> simulator.Observations:
       The initial observations.
     """
     # CARLA setup.
-    self._client, self._world, self._frame, self._server = cutil.setup(
+    self._client, self._world, self._frame, self._server, self._traffic_manager = cutil.setup(
         town=self._town,
         fps=self._fps,
         client_timeout=self._client_timeout,
@@ -1721,17 +1729,15 @@ def reset(self) -> simulator.Observations:
     self._hero = cutil.spawn_hero(
         world=self._world,
         spawn_point=self.spawn_point,
-        vehicle_id="vehicle.ford.mustang",
+        vehicle_id="vehicle.tesla.model3",
     )
     # Initializes the other vehicles.
-    self._vehicles = cutil.spawn_vehicles(
+    self._vehicles, self._pedestrians = cutil.spawn_vehicles_and_pedestrians(
         world=self._world,
+        client=self._client,
+        traffic_manager=self._traffic_manager,
         num_vehicles=self._num_vehicles,
-    )
-    # Initializes the pedestrians.
-    self._pedestrians = cutil.spawn_pedestrians(
-        world=self._world,
-        num_pedestrians=self._num_pedestrians,
+        num_pedestrians=self._num_pedestrians
     )
     # Registers the sensors.
     self._sensor_suite = simulator.SensorSuite([
@@ -1786,7 +1792,7 @@ def render(self, mode: str = "human", **kwargs) -> Any:
     if mode not in ("human", "rgb_array"):
       raise ValueError("Unrecognised mode value {} passed.".format(mode))
 
-    if self._display is None or self._clock is None is None:
+    if self._display is None or self._clock is None:
       # TODO(filangel): clean this up
       width = 0
       if "left_camera_rgb" in self._observations:
diff --git a/oatomobile/utils/carla.py b/oatomobile/utils/carla.py
index af9e3ae..08a68c7 100644
--- a/oatomobile/utils/carla.py
+++ b/oatomobile/utils/carla.py
@@ -47,7 +47,7 @@ def setup(
     server_timestop: float = 20.0,
     client_timeout: float = 20.0,
     num_max_restarts: int = 5,
-) -> Tuple[carla.Client, carla.World, int, subprocess.Popen]:  # pylint: disable=no-member
+) -> Tuple[carla.Client, carla.World, int, subprocess.Popen, carla.TrafficManager]:  # pylint: disable=no-member
   """Returns the `CARLA` `server`, `client` and `world`.
 
   Args:
@@ -79,14 +79,15 @@ def setup(
 
     # Start CARLA server.
     env = os.environ.copy()
-    env["SDL_VIDEODRIVER"] = "offscreen"
-    env["SDL_HINT_CUDA_DEVICE"] = "0"
+    # to enable off-screen uncomment the following line and use -opengl flag for running carla
+    # env["DISPLAY"] = ""
     logging.debug("Inits a CARLA server at port={}".format(port))
     server = subprocess.Popen(
         [
             os.path.join(os.environ.get("CARLA_ROOT"), "CarlaUE4.sh"),
             "-carla-rpc-port={}".format(port),
             "-quality-level=Epic",
+            "-opengl"
         ],
         stdout=None,
         stderr=subprocess.STDOUT,
@@ -103,6 +104,9 @@ def setup(
       client.set_timeout(client_timeout)
       client.load_world(map_name=town)
       world = client.get_world()
+
+      traffic_manager = client.get_trafficmanager(8000)
+
       world.set_weather(carla.WeatherParameters.ClearNoon)  # pylint: disable=no-member
       frame = world.apply_settings(
           carla.WorldSettings(  # pylint: disable=no-member
@@ -110,9 +114,13 @@ def setup(
               synchronous_mode=True,
               fixed_delta_seconds=1.0 / fps,
           ))
+
+      traffic_manager.set_global_distance_to_leading_vehicle(1.0)
+      traffic_manager.set_synchronous_mode(True)
+
       logging.debug("Server version: {}".format(client.get_server_version()))
       logging.debug("Client version: {}".format(client.get_client_version()))
-      return client, world, frame, server
+      return client, world, frame, server, traffic_manager
     except RuntimeError as msg:
       logging.debug(msg)
       attempts += 1
@@ -206,7 +214,7 @@ def splat_points(
 
   # Serialise and parse to `NumPy` tensor.
   points = np.frombuffer(lidar_measurement.raw_data, dtype=np.dtype("f4"))
-  points = np.reshape(points, (int(points.shape[0] / 3), 3))
+  points = np.reshape(points, (int(points.shape[0] / 4), 4))
 
   # Split observations in the Z dimension (height).
   below = points[points[..., 2] <= -2.5]
@@ -259,14 +267,17 @@ def spawn_hero(
   hero_bp.set_attribute("role_name", "hero")
   logging.debug("Spawns hero actor at {}".format(
       carla_xyz_to_ndarray(spawn_point.location)))
-  hero = world.spawn_actor(hero_bp, spawn_point)
+  hero = world.try_spawn_actor(hero_bp, spawn_point)
   return hero
 
 
-def spawn_vehicles(
+def spawn_vehicles_and_pedestrians(
     world: carla.World,  # pylint: disable=no-member
+    client: carla.Client,
+    traffic_manager: carla.TrafficManager,
     num_vehicles: int,
-) -> Sequence[carla.Vehicle]:  # pylint: disable=no-member
+    num_pedestrians: int,
+) -> Tuple[Sequence[str], Sequence[str]]:  # pylint: disable=no-member
   """Spawns `vehicles` randomly in spawn points.
 
   Args:
@@ -276,62 +287,172 @@ def spawn_vehicles(
   Returns:
     The list of vehicles actors.
   """
-  # Blueprints library.
-  bl = world.get_blueprint_library()
-  # List of spawn points.
-  spawn_points = world.get_map().get_spawn_points()
-  # Output container
-  actors = list()
-  for _ in range(num_vehicles):
-    # Fetch random blueprint.
-    vehicle_bp = random.choice(bl.filter("vehicle.*"))
-    # Attempt to spawn vehicle in random location.
-    actor = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points))
-    if actor is not None:
-      # Enable autopilot.
-      actor.set_autopilot(True)
-      # Append actor to the list.
-      actors.append(actor)
-  logging.debug("Spawned {} other vehicles".format(len(actors)))
-  return actors
-
-
-def spawn_pedestrians(
-    world: carla.World,  # pylint: disable=no-member
-    num_pedestrians: int,
-    speeds: Sequence[float] = (1.0, 1.5, 2.0),
-) -> Sequence[carla.Vehicle]:  # pylint: disable=no-member
-  """Spawns `pedestrians` in random locations.
+  vehicles_list = []
+  walkers_list = []
+  all_id = []
 
-  Args:
-    world: The world object associated with the simulation.
-    num_pedestrians: The number of pedestrians to spawn.
-    speeds: The valid set of speeds for the pedestrians.
+  # Assume synchronous mode
+  synchronous_master = True
+  try:
 
-  Returns:
-    The list of pedestrians actors.
-  """
-  # Blueprints library.
-  bl = world.get_blueprint_library()
-  # Output container
-  actors = list()
-  for n in range(num_pedestrians):
-    # Fetch random blueprint.
-    pedestrian_bp = random.choice(bl.filter("walker.pedestrian.*"))
-    # Make pedestrian invicible.
-    pedestrian_bp.set_attribute("is_invincible", "true")
-    while len(actors) != n:
-      # Get random location.
-      spawn_point = carla.Transform()  # pylint: disable=no-member
-      spawn_point.location = world.get_random_location_from_navigation()
-      if spawn_point.location is None:
-        continue
-      # Attempt to spawn vehicle in random location.
-      actor = world.try_spawn_actor(pedestrian_bp, spawn_point)
-      if actor is not None:
-        actors.append(actor)
-  logging.debug("Spawned {} pedestrians".format(len(actors)))
-  return actors
+
+      blueprints = world.get_blueprint_library().filter('vehicle.*')
+      blueprintsWalkers = world.get_blueprint_library().filter('walker.pedestrian.*')
+
+      # avoid spawning vehicles prone to accidents
+      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')]
+
+      blueprints = sorted(blueprints, key=lambda bp: bp.id)
+
+      spawn_points = world.get_map().get_spawn_points()
+      number_of_spawn_points = len(spawn_points)
+
+      if num_vehicles < number_of_spawn_points:
+          random.shuffle(spawn_points)
+      elif num_vehicles > number_of_spawn_points:
+          msg = 'requested %d vehicles, but could only find %d spawn points'
+          logging.warning(msg, num_vehicles, number_of_spawn_points)
+          num_vehicles = number_of_spawn_points
+
+      # @todo cannot import these directly.
+      SpawnActor = carla.command.SpawnActor
+      SetAutopilot = carla.command.SetAutopilot
+      SetVehicleLightState = carla.command.SetVehicleLightState
+      FutureActor = carla.command.FutureActor
+
+      # --------------
+      # Spawn vehicles
+      # --------------
+      batch = []
+      for n, transform in enumerate(spawn_points):
+          if n >= num_vehicles:
+              break
+          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)
+          blueprint.set_attribute('role_name', 'autopilot')
+
+          # prepare the light state of the cars to spawn
+          light_state = carla.VehicleLightState.Position | carla.VehicleLightState.LowBeam  # (farzad) Does not work!
+          # light_state = carla.VehicleLightState.NONE
+
+          # spawn the cars and set their autopilot and light state all together
+          batch.append(SpawnActor(blueprint, transform)
+                       .then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))
+                       .then(SetVehicleLightState(FutureActor, light_state)))
+
+      for response in client.apply_batch_sync(batch, synchronous_master):
+          if response.error:
+              logging.error(response.error)
+          else:
+              vehicles_list.append(response.actor_id)
+
+      # -------------
+      # Spawn Walkers
+      # -------------
+      # some settings
+      percentagePedestriansRunning = 0.0  # how many pedestrians will run
+      percentagePedestriansCrossing = 0.0  # how many pedestrians will walk through the road
+      # 1. take all the random locations to spawn
+      spawn_points = []
+      for i in range(num_pedestrians):
+          spawn_point = carla.Transform()
+          loc = world.get_random_location_from_navigation()
+          if (loc != None):
+              spawn_point.location = loc
+              spawn_points.append(spawn_point)
+      # 2. we spawn the walker object
+      batch = []
+      walker_speed = []
+      for spawn_point in spawn_points:
+          walker_bp = random.choice(blueprintsWalkers)
+          # 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() > percentagePedestriansRunning):
+                  # walking
+                  walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1])
+              else:
+                  # running
+                  walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2])
+          else:
+              print("Walker has no speed")
+              walker_speed.append(0.0)
+          batch.append(SpawnActor(walker_bp, spawn_point))
+      results = client.apply_batch_sync(batch, True)
+      walker_speed2 = []
+      for i in range(len(results)):
+          if results[i].error:
+              logging.error(results[i].error)
+          else:
+              walkers_list.append({"id": results[i].actor_id})
+              walker_speed2.append(walker_speed[i])
+      walker_speed = walker_speed2
+      # 3. we spawn the walker controller
+      batch = []
+      walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker')
+      for i in range(len(walkers_list)):
+          batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"]))
+      results = client.apply_batch_sync(batch, True)
+      for i in range(len(results)):
+          if results[i].error:
+              logging.error(results[i].error)
+          else:
+              walkers_list[i]["con"] = results[i].actor_id
+      # 4. we put altogether the walkers and controllers id to get the objects from their id
+      for i in range(len(walkers_list)):
+          all_id.append(walkers_list[i]["con"])
+          all_id.append(walkers_list[i]["id"])
+      all_actors = world.get_actors(all_id)
+
+      # wait for a tick to ensure client receives the last transform of the walkers we have just created
+      if not synchronous_master:
+          world.wait_for_tick()
+      else:
+          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
+      world.set_pedestrians_cross_factor(percentagePedestriansCrossing)
+      for i in range(0, len(all_id), 2):
+          # start walker
+          all_actors[i].start()
+          # set walk to random point
+          all_actors[i].go_to_location(world.get_random_location_from_navigation())
+          # max speed
+          all_actors[i].set_max_speed(float(walker_speed[int(i / 2)]))
+  except:
+      if synchronous_master:
+          settings = world.get_settings()
+          settings.synchronous_mode = False
+          traffic_manager.set_synchronous_mode(False)
+          settings.fixed_delta_seconds = None
+          world.apply_settings(settings)
+
+      logging.debug(('\ndestroying %d vehicles' % len(vehicles_list)))
+      client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list])
+
+      # stop walker controllers (list is [controller, actor, controller, actor ...])
+      for i in range(0, len(all_id), 2):
+          all_actors[i].stop()
+
+      logging.debug('\ndestroying %d walkers' % len(walkers_list))
+      client.apply_batch([carla.command.DestroyActor(x) for x in all_id])
+
+      time.sleep(0.5)
+
+  logging.debug("Spawned {} vehicles and {} pedestrians".format(len(vehicles_list), len(walkers_list)))
+  return vehicles_list, [w['id'] for w in walkers_list]
 
 
 def spawn_camera(
@@ -368,6 +489,7 @@ def spawn_camera(
           carla.Rotation(**config["actor"]["rotation"]),  # pylint: disable=no-member
       ),
       attach_to=hero,
+      attachment_type=carla.AttachmentType.Rigid
   )
 
 
@@ -550,7 +672,7 @@ def carla_xyz_to_ndarray(xyz: Any) -> np.ndarray:
 
 
 def carla_rotation_to_ndarray(
-    rotation: carla.VehicleControl  # pylint: disable=no-member
+    rotation: carla.Rotation  # pylint: disable=no-member
 ) -> np.ndarray:
   """Converts a `CARLA` rotation to a neural network friendly tensor."""
   return np.asarray(

From 7bd41ad2c871545ef5dd7471621dfc6fa0c9a8eb Mon Sep 17 00:00:00 2001
From: Farzad <fnozarian@gmail.com>
Date: Sat, 14 Nov 2020 17:48:03 +0100
Subject: [PATCH 2/2] Added support for starting/stopping Carla server

---
 README.md                                     |  12 +-
 .../baselines/rulebased/autopilot/run.py      |   8 +-
 oatomobile/baselines/rulebased/blind/run.py   |   9 +-
 oatomobile/envs/carla.py                      |   9 +-
 oatomobile/simulators/carla/simulator.py      |  70 +++++++--
 oatomobile/utils/carla.py                     | 145 +++++++-----------
 6 files changed, 143 insertions(+), 110 deletions(-)

diff --git a/README.md b/README.md
index 2f31526..13e3bd5 100644
--- a/README.md
+++ b/README.md
@@ -71,18 +71,18 @@ We have tested OATomobile on Python 3.5.
 1.  To install the core libraries (including [CARLA], the backend simulator):
 
     ```bash
-    # The path to download CARLA 0.9.6.
+    # The path to download CARLA 0.9.10.
     export CARLA_ROOT=...
     mkdir -p $CARLA_ROOT
 
     # Downloads hosted binaries.
-    wget http://carla-assets-internal.s3.amazonaws.com/Releases/Linux/CARLA_0.9.6.tar.gz
+    wget https://carla-releases.s3.eu-west-3.amazonaws.com/Linux/CARLA_0.9.10.tar.gz
 
-    # CARLA 0.9.6 installation.
-    tar -xvzf CARLA_0.9.6.tar.gz -C $CARLA_ROOT
+    # CARLA 0.9.10 installation.
+    tar -xvzf CARLA_0.9.10.tar.gz -C $CARLA_ROOT
 
-    # Installs CARLA 0.9.6 Python API.
-    easy_install $CARLA_ROOT/PythonAPI/carla/dist/carla-0.9.6-py3.5-linux-x86_64.egg
+    # Installs CARLA 0.9.10 Python API.
+    easy_install $CARLA_ROOT/PythonAPI/carla/dist/carla-0.9.10-py3.7-linux-x86_64.egg
     ```
 
 1.  To install the OATomobile core API:
diff --git a/oatomobile/baselines/rulebased/autopilot/run.py b/oatomobile/baselines/rulebased/autopilot/run.py
index ae5593e..132a62e 100644
--- a/oatomobile/baselines/rulebased/autopilot/run.py
+++ b/oatomobile/baselines/rulebased/autopilot/run.py
@@ -67,7 +67,11 @@
     default=False,
     help="If True it spawn the `PyGame` display.",
 )
-
+flags.DEFINE_bool(
+    name="off_screen",
+    default=False,
+    help="If True it runs the Carla server in off-screen mode",
+)
 
 def main(argv):
   # Debugging purposes.
@@ -80,6 +84,7 @@ def main(argv):
   max_episode_steps = FLAGS.max_episode_steps
   output_dir = FLAGS.output_dir
   render = FLAGS.render
+  off_screen = FLAGS.off_screen
 
   try:
     # Setups the environment.
@@ -87,6 +92,7 @@ def main(argv):
         town=town,
         fps=20,
         sensors=sensors,
+        off_screen=off_screen
     )
     if max_episode_steps is not None:
       env = oatomobile.FiniteHorizonWrapper(
diff --git a/oatomobile/baselines/rulebased/blind/run.py b/oatomobile/baselines/rulebased/blind/run.py
index 85b8d8b..cd00f62 100644
--- a/oatomobile/baselines/rulebased/blind/run.py
+++ b/oatomobile/baselines/rulebased/blind/run.py
@@ -70,7 +70,11 @@
     default=False,
     help="If True it spawn the `PyGame` display.",
 )
-
+flags.DEFINE_bool(
+    name="off_screen",
+    default=False,
+    help="If True it runs the Carla server in off-screen mode",
+)
 
 def main(argv):
   # Debugging purposes.
@@ -83,7 +87,7 @@ def main(argv):
   max_episode_steps = FLAGS.max_episode_steps
   output_dir = FLAGS.output_dir
   render = FLAGS.render
-
+  off_screen = FLAGS.off_screen
   try:
     # Setups the environment.
     env = oatomobile.envs.CARLANavEnv(
@@ -92,6 +96,7 @@ def main(argv):
         destination=25,
         fps=20,
         sensors=sensors,
+        off_screen=off_screen
     )
     if max_episode_steps is not None:
       env = oatomobile.FiniteHorizonWrapper(
diff --git a/oatomobile/envs/carla.py b/oatomobile/envs/carla.py
index 9e7b428..90e8e46 100644
--- a/oatomobile/envs/carla.py
+++ b/oatomobile/envs/carla.py
@@ -48,7 +48,8 @@ def __init__(
       fps: int = defaults.SIMULATOR_FPS,
       sensors: Sequence[str] = defaults.CARLA_SENSORS,
       num_vehicles: int = 0,
-      num_pedestrians: int = 0) -> None:
+      num_pedestrians: int = 0,
+      off_screen: bool = False) -> None:
     """Constructs a CARLA simulator-based OpenAI gym-compatible environment.
 
     Args:
@@ -65,6 +66,7 @@ def __init__(
       sensors: The set of sensors registered on the ego vehicle.
       num_vehicles: The number of vehicles to spawn.
       num_pedestrians: The number of pedestrians to spawn.
+      off_screen: If true it starts Carla server in off-screen mode.
     """
     # Makes sure main sensors are registered and that passed are registered.
     _sensors = set([
@@ -91,6 +93,7 @@ def __init__(
         destination=destination,
         num_vehicles=num_vehicles,
         num_pedestrians=num_pedestrians,
+        off_screen=off_screen
     )
 
   @property
@@ -131,7 +134,8 @@ def __init__(
       sensors: Sequence[str] = defaults.CARLA_SENSORS,
       num_vehicles: int = 0,
       num_pedestrians: int = 0,
-      proximity_destination_threshold: float = 7.5) -> None:
+      proximity_destination_threshold: float = 7.5,
+      off_screen: bool = False) -> None:
     """Constructs a CARLA simulator-based OpenAI gym-compatible environment.
 
     Args:
@@ -159,6 +163,7 @@ def __init__(
         sensors=sensors,
         num_vehicles=num_vehicles,
         num_pedestrians=num_pedestrians,
+        off_screen=off_screen
     )
     # Internalize hyperparameters.
     self._proximity_destination_threshold = proximity_destination_threshold
diff --git a/oatomobile/simulators/carla/simulator.py b/oatomobile/simulators/carla/simulator.py
index 43dc813..a8c306f 100644
--- a/oatomobile/simulators/carla/simulator.py
+++ b/oatomobile/simulators/carla/simulator.py
@@ -20,6 +20,7 @@
 import queue
 import random
 import signal
+import sys
 import time
 from typing import Any
 from typing import Mapping
@@ -1603,6 +1604,30 @@ def default(
 class CARLASimulator(simulator.Simulator):
   """A thin CARLA simulator wrapper."""
 
+  # Restarting server causes traffic_manager to raise TimeoutException.
+  # This shares Carla server between all simulators and keep it running. Thus, the envs can be run only sequentially.
+  carla_server = None
+  server_port = None
+
+  @classmethod
+  def _start_carla_server(cls, town: str,
+                          off_screen: bool = False,
+                          server_timestop: float = 20.0):
+
+    # Check if the server is not yet started
+    if cls.carla_server is None:
+      cls.carla_server, cls.server_port = cutil.setup(town=town,
+                                                      off_screen=off_screen,
+                                                      server_timestop=server_timestop)
+
+  @classmethod
+  def stop_carla_server(cls):
+    logging.debug("Stopping CARLA server at port={}".format(cls.server_port))
+    os.killpg(cls.carla_server.pid, signal.SIGKILL)
+    atexit.unregister(lambda: os.killpg(cls.carla_server.pid, signal.SIGKILL))
+    cls.carla_server = None
+    cls.server_port = None
+
   def __init__(
       self,
       town: str,
@@ -1613,6 +1638,7 @@ def __init__(
       num_pedestrians: int = 0,
       fps: int = defaults.SIMULATOR_FPS,
       client_timeout: float = defaults.CARLA_CLIENT_TIMEOUT,
+      off_screen: bool = False
   ) -> None:
     """Constructs a CARLA simulator wrapper.
 
@@ -1632,12 +1658,14 @@ def __init__(
       fps: The frequency (in Hz) of the simulation.
       client_timeout: The time interval before stopping
         the search for the carla server.
+      off_screen: If true it starts Carla server in off-screen mode.
     """
     # Configuration variables.
     self._town = town
     self._sensors = sensors
     self._fps = fps
     self._client_timeout = client_timeout
+    self._off_screen = off_screen
     self._num_vehicles = num_vehicles
     self._num_pedestrians = num_pedestrians
 
@@ -1667,6 +1695,24 @@ def __init__(
     self._display = None
     self._clock = None
 
+    CARLASimulator._start_carla_server(town=self._town, off_screen=self._off_screen)
+
+    # Connect client.
+    logging.debug("Connects a CARLA client at port={}".format(CARLASimulator.server_port))
+    try:
+        self._client = carla.Client("localhost", CARLASimulator.server_port)  # pylint: disable=no-member
+        self._client.set_timeout(client_timeout)
+        self._traffic_manager = self._client.get_trafficmanager(8000)
+        self._traffic_manager.set_global_distance_to_leading_vehicle(1.0)
+        self._traffic_manager.set_synchronous_mode(True)
+        logging.debug("Server version: {}".format(self._client.get_server_version()))
+        logging.debug("Client version: {}".format(self._client.get_client_version()))
+    except RuntimeError as msg:
+      logging.debug(msg)
+      CARLASimulator.stop_carla_server()
+      logging.error("Failed to connect to CARLA server.")
+      sys.exit()
+
   @property
   def hero(self) -> carla.Vehicle:  # pylint: disable=no-member
     """Returns a reference to the ego car."""
@@ -1716,12 +1762,10 @@ def reset(self) -> simulator.Observations:
     Returns:
       The initial observations.
     """
-    # CARLA setup.
-    self._client, self._world, self._frame, self._server, self._traffic_manager = cutil.setup(
-        town=self._town,
-        fps=self._fps,
-        client_timeout=self._client_timeout,
-    )
+    self._world, self._frame = cutil.load_and_wait_for_world(self._client,
+                                                             self._town,
+                                                             self._fps,
+                                                             self._traffic_manager)
     self._frame0 = int(self._frame)
     self._dt = self._world.get_settings().fixed_delta_seconds
 
@@ -1844,10 +1888,10 @@ def close(self) -> None:
     if self.sensor_suite is not None:
       self.sensor_suite.close()
       self._sensor_suite = None
-    settings = self._world.get_settings()
-    settings.synchronous_mode = False
-    self._world.apply_settings(settings)
-    logging.debug("Closes the CARLA server with process PID {}".format(
-        self._server.pid))
-    os.killpg(self._server.pid, signal.SIGKILL)
-    atexit.unregister(lambda: os.killpg(self._server.pid, signal.SIGKILL))
+    if self._world is not None:
+      settings = self._world.get_settings()
+      settings.synchronous_mode = False
+      self._world.apply_settings(settings)
+      self._world = None
+    if self._client is not None:
+      self._client = None
diff --git a/oatomobile/utils/carla.py b/oatomobile/utils/carla.py
index 08a68c7..cb08578 100644
--- a/oatomobile/utils/carla.py
+++ b/oatomobile/utils/carla.py
@@ -41,97 +41,70 @@
 import carla
 
 
-def setup(
-    town: str,
-    fps: int = 20,
-    server_timestop: float = 20.0,
-    client_timeout: float = 20.0,
-    num_max_restarts: int = 5,
-) -> Tuple[carla.Client, carla.World, int, subprocess.Popen, carla.TrafficManager]:  # pylint: disable=no-member
-  """Returns the `CARLA` `server`, `client` and `world`.
+def load_and_wait_for_world(client: carla.Client,
+                            town: str,
+                            fps: int,
+                            traffic_manager: carla.TrafficManager
+                            ) -> Tuple[carla.World, int]:
+    """
+    Load a new CARLA world
+    """
 
-  Args:
-    town: The `CARLA` town identifier.
-    fps: The frequency (in Hz) of the simulation.
-    server_timestop: The time interval between spawing the server
-      and resuming program.
-    client_timeout: The time interval before stopping
-      the search for the carla server.
-    num_max_restarts: Number of attempts to connect to the server.
+    client.load_world(town)
+    world = client.get_world()
 
-  Returns:
-    client: The `CARLA` client.
-    world: The `CARLA` world.
-    frame: The synchronous simulation time step ID.
-    server: The `CARLA` server.
-  """
-  assert town in ("Town01", "Town02", "Town03", "Town04", "Town05")
+    settings = world.get_settings()
+    settings.synchronous_mode = True
+    settings.fixed_delta_seconds = 1.0 / fps
+    frame = world.apply_settings(settings)
+
+    traffic_manager.set_synchronous_mode(True)
+
+    world.tick()
+
+    return world, frame
 
-  # The attempts counter.
-  attempts = 0
 
-  while attempts < num_max_restarts:
-    logging.debug("{} out of {} attempts to setup the CARLA simulator".format(
-        attempts + 1, num_max_restarts))
+def setup(
+        town: str,
+        off_screen: bool = False,
+        server_timestop: float = 20.0
+) -> Tuple[subprocess.Popen, int]:  # pylint: disable=no-member
+    """Returns the `CARLA` `server`, `client` and `world`.
+
+    Args:
+      town: The `CARLA` town identifier.
+      server_timestop: The time interval between spawning the server
+        and resuming program.
+    Returns:
+      server: The `CARLA` server.
+    """
+    assert town in ("Town01", "Town02", "Town03", "Town04", "Town05")
 
     # Random assignment of port.
     port = np.random.randint(2000, 3000)
 
     # Start CARLA server.
     env = os.environ.copy()
-    # to enable off-screen uncomment the following line and use -opengl flag for running carla
-    # env["DISPLAY"] = ""
+    params = [
+        os.path.join(os.environ.get("CARLA_ROOT"), "CarlaUE4.sh"),
+        "-carla-rpc-port={}".format(port),
+        "-quality-level=Epic"
+    ]
+    if off_screen:
+        env["DISPLAY"] = ""
+        params.append("-opengl")
+
     logging.debug("Inits a CARLA server at port={}".format(port))
-    server = subprocess.Popen(
-        [
-            os.path.join(os.environ.get("CARLA_ROOT"), "CarlaUE4.sh"),
-            "-carla-rpc-port={}".format(port),
-            "-quality-level=Epic",
-            "-opengl"
-        ],
-        stdout=None,
-        stderr=subprocess.STDOUT,
-        preexec_fn=os.setsid,
-        env=env,
-    )
+    server = subprocess.Popen(params,
+                              stdout=None,
+                              stderr=subprocess.STDOUT,
+                              preexec_fn=os.setsid,
+                              env=env)
     atexit.register(os.killpg, server.pid, signal.SIGKILL)
     time.sleep(server_timestop)
 
-    # Connect client.
-    logging.debug("Connects a CARLA client at port={}".format(port))
-    try:
-      client = carla.Client("localhost", port)  # pylint: disable=no-member
-      client.set_timeout(client_timeout)
-      client.load_world(map_name=town)
-      world = client.get_world()
-
-      traffic_manager = client.get_trafficmanager(8000)
-
-      world.set_weather(carla.WeatherParameters.ClearNoon)  # pylint: disable=no-member
-      frame = world.apply_settings(
-          carla.WorldSettings(  # pylint: disable=no-member
-              no_rendering_mode=False,
-              synchronous_mode=True,
-              fixed_delta_seconds=1.0 / fps,
-          ))
-
-      traffic_manager.set_global_distance_to_leading_vehicle(1.0)
-      traffic_manager.set_synchronous_mode(True)
-
-      logging.debug("Server version: {}".format(client.get_server_version()))
-      logging.debug("Client version: {}".format(client.get_client_version()))
-      return client, world, frame, server, traffic_manager
-    except RuntimeError as msg:
-      logging.debug(msg)
-      attempts += 1
-      logging.debug("Stopping CARLA server at port={}".format(port))
-      os.killpg(server.pid, signal.SIGKILL)
-      atexit.unregister(lambda: os.killpg(server.pid, signal.SIGKILL))
-
-  logging.debug(
-      "Failed to connect to CARLA after {} attempts".format(num_max_restarts))
-  sys.exit()
-
+    return server, port
 
 def carla_rgb_image_to_ndarray(image: carla.Image) -> np.ndarray:  # pylint: disable=no-member
   """Returns a `NumPy` array from a `CARLA` RGB image.
@@ -621,16 +594,16 @@ def get_actors(
       vehicle_id="vehicle.ford.mustang",
   )
   # Other vehicles.
-  vehicles = spawn_vehicles(
-      world=world,
-      num_vehicles=num_vehicles,
-  )
+  # vehicles = spawn_vehicles(
+  #     world=world,
+  #     num_vehicles=num_vehicles,
+  # )
   # Other pedestrians.
-  pedestrians = spawn_pedestrians(
-      world=world,
-      num_pedestrians=num_pedestrians,
-  )
-  return hero, vehicles, pedestrians
+  # pedestrians = spawn_pedestrians(
+  #     world=world,
+  #     num_pedestrians=num_pedestrians,
+  # )
+  # return hero, vehicles, pedestrians
 
 
 def vehicle_to_carla_measurements(