Merge remote-tracking branch 'origin/feature/qos-configuration' into …

…feature/image-cropping

happypose_examples/launch/single_view_demo.launch.py

@@ -59,6 +59,7 @@ def launch_setup(
         ),
         launch_arguments={
             "dataset_name": LaunchConfiguration("dataset_name"),
+            "model_type": LaunchConfiguration("model_type"),
             "device": LaunchConfiguration("device"),
             "use_rviz": LaunchConfiguration("use_rviz"),
         }.items(),
@@ -72,7 +73,12 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "dataset_name",
             default_value="ycbv",
-            description="Which dataset to use for inference.",
+            description="Name of BOP dataset, used to load specific weights and object models.",
+        ),
+        DeclareLaunchArgument(
+            "model_type",
+            default_value="pbr",
+            description="Type of neural network model to use. Available: 'pbr'|'synth+real'.",
         ),
         DeclareLaunchArgument(
             "device",

happypose_ros/happypose_ros/camera_wrapper.py

@@ -6,6 +6,7 @@
 from rclpy.node import Node
 from rclpy.time import Time
 from rclpy.qos import qos_profile_sensor_data
+from rclpy.qos_overriding_options import QoSOverridingOptions
 
 from sensor_msgs.msg import CameraInfo, CompressedImage, Image
 
@@ -70,12 +71,14 @@ def __init__(
                 img_msg_type,
                 self._camera_name + topic_postfix,
                 qos_profile=qos_profile_sensor_data,
+                qos_overriding_options=QoSOverridingOptions.with_default_policies(),
             ),
             Subscriber(
                 self._node,
                 CameraInfo,
                 self._camera_name + "/camera_info",
                 qos_profile=qos_profile_sensor_data,
+                qos_overriding_options=QoSOverridingOptions.with_default_policies(),
             ),
         ]
 

happypose_ros/happypose_ros/happypose_node.py

@@ -13,7 +13,9 @@
 import rclpy.logging
 from rclpy.node import Node
 from rclpy.time import Time
+from rclpy.qos import qos_profile_system_default
 from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile
+from rclpy.qos_overriding_options import QoSOverridingOptions
 
 from tf2_ros import TransformBroadcaster
 
@@ -169,20 +171,28 @@ def __init__(self, node_name: str = "happypose_node", **kwargs) -> None:
         self._last_pipeline_trigger = self.get_clock().now()
 
         self._detections_publisher = self.create_publisher(
-            Detection2DArray, "happypose/detections", 10
+            Detection2DArray,
+            "happypose/detections",
+            qos_profile=qos_profile_system_default,
+            qos_overriding_options=QoSOverridingOptions.with_default_policies(),
         )
         self._vision_info_publisher = self.create_publisher(
-            VisionInfo, "happypose/vision_info", 10
+            VisionInfo,
+            "happypose/vision_info",
+            qos_profile=qos_profile_system_default,
+            qos_overriding_options=QoSOverridingOptions.with_default_policies(),
         )
 
-        # Set the message to be "latched"
-        qos = QoSProfile(
-            depth=1,
-            durability=DurabilityPolicy.TRANSIENT_LOCAL,
-            history=HistoryPolicy.KEEP_LAST,
-        )
         self._symmetries_publisher = self.create_publisher(
-            ObjectSymmetriesArray, "happypose/object_symmetries", qos
+            ObjectSymmetriesArray,
+            "happypose/object_symmetries",
+            # Set the message to be "latched"
+            qos_profile=QoSProfile(
+                depth=1,
+                durability=DurabilityPolicy.TRANSIENT_LOCAL,
+                history=HistoryPolicy.KEEP_LAST,
+            ),
+            qos_overriding_options=QoSOverridingOptions.with_default_policies(),
         )
 
         if self._params.cameras.n_min_cameras > len(self._cameras):
@@ -257,7 +267,10 @@ def __init__(self, node_name: str = "happypose_node", **kwargs) -> None:
         # Create debug publisher
         if self._params.visualization.publish_markers:
             self._marker_publisher = self.create_publisher(
-                MarkerArray, "happypose/markers", 10
+                MarkerArray,
+                "happypose/markers",
+                qos_profile=qos_profile_system_default,
+                qos_overriding_options=QoSOverridingOptions.with_default_policies(),
             )
 
         # Start the worker when all possible errors are handled

happypose_ros/test/happypose_testing_utils.py

@@ -2,7 +2,7 @@
 import numpy.typing as npt
 import pinocchio as pin
 import time
-from typing import Any, List, Optional, Tuple, Union
+from typing import Any, Dict, List, Optional, Tuple, Union
 import unittest
 import urllib
 
