qos reliabilities added to nodes and launch files

README.md

@@ -29,6 +29,7 @@ $ ros2 launch yolov8_bringup yolov8.launch.py
 - **enable**: wether to start YOLOv8 enabled (default: True)
 - **threshold**: detection threshold (default: 0.5)
 - **input_image_topic**: camera topic of RGB images (default: /camera/rgb/image_raw)
+- **image_reliability**: reliability for the image topic ( 0=system default, 1=Reliable, 2=Best Effort)
 
 ### YOLOv8 3D
 
@@ -44,8 +45,11 @@ $ ros2 launch yolov8_bringup yolov8_3d.launch.py
 - **enable**: wether to start YOLOv8 enabled (default: True)
 - **threshold**: detection threshold (default: 0.5)
 - **input_image_topic**: camera topic of RGB images (default: /camera/rgb/image_raw)
+- **image_reliability**: reliability for the image topic ( 0=system default, 1=Reliable, 2=Best Effort)
 - **input_depth_topic**: camera topic of depth images (default: /camera/depth/image_raw)
+- **depth_image_reliability**: reliability for the depth image topic ( 0=system default, 1=Reliable, 2=Best Effort)
 - **input_depth_info_topic**: camera topic for info data (default: /camera/depth/camera_info)
+- **depth_info_reliability**: reliability for the depth info topic ( 0=system default, 1=Reliable, 2=Best Effort)
 - **depth_image_units_divisor**: divisor to convert the depth image into metres (default: 1000)
 - **target_frame**: frame to transform the 3D boxes (default: base_link)
 - **maximum_detection_threshold**: maximum detection threshold in the z axis (default: 0.3)

yolov8_bringup/launch/yolov8.launch.py

@@ -61,6 +61,13 @@ def generate_launch_description():
         default_value="/camera/rgb/image_raw",
         description="Name of the input image topic")
 
+    image_reliability = LaunchConfiguration("image_reliability")
+    image_reliability_cmd = DeclareLaunchArgument(
+        "image_reliability",
+        default_value="2",
+        choices=["0", "1", "2"],
+        description="Specific reliability QoS of the input image topic (0=system default, 1=Reliable, 2=Best Effort)")
+
     namespace = LaunchConfiguration("namespace")
     namespace_cmd = DeclareLaunchArgument(
         "namespace",
@@ -75,10 +82,13 @@ def generate_launch_description():
         executable="yolov8_node",
         name="yolov8_node",
         namespace=namespace,
-        parameters=[{"model": model,
-                     "device": device,
-                     "enable": enable,
-                     "threshold": threshold}],
+        parameters=[{
+            "model": model,
+            "device": device,
+            "enable": enable,
+            "threshold": threshold,
+            "image_reliability": image_reliability,
+        }],
         remappings=[("image_raw", input_image_topic)]
     )
 
@@ -87,7 +97,10 @@ def generate_launch_description():
         executable="tracking_node",
         name="tracking_node",
         namespace=namespace,
-        parameters=[{"tracker": tracker}],
+        parameters=[{
+            "tracker": tracker,
+            "image_reliability": image_reliability
+        }],
         remappings=[("image_raw", input_image_topic)]
     )
 
@@ -96,8 +109,11 @@ def generate_launch_description():
         executable="debug_node",
         name="debug_node",
         namespace=namespace,
-        remappings=[("image_raw", input_image_topic),
-                    ("detections", "tracking")]
+        parameters=[{"image_reliability": image_reliability}],
+        remappings=[
+            ("image_raw", input_image_topic),
+            ("detections", "tracking")
+        ]
     )
 
     ld = LaunchDescription()
@@ -108,6 +124,7 @@ def generate_launch_description():
     ld.add_action(enable_cmd)
     ld.add_action(threshold_cmd)
     ld.add_action(input_image_topic_cmd)
+    ld.add_action(image_reliability_cmd)
     ld.add_action(namespace_cmd)
 
     ld.add_action(detector_node_cmd)

