Adapt carla_spawn_objects to native ROS2 interface

carla_spawn_objects/launch/carla_example_ego_vehicle.launch

@@ -9,8 +9,6 @@
   <arg name="spawn_point_ego_vehicle" default=''/>
   <arg name="spawn_sensors_only" default="false"/>
 
-  <arg name="control_id" default="control"/>
-
   <include file="$(find carla_spawn_objects)/launch/carla_spawn_objects.launch">
     <arg name="objects_definition_file" value='$(arg objects_definition_file)'/>
     <arg name="spawn_point_ego_vehicle" value="$(arg spawn_point_ego_vehicle)"/>
@@ -20,6 +18,5 @@
   <!-- This nodes allows to respawn the vehicle <role_name> by publishing on topic /initialpose -->
   <include file="$(find carla_spawn_objects)/launch/set_initial_pose.launch">
     <arg name="role_name" value="$(arg role_name)"/>
-    <arg name="control_id" value="$(arg control_id)"/>
   </include>
 </launch>

carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py

@@ -23,10 +23,6 @@ def generate_launch_description():
             name='spawn_sensors_only',
             default_value='False'
         ),
-        launch.actions.DeclareLaunchArgument(
-            name='control_id',
-            default_value='control'
-        ),
         launch.actions.IncludeLaunchDescription(
             launch.launch_description_sources.PythonLaunchDescriptionSource(
                 os.path.join(get_package_share_directory(
@@ -44,8 +40,7 @@ def generate_launch_description():
                     'carla_spawn_objects'), 'set_initial_pose.launch.py')
             ),
             launch_arguments={
-                'role_name': launch.substitutions.LaunchConfiguration('role_name'),
-                'control_id': launch.substitutions.LaunchConfiguration('control_id')
+                'role_name': launch.substitutions.LaunchConfiguration('role_name')
             }.items()
         )
     ])

carla_spawn_objects/launch/set_initial_pose.launch

@@ -1,11 +1,8 @@
 <!-- -->
 <launch>
   <arg name='role_name' default='ego_vehicle'/>
-  <!-- id of the actor.pseudo.control actor-->
-  <arg name='control_id' default='control'/>
 
   <node pkg="carla_spawn_objects" type="set_initial_pose.py" name="set_initial_pose_$(arg role_name)" output="screen">
     <param name="role_name" value="$(arg role_name)" />
-    <param name="control_id" value="$(arg control_id)" />
   </node>
 </launch>

carla_spawn_objects/launch/set_initial_pose.launch.py

@@ -13,8 +13,8 @@ def generate_launch_description():
             default_value='ego_vehicle'
         ),
         launch.actions.DeclareLaunchArgument(
-            name='control_id',
-            default_value='control'
+            name='objects_definition_file',
+            default_value=''
         ),
         launch_ros.actions.Node(
             package='carla_spawn_objects',
@@ -24,10 +24,10 @@ def generate_launch_description():
             emulate_tty=True,
             parameters=[
                 {
-                    'role_name': launch.substitutions.LaunchConfiguration('role_name')
+                    'objects_definition_file': launch.substitutions.LaunchConfiguration('objects_definition_file')
                 },
                 {
-                    'control_id': launch.substitutions.LaunchConfiguration('control_id')
+                    'role_name': launch.substitutions.LaunchConfiguration('role_name')
                 }
             ]
         )

carla_spawn_objects/src/carla_spawn_objects/carla_spawn_objects.py

@@ -53,19 +53,24 @@ def __init__(self):
         self.vehicles_sensors = []
         self.global_sensors = []
 
-        self.spawn_object_service = self.new_client(SpawnObject, "/carla/spawn_object")
-        self.destroy_object_service = self.new_client(DestroyObject, "/carla/destroy_object")
+        self.spawn_object_service = self.new_client(SpawnObject, "/carla/world/spawn_object")
+        self.destroy_object_service = self.new_client(DestroyObject, "/carla/world/destroy_object")
 
     def spawn_object(self, spawn_object_request):
         response_id = -1
         response = self.call_service(self.spawn_object_service, spawn_object_request, spin_until_response_received=True)
         response_id = response.id