@@ -11,8 +11,7 @@
 from rclpy.node import Node
 from rclpy.parameter import Parameter
 from rclpy.time import Time
-from rclpy.qos import qos_profile_sensor_data
-from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile
+from rclpy.qos import DurabilityPolicy, HistoryPolicy, ReliabilityPolicy, QoSProfile
 
 from tf2_ros.buffer import Buffer
 from tf2_ros.transform_listener import TransformListener
@@ -92,6 +91,12 @@ def __init__(
 
         self._tested_node_name = tested_node_name
 
+        reliable_qos = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            depth=10,
+            history=HistoryPolicy.KEEP_ALL,
+        )
+
         # Create dict with camera topic publishers
         self._cam_pubs = {
             cam[0]: (
@@ -103,12 +108,12 @@ def __init__(
                         if isinstance(cam[1], Metaclass_Image)
                         else f"{cam[0]}/image_raw/compressed"
                     ),
-                    qos_profile=qos_profile_sensor_data,
+                    qos_profile=reliable_qos,
                 ),
                 self.create_publisher(
                     CameraInfo,
                     (f"{cam[0]}/camera_info"),
-                    qos_profile=qos_profile_sensor_data,
+                    qos_profile=reliable_qos,
                 ),
             )
             for cam in cameras
@@ -618,3 +623,27 @@ def _almost_equal(x: float, y: float, delta: float) -> bool:
 
     if abs(msg.center.theta) > 1e-8:
         "Bbox theta is not 0.0!"
+
+
+def create_camera_reliable_qos_config(
+    namespace: str, cam_name: str
+) -> Dict[str, Union[str, int]]:
+    """Creates dictionary with parameters configuring reliability of sensor topics.
+
+    :param topic_name: Name of the topic for which the config has to be created.
+    :type topic_name: str
+    :return: Dictionary with parameters.
+    :rtype: Dict[str, Union[str, int]]
+    """
+
+    qos_settings = {
+        "reliability": "reliable",
+        "depth": 10,
+        "history": "keep_all",
+    }
+
+    return {
+        f"qos_overrides./{namespace}/{cam_name}/{topic}.subscription.{key}": value
+        for topic in ("camera_info", "image_raw")
+        for key, value in qos_settings.items()
+    }

happypose_ros/test/test_multi_view_integration.py

@@ -34,6 +34,7 @@
     assert_and_find_detection,
     assert_pose_equal,
     assert_transform_equal,
+    create_camera_reliable_qos_config,
 )
 
 
@@ -53,13 +54,18 @@ def generate_test_description():
     )
 
     # Spawn the happypose_ros node
+    ns = "test_multi_view"
     happypose_node = launch_ros.actions.Node(
         package="happypose_ros",
         executable="happypose_node",
         name="happypose_node",
-        namespace="test_multi_view",
+        namespace=ns,
         # Dynamically set device
-        parameters=[{"device": device}, happypose_params_path],
+        parameters=[
+            {"device": device},
+            *[create_camera_reliable_qos_config(ns, f"cam_{i}") for i in range(3)],
+            happypose_params_path,
+        ],
     )
 
     return LaunchDescription(

happypose_ros/test/test_single_view.py

@@ -11,6 +11,7 @@
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
 
+from happypose_testing_utils import create_camera_reliable_qos_config
 from single_view_base import SingleViewBase
 
 
@@ -30,14 +31,16 @@ def generate_test_description():
     )
 
     # Spawn the happypose_ros node
+    ns = "test_single_view"
     happypose_node = launch_ros.actions.Node(
         package="happypose_ros",
         executable="happypose_node",
         name="happypose_node",
-        namespace="test_single_view",
+        namespace=ns,
         parameters=[
             # Dynamically set device and expect raw images
             {"device": device, "cameras.cam_1.compressed": False},
+            create_camera_reliable_qos_config(ns, "cam_1"),
             happypose_params_path,
         ],
     )

happypose_ros/test/test_single_view_compressed.py

@@ -11,6 +11,7 @@
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
 
+from happypose_testing_utils import create_camera_reliable_qos_config
 from single_view_base import SingleViewBase
 
 
@@ -30,14 +31,16 @@ def generate_test_description():
     )
 
     # Spawn the happypose_ros node
+    ns = "test_single_view_compressed"
     happypose_node = launch_ros.actions.Node(
         package="happypose_ros",
         executable="happypose_node",
         name="happypose_node",
-        namespace="test_single_view_compressed",
+        namespace=ns,
         parameters=[
             # Dynamically set device and expect compressed images
             {"device": device, "cameras.cam_1.compressed": True},
+            create_camera_reliable_qos_config(ns, "cam_1"),
             happypose_params_path,
         ],
     )