yolov8_bringup/launch/yolov8_3d.launch.py

@@ -61,18 +61,39 @@ def generate_launch_description():
         default_value="/camera/rgb/image_raw",
         description="Name of the input image topic")
 
+    image_reliability = LaunchConfiguration("image_reliability")
+    image_reliability_cmd = DeclareLaunchArgument(
+        "image_reliability",
+        default_value="2",
+        choices=["0", "1", "2"],
+        description="Specific reliability QoS of the input image topic (0=system default, 1=Reliable, 2=Best Effort)")
+
     input_depth_topic = LaunchConfiguration("input_depth_topic")
     input_depth_topic_cmd = DeclareLaunchArgument(
         "input_depth_topic",
         default_value="/camera/depth/image_raw",
         description="Name of the input depth topic")
 
+    depth_image_reliability = LaunchConfiguration("depth_image_reliability")
+    depth_image_reliability_cmd = DeclareLaunchArgument(
+        "depth_image_reliability",
+        default_value="2",
+        choices=["0", "1", "2"],
+        description="Specific reliability QoS of the input depth image topic (0=system default, 1=Reliable, 2=Best Effort)")
+
     input_depth_info_topic = LaunchConfiguration("input_depth_info_topic")
     input_depth_info_topic_cmd = DeclareLaunchArgument(
         "input_depth_info_topic",
         default_value="/camera/depth/camera_info",
         description="Name of the input depth info topic")
 
+    depth_info_reliability = LaunchConfiguration("depth_info_reliability")
+    depth_info_reliability_cmd = DeclareLaunchArgument(
+        "depth_info_reliability",
+        default_value="2",
+        choices=["0", "1", "2"],
+        description="Specific reliability QoS of the input depth info topic (0=system default, 1=Reliable, 2=Best Effort)")
+
     depth_image_units_divisor = LaunchConfiguration(
         "depth_image_units_divisor")
     depth_image_units_divisor_cmd = DeclareLaunchArgument(
@@ -107,10 +128,13 @@ def generate_launch_description():
         executable="yolov8_node",
         name="yolov8_node",
         namespace=namespace,
-        parameters=[{"model": model,
-                     "device": device,
-                     "enable": enable,
-                     "threshold": threshold}],
+        parameters=[{
+            "model": model,
+            "device": device,
+            "enable": enable,
+            "threshold": threshold,
+            "image_reliability": image_reliability,
+        }],
         remappings=[("image_raw", input_image_topic)]
     )
 
@@ -119,7 +143,10 @@ def generate_launch_description():
         executable="tracking_node",
         name="tracking_node",
         namespace=namespace,
-        parameters=[{"tracker": tracker}],
+        parameters=[{
+            "tracker": tracker,
+            "image_reliability": image_reliability
+        }],
         remappings=[("image_raw", input_image_topic)]
     )
 
@@ -128,21 +155,30 @@ def generate_launch_description():
         executable="detect_3d_node",
         name="detect_3d_node",
         namespace=namespace,
-        parameters=[{"target_frame": target_frame,
-                     "maximum_detection_threshold": maximum_detection_threshold,
-                     "depth_image_units_divisor": depth_image_units_divisor}],
-        remappings=[("depth_image", input_depth_topic),
-                    ("depth_info", input_depth_info_topic),
-                    ("detections", "tracking")]
+        parameters=[{
+            "target_frame": target_frame,
+            "maximum_detection_threshold": maximum_detection_threshold,
+            "depth_image_units_divisor": depth_image_units_divisor,
+            "depth_image_reliability": depth_image_reliability,
+            "depth_info_reliability": depth_info_reliability
+        }],
+        remappings=[
+            ("depth_image", input_depth_topic),
+            ("depth_info", input_depth_info_topic),
+            ("detections", "tracking")
+        ]
     )
 
     debug_node_cmd = Node(
         package="yolov8_ros",
         executable="debug_node",
         name="debug_node",
         namespace=namespace,
-        remappings=[("image_raw", input_image_topic),
-                    ("detections", "detections_3d")]
+        parameters=[{"image_reliability": image_reliability}],
+        remappings=[
+            ("image_raw", input_image_topic),
+            ("detections", "detections_3d")
+        ]
     )
 
     ld = LaunchDescription()
