Reworked spawning of actors to speed up scenario initialization

The BasicScenario actor initialization now uses batch mode
initialization.

CarlaActorPool:
- Added _blueprint_library member to buffer the CARLA blueprint library
and avoid world access for every blueprint
- Added create_blueprint() to eliminate code dublication
- Added handle_actor_batch() to eliminate code dublication
- Added setup_actors() new function to spawn actors in batch mode
- Added request_new_actors() new public function to use new batch mode
(used by BasicScenario class)

Change-Id: Ia0ce57841ee100a1f661171af9824a6e3e8482f8

Docs/CHANGELOG.md

@@ -75,6 +75,7 @@
 * Fixed spawning of debris for ControlLoss scenario (Scenario01)
 * Fixed CTRL+C termination of ScenarioRunner
 ### :ghost: Maintenance
+* Increased speed of actor initialization by using CARLA batch mode and buffering CARLA blueprint library
 * Split of behaviors into behaviors and conditions
 * Moved atomics into new submodule scenarioatomics
 * Updated documentation for all behaviors, conditions and test criteria

srunner/scenariomanager/carla_data_provider.py

@@ -371,6 +371,7 @@ class CarlaActorPool(object):
     _carla_actor_pool = dict()
     _spawn_points = None
     _spawn_index = 0
+    _blueprint_library = None
 
     @staticmethod
     def set_client(client):
@@ -385,6 +386,7 @@ def set_world(world):
         Set the CARLA world
         """
         CarlaActorPool._world = world
+        CarlaActorPool._blueprint_library = world.get_blueprint_library()
         CarlaActorPool.generate_spawn_points()
 
     @staticmethod
@@ -407,8 +409,7 @@ def generate_spawn_points():
         CarlaActorPool._spawn_index = 0
 
     @staticmethod
-    def setup_actor(model, spawn_point, rolename='scenario', hero=False, autopilot=False,
-                    random_location=False, color=None, actor_category="car"):
+    def create_blueprint(model, rolename='scenario', hero=False, autopilot=False, color=None, actor_category="car"):
         """
         Function to setup the most relevant actor parameters,
         incl. spawn point and vehicle model.
@@ -428,19 +429,17 @@ def setup_actor(model, spawn_point, rolename='scenario', hero=False, autopilot=F
             'pedestrian': 'walker.pedestrian.0001',
         }
 
-        blueprint_library = CarlaActorPool._world.get_blueprint_library()
-
         # Get vehicle by model
         try:
-            blueprint = random.choice(blueprint_library.filter(model))
+            blueprint = random.choice(CarlaActorPool._blueprint_library.filter(model))
         except IndexError:
             # The model is not part of the blueprint library. Let's take a default one for the given category
             bp_filter = "vehicle.*"
             new_model = _actor_blueprint_categories[actor_category]
             if new_model != '':
                 bp_filter = new_model
             print("WARNING: Actor model {} not available. Using instead {}".format(model, new_model))
-            blueprint = random.choice(blueprint_library.filter(bp_filter))
+            blueprint = random.choice(CarlaActorPool._blueprint_library.filter(bp_filter))
 
         if color:
             if not blueprint.has_attribute('color'):