+        role_name = ""
+        for attribute in spawn_object_request.blueprint.attributes:
+            if attribute.key == "role_name":
+                role_name = attribute.value
+                break
         if response_id != -1:
-            self.loginfo("Object (type='{}', id='{}') spawned successfully as {}.".format(
-                spawn_object_request.type, spawn_object_request.id, response_id))
+            self.loginfo("Object (id='{}', role_name='{}') spawned successfully as {}.".format(
+                spawn_object_request.blueprint.id, role_name, response_id))
         else:
-            self.logwarn("Error while spawning object (type='{}', id='{}').".format(
-                spawn_object_request.type, spawn_object_request.id))
+            self.logwarn("Error while spawning object (id='{}', role_name='{}').".format(
+                spawn_object_request.blueprint.id, role_name))
             raise RuntimeError(response.error_string)
         return response_id
 
@@ -86,29 +91,26 @@ def spawn_objects(self):
 
         global_sensors = []
         vehicles = []
-        found_sensor_actor_list = False
 
         for actor in json_actors["objects"]:
-            actor_type = actor["type"].split('.')[0]
-            if actor["type"] == "sensor.pseudo.actor_list" and self.spawn_sensors_only:
-                global_sensors.append(actor)
-                found_sensor_actor_list = True
+            actor_type_split = actor["type"].split('.')
+            actor_type = actor_type_split[0]
+            if actor_type_split[1] == "pseudo":
+                self.logwarn(
+                    "Object with type {} is not a valid sensor anymore, ignoring".format(actor["type"]))
             elif actor_type == "sensor":
                 global_sensors.append(actor)
             elif actor_type == "vehicle" or actor_type == "walker":
                 vehicles.append(actor)
             else:
                 self.logwarn(
                     "Object with type {} is not a vehicle, a walker or a sensor, ignoring".format(actor["type"]))
-        if self.spawn_sensors_only is True and found_sensor_actor_list is False:
-            raise RuntimeError("Parameter 'spawn_sensors_only' enabled, " +
-                               "but 'sensor.pseudo.actor_list' is not instantiated, add it to your config file.")
 
         self.setup_sensors(global_sensors)
 
         if self.spawn_sensors_only is True:
-            # get vehicle id from topic /carla/actor_list for all vehicles listed in config file
-            actor_info_list = self.wait_for_message("/carla/actor_list", CarlaActorList)
+            # get vehicle id from topic /carla/world/actor_list for all vehicles listed in config file
+            actor_info_list = self.wait_for_message("/carla/world/actor_list", CarlaActorList)
             for vehicle in vehicles:
                 for actor_info in actor_info_list.actors:
                     if actor_info.type == vehicle["type"] and actor_info.rolename == vehicle["id"]:
@@ -131,8 +133,8 @@ def setup_vehicles(self, vehicles):
                 self.setup_sensors(vehicle["sensors"], carla_id)
             else:
                 spawn_object_request = roscomp.get_service_request(SpawnObject)
-                spawn_object_request.type = vehicle["type"]
-                spawn_object_request.id = vehicle["id"]
+                spawn_object_request.blueprint.id = vehicle["type"]
+                spawn_object_request.blueprint.attributes.append(KeyValue(key="role_name", value=vehicle["id"]))
                 spawn_object_request.attach_to = 0
                 spawn_object_request.random_pose = False
 
@@ -204,12 +206,17 @@ def setup_sensors(self, sensors, attached_vehicle_id=None):
                 sensor_type = str(sensor_spec.pop("type"))
                 sensor_id = str(sensor_spec.pop("id"))
 
