Sync carla actor creation

carla_ros_bridge/src/carla_ros_bridge/actor_factory.py

@@ -71,6 +71,7 @@ def __init__(self, node, world, sync_mode=False):
         self.actors = {}
 
         self._task_queue = queue.Queue()
+        self._last_update_frame = 0
         self._known_actor_ids = []  # used to immediately reply to spawn_actor/destroy_actor calls
 
         self.lock = Lock()
@@ -92,10 +93,10 @@ def _update_thread(self):
         """
         while not self.node.shutdown.is_set():
             time.sleep(ActorFactory.TIME_BETWEEN_UPDATES)
-            self.world.wait_for_tick()
-            self.update_available_objects()
+            world_snapshot = self.world.wait_for_tick()
+            self.update_available_objects(world_snapshot.timestamp.frame)
 
-    def update_available_objects(self):
+    def update_available_objects(self, frame = None):
         """
         update the available actors
         """
@@ -118,19 +119,27 @@ def update_available_objects(self):
 
         # Create/destroy objects managed by the bridge.
         with self.spawn_lock:
+            if frame:
+                self._last_update_frame = frame
+            task_queue = queue.Queue()
             while not self._task_queue.empty():
                 task = self._task_queue.get()
                 task_type = task[0]
                 actor_id, req = task[1]
+                desired_task_frame = task[2]
 
-                if task_type == ActorFactory.TaskType.SPAWN_ACTOR and not self.node.shutdown.is_set():
+                if frame and desired_task_frame and desired_task_frame > frame:
+                    task_queue.put(task)
+                    self.node.logdebug("Frame {}: Delaying task triggered at {} on actor {} to next tick".format(frame, desired_task_frame, actor_id))
+                elif task_type == ActorFactory.TaskType.SPAWN_ACTOR and not self.node.shutdown.is_set():
+                    self.node.logdebug("Frame {}: Execute task triggered at {} on actor {}".format(frame, desired_task_frame, actor_id))
                     carla_actor = self.world.get_actor(actor_id)
                     self._create_object_from_actor(carla_actor, req)
                 elif task_type == ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR and not self.node.shutdown.is_set():
                     self._create_object(actor_id, req.type, req.id, req.attach_to, req.transform)
                 elif task_type == ActorFactory.TaskType.DESTROY_ACTOR:
                     self._destroy_object(actor_id, delete_actor=True)
-
+            self._task_queue = task_queue
         self.lock.release()
 
     def update_actor_states(self, frame_id, timestamp):
@@ -159,17 +168,22 @@ def spawn_actor(self, req):
         and pseudo objects are appended to a list to get created later.
         """
         with self.spawn_lock:
+            # Since the next frame might already being calculated at the moment,
+            # we have to ensure the task is executed earliest in two frames to allow
+            # CARLA to see the actor at least once, update the actor states and send
+            # them to the brigde client
+            desired_task_frame = self._last_update_frame+2
             if "pseudo" in req.type:
                 # only allow spawning pseudo objects if parent actor already exists in carla
                 if req.attach_to != 0:
                     carla_actor = self.world.get_actor(req.attach_to)
                     if carla_actor is None:
                         raise IndexError("Parent actor {} not found".format(req.attach_to))
                 id_ = next(self.id_gen)
-                self._task_queue.put((ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR, (id_, req)))
+                self._task_queue.put((ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR, (id_, req), desired_task_frame))
             else:
                 id_ = self._spawn_carla_actor(req)
-                self._task_queue.put((ActorFactory.TaskType.SPAWN_ACTOR, (id_, req)))
+                self._task_queue.put((ActorFactory.TaskType.SPAWN_ACTOR, (id_, req), desired_task_frame))
             self._known_actor_ids.append(id_)
         return id_
 
@@ -191,7 +205,7 @@ def get_objects_to_destroy(uid):
         with self.spawn_lock:
             objects_to_destroy = set(get_objects_to_destroy(uid))
             for obj in objects_to_destroy:
-                self._task_queue.put((ActorFactory.TaskType.DESTROY_ACTOR, (obj, None)))
+                self._task_queue.put((ActorFactory.TaskType.DESTROY_ACTOR, (obj, None), None))
         return objects_to_destroy
 
     def _spawn_carla_actor(self, req):

carla_ros_bridge/src/carla_ros_bridge/bridge.py

@@ -262,7 +262,7 @@ def _synchronous_mode_update(self):
                             self._expected_ego_vehicle_control_command_ids.append(
                                 actor_id)
 
-            self.actor_factory.update_available_objects()
+            self.actor_factory.update_available_objects(self.status_publisher.frame)
             frame = self.carla_world.tick()
 
             world_snapshot = self.carla_world.get_snapshot()