@@ -467,6 +466,53 @@ def setup_actor(model, spawn_point, rolename='scenario', hero=False, autopilot=F
         else:
             blueprint.set_attribute('role_name', rolename)
 
+        return blueprint
+
+    @staticmethod
+    def handle_actor_batch(batch):
+        """
+        Forward a CARLA command batch to spawn actors to CARLA, and gather the responses
+
+        returns list of actors on success, none otherwise
+        """
+
+        actors = []
+
+        if CarlaActorPool._client and batch is not None:
+            responses = CarlaActorPool._client.apply_batch_sync(batch)
+        else:
+            return None
+
+        # wait for the actors to be spawned properly before we do anything
+        if CarlaActorPool._world.get_settings().synchronous_mode:
+            CarlaActorPool._world.tick()
+        else:
+            CarlaActorPool._world.wait_for_tick()
+
+        actor_ids = []
+        if responses:
+            for response in responses:
+                if not response.error:
+                    actor_ids.append(response.actor_id)
+                else:
+                    print("WARNING: Could not spawn an actor")
+
+        carla_actors = CarlaActorPool._world.get_actors(actor_ids)
+        for actor in carla_actors:
+            actors.append(actor)
+
+        return actors
+
+    @staticmethod
+    def setup_actor(model, spawn_point, rolename='scenario', hero=False, autopilot=False,
+                    random_location=False, color=None, actor_category="car"):
+        """
+        Function to setup the most relevant actor parameters,
+        incl. spawn point and vehicle model.
+        """
+
+        blueprint = CarlaActorPool.create_blueprint(model, rolename, hero, autopilot, color, actor_category)
+
         if random_location:
             actor = None
             while not actor:
@@ -487,7 +533,7 @@ def setup_actor(model, spawn_point, rolename='scenario', hero=False, autopilot=F
                 "Error: Unable to spawn vehicle {} at {}".format(blueprint.id, spawn_point))
         else:
             # Let's deactivate the autopilot of the actor if it belongs to vehicle
-            if actor in blueprint_library.filter('vehicle.*'):
+            if actor in CarlaActorPool._blueprint_library.filter('vehicle.*'):
                 actor.set_autopilot(autopilot)
             else:
                 pass
@@ -499,6 +545,34 @@ def setup_actor(model, spawn_point, rolename='scenario', hero=False, autopilot=F
 
         return actor
 
+    @staticmethod
+    def setup_actors(actor_list):
+        """
+        Function to setup a complete list of actors
+        """
+
+        SpawnActor = carla.command.SpawnActor       # pylint: disable=invalid-name
+        batch = []
+        actors = []
+        for actor in actor_list:
+            blueprint = CarlaActorPool.create_blueprint(model=actor.model,
+                                                        rolename=actor.rolename,
+                                                        hero=False,
+                                                        autopilot=actor.autopilot,
+                                                        color=actor.color,
+                                                        actor_category=actor.category)
+            # slightly lift the actor to avoid collisions with ground when spawning the actor
+            # DO NOT USE spawn_point directly, as this will modify spawn_point permanently
+            _spawn_point = carla.Transform(carla.Location(), actor.transform.rotation)
+            _spawn_point.location.x = actor.transform.location.x
+            _spawn_point.location.y = actor.transform.location.y
+            _spawn_point.location.z = actor.transform.location.z + 0.2
+            batch.append(SpawnActor(blueprint, _spawn_point))
+
+        actors = CarlaActorPool.handle_actor_batch(batch)
+
+        return actors
+
     @staticmethod
     def setup_batch_actors(model, amount, spawn_point, hero=False, autopilot=False, random_location=False):
         """
@@ -547,25 +621,7 @@ def setup_batch_actors(model, amount, spawn_point, hero=False, autopilot=False,
             if spawn_point:
                 batch.append(SpawnActor(blueprint, spawn_point).then(SetAutopilot(FutureActor, autopilot)))
 
-        if CarlaActorPool._client:
-            responses = CarlaActorPool._client.apply_batch_sync(batch)
-
-        # wait for the actors to be spawned properly before we do anything
-        if CarlaActorPool._world.get_settings().synchronous_mode:
-            CarlaActorPool._world.tick()
-        else:
-            CarlaActorPool._world.wait_for_tick()
-
-        actor_list = []
-        actor_ids = []
-        if responses:
-            for response in responses:
-                if not response.error:
-                    actor_ids.append(response.actor_id)
-
-        carla_actors = CarlaActorPool._world.get_actors(actor_ids)
-        for actor in carla_actors:
-            actor_list.append(actor)
+        actor_list = CarlaActorPool.handle_actor_batch(batch)
 
         return actor_list
 
@@ -598,8 +654,25 @@ def request_new_actor(model, spawn_point, rolename='scenario', hero=False, autop
             return None
 
         CarlaActorPool._carla_actor_pool[actor.id] = actor
+        actor.set_simulate_physics(True)
         return actor
 
+    @staticmethod
+    def request_new_actors(actor_list):
+        """
+        This method tries to create a list of new actors. If this was
+        successful, the new actors are returned, None otherwise.
+        """
+        actors = CarlaActorPool.setup_actors(actor_list)
+
+        if actors is None:
+            return None
+
+        for actor in actors:
+            CarlaActorPool._carla_actor_pool[actor.id] = actor
+            actor.set_simulate_physics(True)
+        return actors
+
     @staticmethod
     def actor_id_exists(actor_id):
         """

srunner/scenarios/basic_scenario.py

@@ -78,18 +78,12 @@ def _initialize_actors(self, config):
         Default initialization of other actors.
         Override this method in child class to provide custom initialization.
         """
-        for actor in config.other_actors:
-            new_actor = CarlaActorPool.request_new_actor(actor.model,
-                                                         actor.transform,
-                                                         rolename=actor.rolename,
-                                                         hero=False,
-                                                         autopilot=actor.autopilot,
-                                                         random_location=actor.random_location,
-                                                         color=actor.color,
-                                                         actor_category=actor.category)
-            if new_actor is None:
-                raise Exception("Error: Unable to add actor {} at {}".format(actor.model, actor.transform))
 
+        new_actors = CarlaActorPool.request_new_actors(config.other_actors)
+        if new_actors is None:
+            raise Exception("Error: Unable to add actors")
+
+        for new_actor in new_actors:
             self.other_actors.append(new_actor)
 
     def _setup_scenario_trigger(self, config):