+                if "pseudo" in sensor_type:
+                    self.logwarn(
+                        "Sensor of type {} is not a valid sensor anymore, ignoring".format(sensor_type))
+                    continue
+
                 sensor_name = sensor_type + "/" + sensor_id
                 if sensor_name in sensor_names:
                     raise NameError
                 sensor_names.append(sensor_name)
 
-                if attached_vehicle_id is None and "pseudo" not in sensor_type:
+                if attached_vehicle_id is None:
                     spawn_point = sensor_spec.pop("spawn_point")
                     sensor_transform = self.create_spawn_point(
                         spawn_point.pop("x"),
@@ -219,7 +226,7 @@ def setup_sensors(self, sensors, attached_vehicle_id=None):
                         spawn_point.pop("pitch", 0.0),
                         spawn_point.pop("yaw", 0.0))
                 else:
-                    # if sensor attached to a vehicle, or is a 'pseudo_actor', allow default pose
+                    # if sensor attached to a vehicle allow default pose
                     spawn_point = sensor_spec.pop("spawn_point", 0)
                     if spawn_point == 0:
                         sensor_transform = self.create_spawn_point(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
@@ -233,8 +240,8 @@ def setup_sensors(self, sensors, attached_vehicle_id=None):
                             spawn_point.pop("yaw", 0.0))
 
                 spawn_object_request = roscomp.get_service_request(SpawnObject)
-                spawn_object_request.type = sensor_type
-                spawn_object_request.id = sensor_id
+                spawn_object_request.blueprint.id = sensor_type
+                spawn_object_request.blueprint.attributes.append(KeyValue(key="role_name", value=sensor_id))
                 spawn_object_request.attach_to = attached_vehicle_id if attached_vehicle_id is not None else 0
                 spawn_object_request.transform = sensor_transform
                 spawn_object_request.random_pose = False  # never set a random pose for a sensor
@@ -245,7 +252,7 @@ def setup_sensors(self, sensors, attached_vehicle_id=None):
                         for attached_object in sensor_spec["attached_objects"]:
                             attached_objects.append(attached_object)
                         continue
-                    spawn_object_request.attributes.append(
+                    spawn_object_request.blueprint.attributes.append(
                         KeyValue(key=str(attribute), value=str(value)))
 
                 response_id = self.spawn_object(spawn_object_request)
@@ -316,7 +323,7 @@ def destroy(self):
                 destroy_object_request = roscomp.get_service_request(DestroyObject)
                 destroy_object_request.id = actor_id
                 self.call_service(self.destroy_object_service,
-                                  destroy_object_request, timeout=0.5, spin_until_response_received=True)
+                                  destroy_object_request, timeout=0.5, spin_until_response_received=True, ignore_future_done=True)
                 self.loginfo("Object {} successfully destroyed.".format(actor_id))
             self.vehicles_sensors = []
 
@@ -325,7 +332,7 @@ def destroy(self):
                 destroy_object_request = roscomp.get_service_request(DestroyObject)
                 destroy_object_request.id = actor_id
                 self.call_service(self.destroy_object_service,
-                                  destroy_object_request, timeout=0.5, spin_until_response_received=True)
+                                  destroy_object_request, timeout=0.5, spin_until_response_received=True, ignore_future_done=True)
                 self.loginfo("Object {} successfully destroyed.".format(actor_id))
             self.global_sensors = []
 
@@ -334,12 +341,13 @@ def destroy(self):
                 destroy_object_request = roscomp.get_service_request(DestroyObject)
                 destroy_object_request.id = player_id
                 self.call_service(self.destroy_object_service,
-                                  destroy_object_request, timeout=0.5, spin_until_response_received=True)
+                                  destroy_object_request, timeout=0.5, spin_until_response_received=True, ignore_future_done=True)
                 self.loginfo("Object {} successfully destroyed.".format(player_id))
             self.players = []