@@ -153,8 +189,11 @@ def generate_launch_description():
     ld.add_action(enable_cmd)
     ld.add_action(threshold_cmd)
     ld.add_action(input_image_topic_cmd)
+    ld.add_action(image_reliability_cmd)
     ld.add_action(input_depth_topic_cmd)
+    ld.add_action(depth_image_reliability_cmd)
     ld.add_action(input_depth_info_topic_cmd)
+    ld.add_action(depth_info_reliability_cmd)
     ld.add_action(depth_image_units_divisor_cmd)
     ld.add_action(target_frame_cmd)
     ld.add_action(maximum_detection_threshold_cmd)

yolov8_ros/yolov8_ros/debug_node.py

@@ -22,7 +22,10 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.duration import Duration
-from rclpy.qos import qos_profile_sensor_data
+from rclpy.qos import QoSProfile
+from rclpy.qos import QoSHistoryPolicy
+from rclpy.qos import QoSDurabilityPolicy
+from rclpy.qos import QoSReliabilityPolicy
 
 import message_filters
 from cv_bridge import CvBridge
@@ -46,6 +49,17 @@ def __init__(self) -> None:
         self._class_to_color = {}
         self.cv_bridge = CvBridge()
 
+        # params
+        self.declare_parameter("image_reliability",
+                               QoSReliabilityPolicy.BEST_EFFORT)
+        image_qos_profile = QoSProfile(
+            reliability=self.get_parameter(
+                "image_reliability").get_parameter_value().integer_value,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth=1
+        )
+
         # pubs
         self._dbg_pub = self.create_publisher(Image, "dbg_image", 10)
         self._bb_markers_pub = self.create_publisher(
@@ -55,7 +69,7 @@ def __init__(self) -> None:
 
         # subs
         image_sub = message_filters.Subscriber(
-            self, Image, "image_raw", qos_profile=qos_profile_sensor_data)
+            self, Image, "image_raw", qos_profile=image_qos_profile)
         detections_sub = message_filters.Subscriber(
             self, DetectionArray, "detections", qos_profile=10)
 

yolov8_ros/yolov8_ros/detect_3d_node.py

@@ -19,16 +19,19 @@
 
 import rclpy
 from rclpy.node import Node
-from rclpy.qos import qos_profile_sensor_data
-from sensor_msgs.msg import CameraInfo, Image
-from geometry_msgs.msg import TransformStamped
+from rclpy.qos import QoSProfile
+from rclpy.qos import QoSHistoryPolicy
+from rclpy.qos import QoSDurabilityPolicy
+from rclpy.qos import QoSReliabilityPolicy
 
 import message_filters
 from cv_bridge import CvBridge
 from tf2_ros.buffer import Buffer
 from tf2_ros import TransformException
 from tf2_ros.transform_listener import TransformListener
 
+from sensor_msgs.msg import CameraInfo, Image
+from geometry_msgs.msg import TransformStamped
 from yolov8_msgs.msg import Detection
 from yolov8_msgs.msg import DetectionArray
 from yolov8_msgs.msg import KeyPoint3D
@@ -45,13 +48,35 @@ def __init__(self) -> None:
         self.declare_parameter("target_frame", "base_link")
         self.target_frame = self.get_parameter(
             "target_frame").get_parameter_value().string_value
+
         self.declare_parameter("maximum_detection_threshold", 0.3)
         self.maximum_detection_threshold = self.get_parameter(
             "maximum_detection_threshold").get_parameter_value().double_value
