diff --git a/README.md b/README.md index 08ab499..1509601 100644 --- a/README.md +++ b/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) diff --git a/yolov8_bringup/launch/yolov8.launch.py b/yolov8_bringup/launch/yolov8.launch.py index 4d4fb62..0322ea0 100644 --- a/yolov8_bringup/launch/yolov8.launch.py +++ b/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) diff --git a/yolov8_bringup/launch/yolov8_3d.launch.py b/yolov8_bringup/launch/yolov8_3d.launch.py index 3aa94dd..46fa8fa 100644 --- a/yolov8_bringup/launch/yolov8_3d.launch.py +++ b/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,12 +155,18 @@ 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( @@ -141,8 +174,11 @@ def generate_launch_description(): 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) diff --git a/yolov8_ros/yolov8_ros/debug_node.py b/yolov8_ros/yolov8_ros/debug_node.py index 1403749..18e730b 100644 --- a/yolov8_ros/yolov8_ros/debug_node.py +++ b/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) diff --git a/yolov8_ros/yolov8_ros/detect_3d_node.py b/yolov8_ros/yolov8_ros/detect_3d_node.py index adfc52c..3d55250 100644 --- a/yolov8_ros/yolov8_ros/detect_3d_node.py +++ b/yolov8_ros/yolov8_ros/detect_3d_node.py @@ -19,9 +19,10 @@ 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 @@ -29,6 +30,8 @@ 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") diff --git a/yolov8_ros/yolov8_ros/tracking_node.py b/yolov8_ros/yolov8_ros/tracking_node.py index a1118c5..e16cae0 100644 --- a/yolov8_ros/yolov8_ros/tracking_node.py +++ b/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) diff --git a/yolov8_ros/yolov8_ros/yolov8_node.py b/yolov8_ros/yolov8_ros/yolov8_node.py index d892651..9b97923 100644 --- a/yolov8_ros/yolov8_ros/yolov8_node.py +++ b/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