From 2ab679c5ee7a1e1f7e75897aac6ccf05f2493913 Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Wed, 22 Sep 2021 17:06:41 +0200 Subject: [PATCH 1/4] Sync carla actor creation Wait one tick before creating carla actors to ensure the transforms of the newly spawned actors have been updated by the server --- .../src/carla_ros_bridge/actor_factory.py | 20 +++++++++++++------ .../src/carla_ros_bridge/bridge.py | 2 +- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py index e3b0667e..ae2842e0 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py +++ b/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,11 +119,17 @@ 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]) - self._create_object_from_actor(carla_actor) + 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)) + else: + 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] self._create_object(pseudo_object[0], pseudo_object[1].type, pseudo_object[1].id, @@ -131,6 +137,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,6 +166,7 @@ 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: @@ -166,10 +174,10 @@ def spawn_actor(self, req): 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 +199,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): diff --git a/carla_ros_bridge/src/carla_ros_bridge/bridge.py b/carla_ros_bridge/src/carla_ros_bridge/bridge.py index da618b4f..abb36718 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/bridge.py +++ b/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 From 1fbeea8ab9b9c7055fbb357f58ee520a76ee40a3 Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Thu, 23 Sep 2021 09:33:59 +0200 Subject: [PATCH 2/4] Also postpone pseudo actors --- .../src/carla_ros_bridge/actor_factory.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py index ae2842e0..f0cb578f 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py +++ b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py @@ -122,14 +122,13 @@ def update_available_objects(self, timestamp = None): 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(): - 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)) - else: - carla_actor = self.world.get_actor(actor_id) - self._create_object_from_actor(carla_actor) + 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] self._create_object(pseudo_object[0], pseudo_object[1].type, pseudo_object[1].id, From a2e6c66a1feb4f1b600702ee0a3b7d24400e1e51 Mon Sep 17 00:00:00 2001 From: Joel Moriana Date: Thu, 23 Sep 2021 14:58:03 +0200 Subject: [PATCH 3/4] fix destroy actor --- carla_ros_bridge/src/carla_ros_bridge/actor_factory.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py index f0cb578f..8f86992f 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py +++ b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py @@ -134,7 +134,6 @@ def update_available_objects(self, timestamp = None): self._create_object(pseudo_object[0], pseudo_object[1].type, pseudo_object[1].id, pseudo_object[1].attach_to, pseudo_object[1].transform) 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() @@ -198,7 +197,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)) return objects_to_destroy def _spawn_carla_actor(self, req): From 32f0fdef2e1fd1a36f28905ad68241d62c1ca55b Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Thu, 23 Sep 2021 16:14:37 +0200 Subject: [PATCH 4/4] Make code robust and fix task_queue assignment --- .../src/carla_ros_bridge/actor_factory.py | 27 ++++++++++++------- .../src/carla_ros_bridge/bridge.py | 2 +- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py index 8f86992f..a5a2734a 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py +++ b/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, timestamp = None): + def update_available_objects(self, frame = None): """ update the available actors """ @@ -119,14 +120,18 @@ def update_available_objects(self, timestamp = None): # update objects for pseudo actors here as they might have an carla actor as parent ###### 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() actor_id = task[1][0] - if timestamp and task[2] and task[2].frame >= timestamp.frame: + desired_task_frame = task[2] + if frame and desired_task_frame and desired_task_frame > frame: task_queue.put(task) - self.node.loginfo("Delaying task on actor {} to next tick".format(actor_id)) + self.node.logdebug("Frame {}: Delaying task triggered at {} on actor {} to next tick".format(frame, desired_task_frame, actor_id)) elif task[0] == 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) elif task[0] == ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR and not self.node.shutdown.is_set(): @@ -135,7 +140,7 @@ def update_available_objects(self, timestamp = None): pseudo_object[1].attach_to, pseudo_object[1].transform) elif task[0] == ActorFactory.TaskType.DESTROY_ACTOR: self._destroy_object(actor_id, delete_actor=True) - self._task_queue = task_queue + self._task_queue = task_queue self.lock.release() def update_actor_states(self, frame_id, timestamp): @@ -164,7 +169,11 @@ 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 + # 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: @@ -172,10 +181,10 @@ def spawn_actor(self, req): 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), timestamp)) + 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_, None), timestamp)) + self._task_queue.put((ActorFactory.TaskType.SPAWN_ACTOR, (id_, None), desired_task_frame)) self._known_actor_ids.append(id_) return id_ diff --git a/carla_ros_bridge/src/carla_ros_bridge/bridge.py b/carla_ros_bridge/src/carla_ros_bridge/bridge.py index abb36718..6a5433a4 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/bridge.py +++ b/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(world_snapshot.timestamp) + self.actor_factory.update_available_objects(frame) if self.parameters['synchronous_mode_wait_for_vehicle_control_command']: # wait for all ego vehicles to send a vehicle control command