From 9a9eb9b586ec06cc41e5362b55feb59e6c2f730e Mon Sep 17 00:00:00 2001
From: joel-mb <joel.moriana@gmail.com>
Date: Tue, 29 Jun 2021 15:43:25 +0200
Subject: [PATCH] New register all sensors parameter (#544)

Adds a new parameter (register_all_sensors) to avoid publishing data of all the sensors present in the simulation. When enabled, only sensors spawned by the bridge are registered. Otherwise, all the sensors present in the simulation are registered (current behavior).
---
 .../launch/carla_ros_bridge.launch            |  5 +-
 .../launch/carla_ros_bridge.launch.py         | 35 ++++++++++----
 .../src/carla_ros_bridge/actor_factory.py     | 47 ++++++++++---------
 .../src/carla_ros_bridge/bridge.py            |  1 +
 docs/run_ros.md                               |  4 ++
 5 files changed, 60 insertions(+), 32 deletions(-)

diff --git a/carla_ros_bridge/launch/carla_ros_bridge.launch b/carla_ros_bridge/launch/carla_ros_bridge.launch
index b4f47fab..e92eb0e1 100644
--- a/carla_ros_bridge/launch/carla_ros_bridge.launch
+++ b/carla_ros_bridge/launch/carla_ros_bridge.launch
@@ -17,6 +17,9 @@
   <!-- set the fixed timestep length -->
   <arg name='fixed_delta_seconds' default='0.05'/>
   <arg name='town' default='Town01'/>
+  <!-- enable/disable the registration of all sensors. If disabled, only sensors
+  spawned by the bridge are registered -->
+  <arg name='register_all_sensors' default='True'/>
   <!-- 
     the role name of the vehicles that acts as ego vehicle for this ros bridge instance
     Only the vehicles within this list are controllable from within ROS.
@@ -32,8 +35,8 @@
     <param name="synchronous_mode" value="$(arg synchronous_mode)"/>
     <param name="synchronous_mode_wait_for_vehicle_control_command" value="$(arg synchronous_mode_wait_for_vehicle_control_command)"/>
     <param name="fixed_delta_seconds" value="$(arg fixed_delta_seconds)"/>
+    <param name="register_all_sensors" value="$(arg register_all_sensors)"/>
     <param name="town" value="$(arg town)"/>
     <param name="ego_vehicle_role_name" value="$(arg ego_vehicle_role_name)"/>
   </node>
-    
 </launch>
diff --git a/carla_ros_bridge/launch/carla_ros_bridge.launch.py b/carla_ros_bridge/launch/carla_ros_bridge.launch.py
index e1d4627a..0492bd86 100644
--- a/carla_ros_bridge/launch/carla_ros_bridge.launch.py
+++ b/carla_ros_bridge/launch/carla_ros_bridge.launch.py
@@ -6,40 +6,54 @@ def generate_launch_description():
     ld = launch.LaunchDescription([
         launch.actions.DeclareLaunchArgument(
             name='host',
-            default_value='localhost'
+            default_value='localhost',
+            description='IP of the CARLA server'
         ),
         launch.actions.DeclareLaunchArgument(
             name='port',
-            default_value='2000'
+            default_value='2000',
+            description='TCP port of the CARLA server'
         ),
         launch.actions.DeclareLaunchArgument(
             name='timeout',
-            default_value='2'
+            default_value='2',
+            description='Time to wait for a successful connection to the CARLA server'
         ),
         launch.actions.DeclareLaunchArgument(
             name='passive',
-            default_value='False'
+            default_value='False',
+            description='When enabled, the ROS bridge will take a backseat and another client must tick the world (only in synchronous mode)'
         ),
         launch.actions.DeclareLaunchArgument(
             name='synchronous_mode',
-            default_value='True'
+            default_value='True',
+            description='Enable/disable synchronous mode. If enabled, the ROS bridge waits until the expected data is received for all sensors'
         ),
         launch.actions.DeclareLaunchArgument(
             name='synchronous_mode_wait_for_vehicle_control_command',
-            default_value='False'
+            default_value='False',
+            description='When enabled, pauses the tick until a vehicle control is completed (only in synchronous mode)'
         ),
         launch.actions.DeclareLaunchArgument(
             name='fixed_delta_seconds',
-            default_value='0.05'
+            default_value='0.05',
+            description='Simulation time (delta seconds) between simulation steps'
         ),
         launch.actions.DeclareLaunchArgument(
             name='town',
-            default_value='Town01'
+            default_value='Town01',
+            description='Either use an available CARLA town (eg. "Town01") or an OpenDRIVE file (ending in .xodr)'
+        ),
+        launch.actions.DeclareLaunchArgument(
+            name='register_all_sensors',
+            default_value='True',
+            description='Enable/disable the registration of all sensors. If disabled, only sensors spawned by the bridge are registered'
         ),
         launch.actions.DeclareLaunchArgument(
             name='ego_vehicle_role_name',
             default_value=["hero", "ego_vehicle", "hero0", "hero1", "hero2",
-                           "hero3", "hero4", "hero5", "hero6", "hero7", "hero8", "hero9"]
+                           "hero3", "hero4", "hero5", "hero6", "hero7", "hero8", "hero9"],
+            description='Role names to identify ego vehicles. '
         ),
         launch_ros.actions.Node(
             package='carla_ros_bridge',
@@ -76,6 +90,9 @@ def generate_launch_description():
                 {
                     'town': launch.substitutions.LaunchConfiguration('town')
                 },
+                {
+                    'register_all_sensors': launch.substitutions.LaunchConfiguration('register_all_sensors')
+                },
                 {
                     'ego_vehicle_role_name': launch.substitutions.LaunchConfiguration('ego_vehicle_role_name')
                 }
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 fa0aed0b..e3b0667e 100755
--- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py
+++ b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py
@@ -56,9 +56,9 @@ class ActorFactory(object):
     TIME_BETWEEN_UPDATES = 0.1
 
     class TaskType(Enum):
-        SPAWN_PSEUDO_ACTOR = 0
-        DESTROY_ACTOR = 1
-        SYNC = 2
+        SPAWN_ACTOR = 0
+        SPAWN_PSEUDO_ACTOR = 1
+        DESTROY_ACTOR = 2
 
     def __init__(self, node, world, sync_mode=False):
         self.node = node
@@ -111,7 +111,8 @@ def update_available_objects(self):
         self.lock.acquire()
         for actor_id in new_actors:
             carla_actor = self.world.get_actor(actor_id)
-            self._create_object_from_actor(carla_actor)
+            if self.node.parameters["register_all_sensors"] or not isinstance(carla_actor, carla.Sensor):
+                self._create_object_from_actor(carla_actor)
 
         for actor_id in deleted_actors:
             self._destroy_object(actor_id, delete_actor=False)
@@ -120,15 +121,16 @@ def update_available_objects(self):
         with self.spawn_lock:
             while not self._task_queue.empty():
                 task = self._task_queue.get()
-                if task[0] == ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR and not self.node.shutdown.is_set():
+                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)
+                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,
                                         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)
-                elif task[0] == ActorFactory.TaskType.SYNC and not self.node.shutdown.is_set():
-                    break
         self.lock.release()
 
     def update_actor_states(self, frame_id, timestamp):
@@ -167,28 +169,29 @@ def spawn_actor(self, req):
                 self._task_queue.put((ActorFactory.TaskType.SPAWN_PSEUDO_ACTOR, (id_, req)))
             else:
                 id_ = self._spawn_carla_actor(req)
-                self._task_queue.put((ActorFactory.TaskType.SYNC, None))
+                self._task_queue.put((ActorFactory.TaskType.SPAWN_ACTOR, (id_, None)))
             self._known_actor_ids.append(id_)
         return id_
 
     def destroy_actor(self, uid):
-        with self.spawn_lock:
-            objects_to_destroy = set(self._destroy_actor(uid))
-            for obj in objects_to_destroy:
-                self._task_queue.put((ActorFactory.TaskType.DESTROY_ACTOR, obj))
-        return objects_to_destroy
 
-    def _destroy_actor(self, uid):
-        objects_to_destroy = []
-        if uid in self._known_actor_ids:
-            objects_to_destroy.append(uid)
-            self._known_actor_ids.remove(uid)
+        def get_objects_to_destroy(uid):
+            objects_to_destroy = []
+            if uid in self._known_actor_ids:
+                objects_to_destroy.append(uid)
+                self._known_actor_ids.remove(uid)
 
-        # remove actors that have the actor to be removed as parent.
-        for actor in list(self.actors.values()):
-            if actor.parent is not None and actor.parent.uid == uid:
-                objects_to_destroy.extend(self._destroy_actor(actor.uid))
+            # remove actors that have the actor to be removed as parent.
+            for actor in list(self.actors.values()):
+                if actor.parent is not None and actor.parent.uid == uid:
+                    objects_to_destroy.extend(get_objects_to_destroy(actor.uid))
 
+            return objects_to_destroy
+
+        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))
         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 49111dfb..da618b4f 100755
