diff --git a/happypose_examples/launch/single_view_demo.launch.py b/happypose_examples/launch/single_view_demo.launch.py index 81059bc..d6ca144 100644 --- a/happypose_examples/launch/single_view_demo.launch.py +++ b/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", diff --git a/happypose_ros/happypose_ros/camera_wrapper.py b/happypose_ros/happypose_ros/camera_wrapper.py index 94cd177..f176f1e 100644 --- a/happypose_ros/happypose_ros/camera_wrapper.py +++ b/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(), ), ] diff --git a/happypose_ros/happypose_ros/happypose_node.py b/happypose_ros/happypose_ros/happypose_node.py index eba2d7b..b27255b 100644 --- a/happypose_ros/happypose_ros/happypose_node.py +++ b/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 diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py index cf01147..e3b4a34 100644 --- a/happypose_ros/test/happypose_testing_utils.py +++ b/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() + } diff --git a/happypose_ros/test/test_multi_view_integration.py b/happypose_ros/test/test_multi_view_integration.py index e9add73..66aaa18 100644 --- a/happypose_ros/test/test_multi_view_integration.py +++ b/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( diff --git a/happypose_ros/test/test_single_view.py b/happypose_ros/test/test_single_view.py index fd0e047..7850cde 100644 --- a/happypose_ros/test/test_single_view.py +++ b/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, ], ) diff --git a/happypose_ros/test/test_single_view_compressed.py b/happypose_ros/test/test_single_view_compressed.py index 9b6199b..98699f9 100644 --- a/happypose_ros/test/test_single_view_compressed.py +++ b/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, ], )