+
         self.declare_parameter("depth_image_units_divisor", 1000)
         self.depth_image_units_divisor = self.get_parameter(
             "depth_image_units_divisor").get_parameter_value().integer_value
 
+        self.declare_parameter("depth_image_reliability",
+                               QoSReliabilityPolicy.BEST_EFFORT)
+        depth_image_qos_profile = QoSProfile(
+            reliability=self.get_parameter(
+                "depth_image_reliability").get_parameter_value().integer_value,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth=1
+        )
+
+        self.declare_parameter("depth_info_reliability",
+                               QoSReliabilityPolicy.BEST_EFFORT)
+        depth_info_qos_profile = QoSProfile(
+            reliability=self.get_parameter(
+                "depth_info_reliability").get_parameter_value().integer_value,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth=1
+        )
+
         # aux
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
@@ -63,10 +88,10 @@ def __init__(self) -> None:
         # subs
         self.depth_sub = message_filters.Subscriber(
             self, Image, "depth_image",
-            qos_profile=qos_profile_sensor_data)
+            qos_profile=depth_image_qos_profile)
         self.depth_info_sub = message_filters.Subscriber(
             self, CameraInfo, "depth_info",
-            qos_profile=qos_profile_sensor_data)
+            qos_profile=depth_info_qos_profile)
         self.detections_sub = message_filters.Subscriber(
             self, DetectionArray, "detections")
 

yolov8_ros/yolov8_ros/tracking_node.py

@@ -17,8 +17,11 @@
 import numpy as np
 
 import rclpy
-from rclpy.qos import qos_profile_sensor_data
 from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from rclpy.qos import QoSHistoryPolicy
+from rclpy.qos import QoSDurabilityPolicy
+from rclpy.qos import QoSReliabilityPolicy
 
 import message_filters
 from cv_bridge import CvBridge
@@ -44,6 +47,16 @@ def __init__(self) -> None:
         tracker = self.get_parameter(
             "tracker").get_parameter_value().string_value
 
+        self.declare_parameter("image_reliability",
+                               QoSReliabilityPolicy.BEST_EFFORT)
+        image_qos_profile = QoSProfile(
+            reliability=self.get_parameter(
+                "image_reliability").get_parameter_value().integer_value,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth=1
+        )
+
         self.cv_bridge = CvBridge()
         self.tracker = self.create_tracker(tracker)
 
@@ -52,7 +65,7 @@ def __init__(self) -> None:
 
         # subs
         image_sub = message_filters.Subscriber(
-            self, Image, "image_raw", qos_profile=qos_profile_sensor_data)
+            self, Image, "image_raw", qos_profile=image_qos_profile)
         detections_sub = message_filters.Subscriber(
             self, DetectionArray, "detections", qos_profile=10)
 

yolov8_ros/yolov8_ros/yolov8_node.py

@@ -17,8 +17,11 @@
 from typing import List, Dict
 
 import rclpy
-from rclpy.qos import qos_profile_sensor_data
 from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from rclpy.qos import QoSHistoryPolicy
+from rclpy.qos import QoSDurabilityPolicy
+from rclpy.qos import QoSReliabilityPolicy
 
 from cv_bridge import CvBridge
 
@@ -61,6 +64,16 @@ def __init__(self) -> None:
         self.enable = self.get_parameter(
             "enable").get_parameter_value().bool_value
 
+        self.declare_parameter("image_reliability",
+                               QoSReliabilityPolicy.BEST_EFFORT)
+        image_qos_profile = QoSProfile(
+            reliability=self.get_parameter(
+                "image_reliability").get_parameter_value().integer_value,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            depth=1
+        )
+
         self.cv_bridge = CvBridge()
         self.yolo = YOLO(model)
         self.yolo.fuse()
@@ -71,7 +84,7 @@ def __init__(self) -> None:
         # subs
         self._sub = self.create_subscription(
             Image, "image_raw", self.image_cb,
-            qos_profile_sensor_data
+            image_qos_profile
         )
 
         # services