--- a/carla_ros_bridge/src/carla_ros_bridge/bridge.py
+++ b/carla_ros_bridge/src/carla_ros_bridge/bridge.py
@@ -394,6 +394,7 @@ def main(args=None):
         'synchronous_mode_wait_for_vehicle_control_command', False)
     parameters['fixed_delta_seconds'] = carla_bridge.get_param('fixed_delta_seconds',
                                                                0.05)
+    parameters['register_all_sensors'] = carla_bridge.get_param('register_all_sensors', True)
     parameters['town'] = carla_bridge.get_param('town', 'Town01')
     role_name = carla_bridge.get_param('ego_vehicle_role_name',
                                        ["hero", "ego_vehicle", "hero1", "hero2", "hero3"])
diff --git a/docs/run_ros.md b/docs/run_ros.md
index 6a9b11fb..cc9c5934 100644
--- a/docs/run_ros.md
+++ b/docs/run_ros.md
@@ -77,6 +77,10 @@ The following settings are available:
 *  __fixed_delta_seconds__: Simulation time (delta seconds) between simulation steps. __It must be lower than 0.1__. Take a look at the [documentation](https://carla.readthedocs.io/en/latest/adv_synchrony_timestep/) to learn more about this.
 *  __ego_vehicle__: Role names to identify ego vehicles. Relevant topics will be created so these vehicles will be able to be controlled from ROS.
 * __town__: Either use an available CARLA town (eg. 'town01') or an OpenDRIVE file (ending in `.xodr`).
+*  __register_all_sensors__:
+	*  __If false__: Only sensors spawned by the bridge are registered.
+	*  __If true (default)__: All the sensors present in the simulation are registered.
+
 
 [ros_clock]: https://wiki.ros.org/Clock