Some fixes

- make local_planner subscription to CarlaVehicleStatus best effort and
extend ros compatibility qos setting
- adapt cala_walker_agent to carla native ros interface
- fix derived_objects_visualizer

Author: berndgassmann

carla_ad_agent/src/carla_ad_agent/local_planner.py

@@ -16,7 +16,7 @@
 
 import ros_compatibility as roscomp
 from ros_compatibility.node import CompatibleNode
-from ros_compatibility.qos import QoSProfile, DurabilityPolicy
+from ros_compatibility.qos import QoSProfile, DurabilityPolicy, QoSProfileSubscriber
 
 from carla_ad_agent.vehicle_pid_controller import VehiclePIDController
 from carla_ad_agent.misc import distance_vehicle
@@ -73,7 +73,7 @@ def __init__(self):
             CarlaVehicleStatus,
             "/carla/vehicles/{}/vehicle_status".format(role_name),
             self.vehicle_status_cb,
-            qos_profile=10)
+            QoSProfileSubscriber(depth=10))
         self._path_subscriber = self.new_subscription(
             Path,
             "/carla/vehicles/{}/waypoints".format(role_name),

carla_msgs

@@ -44,31 +44,31 @@ def __init__(self):
 
         # wait for ros bridge to create relevant topics
         try:
-            self.wait_for_message("/carla/vehicles/{}/odometry".format(role_name), Odometry, qos_profile=10)
+            self.wait_for_message("/carla/walkers/{}/object".format(role_name), Odometry, qos_profile=10)
         except ROSInterruptException as e:
             if not roscomp.ok:
                 raise e
 
-        self._odometry_subscriber = self.new_subscription(
+        self._object_subscriber = self.new_subscription(
             Odometry,
-            "/carla/vehicles/{}/odometry".format(role_name),
-            self.odometry_updated,
+            "/carla/walkers/{}/object".format(role_name),
+            self.object_updated,
             qos_profile=10)
 
         self.control_publisher = self.new_publisher(
             CarlaWalkerControl,
-            "/carla/vehicles/{}/walker_control_cmd".format(role_name),
+            "/carla/walkers/{}/control/walker_control_cmd".format(role_name),
             qos_profile=1)
 
         self._route_subscriber = self.new_subscription(
             Path,
-            "/carla/vehicles/{}/waypoints".format(role_name),
+            "/carla/walkers/{}/waypoints".format(role_name),
             self.path_updated,
             qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
 
         self._target_speed_subscriber = self.new_subscription(
             Float64, 
-            "/carla/vehicles/{}/target_speed".format(role_name),
+            "/carla/walkers/{}/target_speed".format(role_name),
             self.target_speed_updated,
             qos_profile=10)
 
@@ -97,9 +97,9 @@ def path_updated(self, path):
         for elem in path.poses:
             self._waypoints.append(elem.pose)
 
-    def odometry_updated(self, odo):
+    def object_updated(self, odo):
         """
-        callback on new odometry
+        callback on new object
         """
         self._current_pose = odo.pose.pose
 

carla_walker_agent/src/carla_walker_agent/carla_walker_agent.py

@@ -10,9 +10,9 @@
 import tf2_ros
 
 
-class ObjectVisualizer(Node):
+class DerivedObjectsVisualizer(Node):
     def __init__(self):
-        super().__init__('object_visualizer')
+        super().__init__('derived_objects_visualizer')
 
         self.declare_parameter("objects_topic", value="/carla/world/objects_with_covariance")
 

derived_objects_visualizer/derived_objects_visualizer/derived_objects_visualizer.py

@@ -1,26 +1,26 @@
 import rclpy
 
-from object_visualizer.object_visualizer import ObjectVisualizer
+from derived_objects_visualizer.derived_objects_visualizer import DerivedObjectsVisualizer
 
 
 def main(args=None):
     rclpy.init(args=args)
 
     # create nodes
-    object_visualizer = ObjectVisualizer()
+    derived_objects_visualizer = DerivedObjectsVisualizer()
 
     # create executor and add nodes
     executor = rclpy.executors.MultiThreadedExecutor()
-    if(not executor.add_node(object_visualizer)):
-        object_visualizer.get_logger().error(
-            "Can't add object_visualizer to executor")
+    if(not executor.add_node(derived_objects_visualizer)):
+        derived_objects_visualizer.get_logger().error(
+            "Can't add derived_objects_visualizer to executor")
 
     try:
         executor.spin()
     except KeyboardInterrupt:
         print("Keyboard interrupt. Shutting down.")
 
-    object_visualizer.before_shutdown()
+    derived_objects_visualizer.before_shutdown()
     if rclpy.ok():
         rclpy.shutdown()
 

derived_objects_visualizer/derived_objects_visualizer/derived_objects_visualizer_main.py

@@ -123,11 +123,16 @@ def destroy(self):
         ros_compatibility.qos.DurabilityPolicy.TRANSIENT_LOCAL: rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL,
         ros_compatibility.qos.DurabilityPolicy.VOLATILE: rclpy.qos.DurabilityPolicy.VOLATILE
     }
-
+    _RELIABILITY_POLICY_MAP = {
+        ros_compatibility.qos.ReliabilityPolicy.BEST_EFFORT: rclpy.qos.ReliabilityPolicy.BEST_EFFORT,
+        ros_compatibility.qos.ReliabilityPolicy.RELIABLE: rclpy.qos.ReliabilityPolicy.RELIABLE
+    }
+ 
     def _get_rclpy_qos_profile(qos_profile):
         return rclpy.qos.QoSProfile(
             depth=qos_profile.depth,
-            durability=_DURABILITY_POLICY_MAP[qos_profile.durability]
+            durability=_DURABILITY_POLICY_MAP[qos_profile.durability],
+            reliability=_RELIABILITY_POLICY_MAP[qos_profile.reliability]
         )
 
     class CompatibleNode(Node):

ros_compatibility/src/ros_compatibility/node.py

@@ -15,11 +15,26 @@ class DurabilityPolicy(enum.Enum):
     TRANSIENT_LOCAL = 1
     VOLATILE = 2
 
+class ReliabilityPolicy(enum.Enum):
+
+    RELIABLE = 1
+    BEST_EFFORT = 2
+
 
 class QoSProfile(object):
-    def __init__(self, depth, durability=DurabilityPolicy.VOLATILE):
+    def __init__(self, depth, durability=DurabilityPolicy.VOLATILE, reliability=ReliabilityPolicy.RELIABLE):
         self.depth = depth
         self.durability = durability
+        self.reliability = reliability
 
     def is_latched(self):
         return self.durability == DurabilityPolicy.TRANSIENT_LOCAL
+
+class QoSProfilePublisher(QoSProfile):
+    def __init__(self, depth, durability=DurabilityPolicy.TRANSIENT_LOCAL, reliability=ReliabilityPolicy.RELIABLE):
+        super(QoSProfilePublisher, self).__init__(depth, durability, reliability)
+    
+class QoSProfileSubscriber(QoSProfile):
+    def __init__(self, depth, durability=DurabilityPolicy.VOLATILE, reliability=ReliabilityPolicy.BEST_EFFORT):
+        super(QoSProfileSubscriber, self).__init__(depth, durability, reliability)
+