Sync carla actor creation

carla_ros_bridge/src/carla_ros_bridge/actor_factory.py

@@ -95,7 +95,7 @@ def _update_thread(self):
             self.world.wait_for_tick()
             self.update_available_objects()
 
-    def update_available_objects(self):
+    def update_available_objects(self, timestamp = None):
         """
         update the available actors
         """
@@ -119,10 +119,15 @@ def update_available_objects(self):
 
         # update objects for pseudo actors here as they might have an carla actor as parent ######
         with self.spawn_lock:
+            task_queue = queue.Queue()
             while not self._task_queue.empty():
                 task = self._task_queue.get()
-                if task[0] == ActorFactory.TaskType.SPAWN_ACTOR and not self.node.shutdown.is_set():
-                    carla_actor = self.world.get_actor(task[1][0])
+                actor_id = task[1][0]
+                if timestamp and task[2] and task[2].frame >= timestamp.frame:
+                    task_queue.put(task)
+                    self.node.loginfo("Delaying task on actor {} to next tick".format(actor_id))
+                elif task[0] == ActorFactory.TaskType.SPAWN_ACTOR and not self.node.shutdown.is_set():
+                    carla_actor = self.world.get_actor(actor_id)
                     self._create_object_from_actor(carla_actor)
                 elif task[0] == ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR and not self.node.shutdown.is_set():
                     pseudo_object = task[1]
@@ -131,6 +136,7 @@ def update_available_objects(self):
                 elif task[0] == ActorFactory.TaskType.DESTROY_ACTOR:
                     actor_id = task[1]
                     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 +165,18 @@ def spawn_actor(self, req):
         and pseudo objects are appended to a list to get created later.
         """
         with self.spawn_lock:
+            timestamp = self.world.get_snapshot().timestamp
             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), timestamp))
             else:
                 id_ = self._spawn_carla_actor(req)
-                self._task_queue.put((ActorFactory.TaskType.SPAWN_ACTOR, (id_, None)))
+                self._task_queue.put((ActorFactory.TaskType.SPAWN_ACTOR, (id_, None), timestamp))
             self._known_actor_ids.append(id_)
         return id_
 
@@ -191,7 +198,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))
+                self._task_queue.put((ActorFactory.TaskType.DESTROY_ACTOR, obj, None))
         return objects_to_destroy
 
     def _spawn_carla_actor(self, req):

carla_ros_bridge/src/carla_ros_bridge/bridge.py

@@ -270,7 +270,7 @@ def _synchronous_mode_update(self):
                 frame))
             self._update(frame, world_snapshot.timestamp.elapsed_seconds)
             self.logdebug("Waiting for sensor data finished.")
-            self.actor_factory.update_available_objects()
+            self.actor_factory.update_available_objects(world_snapshot.timestamp)
 
             if self.parameters['synchronous_mode_wait_for_vehicle_control_command']:
                 # wait for all ego vehicles to send a vehicle control command