-        except ServiceException:
+
+        except ServiceException as e:
             self.logwarn(
-                'Could not call destroy service on objects, the ros bridge is probably already shutdown')
+                'Could not call destroy service on objects, the ros bridge is probably already shutdown: {}'.format(e))
 
 # ==============================================================================
 # -- main() --------------------------------------------------------------------

carla_spawn_objects/src/carla_spawn_objects/set_initial_pose.py

@@ -17,25 +17,45 @@
 /initialpose might be published via RVIZ '2D Pose Estimate" button.
 """
 
+import json
+import os
+
 import ros_compatibility as roscomp
 from ros_compatibility.node import CompatibleNode
 
 from geometry_msgs.msg import PoseWithCovarianceStamped, Pose
 
-
 class SetInitialPose(CompatibleNode):
 
     def __init__(self):
         super(SetInitialPose, self).__init__("set_initial_pose")
 
-        self.role_name = self.get_param("role_name", "ego_vehicle")
-        # control_id should correspond to the id of the actor.pseudo.control
-        # actor that is set in the config file used to spawn it
-        self.control_id = self.get_param("control_id", "control")
+        self.vehicle_role_names = []
+
+        # Read vehicle role_name from file if provided
+        self.objects_definition_file = self.get_param('objects_definition_file', '')
+        if self.objects_definition_file:
+            if not os.path.exists(self.objects_definition_file):
+                raise RuntimeError(
+                    "Could not read object definitions from {}".format(self.objects_definition_file))
+            with open(self.objects_definition_file) as handle:
+                json_actors = json.loads(handle.read())
+
+        for actor in json_actors["objects"]:
+            actor_type_split = actor["type"].split('.')
+            actor_type = actor_type_split[0]
+            if actor_type == "vehicle":
+                self.vehicle_role_names.append(actor["id"])
+
+        if len(self.vehicle_role_names) == 0:
+            self.vehicle_role_names = self.get_param("role_name", "ego_vehicle")
+
+        if len(self.vehicle_role_names) > 1:
+            self.logwarn("Set initial pose only supports one single vehicle. Using {} from [{}]".format(self.vehicle_role_names[0], self.vehicle_role_names))
 
         self.transform_publisher = self.new_publisher(
             Pose,
-            "/carla/{}/{}/set_transform".format(self.role_name, self.control_id),
+            "/carla/vehicles/{}/control/set_transform".format(self.vehicle_role_names[0]),
             qos_profile=10)
 
         self.initial_pose_subscriber = self.new_subscription(
@@ -62,7 +82,8 @@ def main():
     except KeyboardInterrupt:
         roscomp.loginfo("Cancelled by user.")
     finally:
-        roscomp.shutdown()
+        if roscomp.ok():
+            roscomp.shutdown()
 
 if __name__ == '__main__':
     main()

ros_compatibility/src/ros_compatibility/__init__.py

@@ -82,7 +82,8 @@ def shutdown():
         global _shutdown_hooks
         for h in _shutdown_hooks:
             h()
-        rclpy.shutdown()
+        if ok():
+            rclpy.shutdown()
 
     def _add_shutdown_hook(hook):
         if not callable(hook):

ros_compatibility/src/ros_compatibility/node.py

@@ -269,7 +269,7 @@ def new_client(self, srv_type, srv_name, timeout_sec=None, callback_group=None):
                     "Timeout of {}sec while waiting for service".format(timeout_sec))
             return client
 
-        def call_service(self, client, req, timeout=None, spin_until_response_received=False):
+        def call_service(self, client, req, timeout=None, spin_until_response_received=False, ignore_future_done=False):
             if not spin_until_response_received:
                 response = client.call(req)
                 return response
@@ -278,7 +278,7 @@ def call_service(self, client, req, timeout=None, spin_until_response_received=F
                 rclpy.spin_until_future_complete(
                     self, future, self.executor, timeout)
 
-                if future.done():
+                if ignore_future_done or future.done():
                     return future.result()
                 else:
                     if timeout is not None: