From 19641f7687e58a320d6fb84da0c0eba9c45030a7 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 30 Oct 2024 13:01:44 +0000 Subject: [PATCH 1/4] Add advanced QOS settings --- .../launch/single_view_demo.launch.py | 28 ++++++++++++++-- happypose_ros/happypose_ros/camera_wrapper.py | 18 ++++++++-- happypose_ros/happypose_ros/happypose_node.py | 33 +++++++++++++------ happypose_ros/test/happypose_testing_utils.py | 27 ++++++++++++++- .../test/test_multi_view_integration.py | 10 ++++-- happypose_ros/test/test_single_view.py | 5 ++- .../test/test_single_view_compressed.py | 5 ++- 7 files changed, 107 insertions(+), 19 deletions(-) diff --git a/happypose_examples/launch/single_view_demo.launch.py b/happypose_examples/launch/single_view_demo.launch.py index 81059bc..69d4da0 100644 --- a/happypose_examples/launch/single_view_demo.launch.py +++ b/happypose_examples/launch/single_view_demo.launch.py @@ -30,7 +30,7 @@ def launch_setup( image_publisher_node = Node( package="image_publisher", executable="image_publisher_node", - namespace="cam_1", + namespace="cam_1/uncropped", output="screen", parameters=[ { @@ -44,6 +44,30 @@ def launch_setup( ], ) + image_cropper = Node( + package="image_proc", + executable="crop_decimate_node", + namespace="cam_1", + output="screen", + parameters=[ + { + "use_sim_time": False, + "decimation_x": 1, + "decimation_y": 1, + "width": 504, + "height": 378, + "offset_x": 10, + "offset_y": 51, + } + ], + remappings=[ + ("in/image_raw", "/cam_1/uncropped/image_raw"), + ("in/camera_info", "/cam_1/uncropped/camera_info"), + ("out/image_raw", "/cam_1/image_raw"), + ("out/camera_info", "/cam_1/camera_info"), + ], + ) + # Include common part of the demo launch files happypose_example_common_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -64,7 +88,7 @@ def launch_setup( }.items(), ) - return [happypose_example_common_launch, image_publisher_node] + return [happypose_example_common_launch, image_publisher_node, image_cropper] def generate_launch_description(): diff --git a/happypose_ros/happypose_ros/camera_wrapper.py b/happypose_ros/happypose_ros/camera_wrapper.py index 2eada75..89934b1 100644 --- a/happypose_ros/happypose_ros/camera_wrapper.py +++ b/happypose_ros/happypose_ros/camera_wrapper.py @@ -5,6 +5,8 @@ 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 @@ -61,8 +63,20 @@ def __init__( self._cvb = CvBridge() sync_topics = [ - Subscriber(self._node, img_msg_type, self._camera_name + topic_postfix), - Subscriber(self._node, CameraInfo, self._camera_name + "/camera_info"), + Subscriber( + self._node, + 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(), + ), ] # Create time approximate time synchronization 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 7a506aa..9295e64 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 @@ -109,6 +109,7 @@ def __init__( (f"{cam[0]}/camera_info"), 10, ), + self.create_publisher(CameraInfo, (f"{cam[0]}/camera_info"), 10), ) for cam in cameras } @@ -590,3 +591,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, ], ) From 395511f7290ca21a3e4f64496e7739a9821a45f5 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 30 Oct 2024 13:04:13 +0000 Subject: [PATCH 2/4] Bring back standard single view example --- .../launch/single_view_demo.launch.py | 36 +++++-------------- 1 file changed, 9 insertions(+), 27 deletions(-) diff --git a/happypose_examples/launch/single_view_demo.launch.py b/happypose_examples/launch/single_view_demo.launch.py index 69d4da0..d6ca144 100644 --- a/happypose_examples/launch/single_view_demo.launch.py +++ b/happypose_examples/launch/single_view_demo.launch.py @@ -30,7 +30,7 @@ def launch_setup( image_publisher_node = Node( package="image_publisher", executable="image_publisher_node", - namespace="cam_1/uncropped", + namespace="cam_1", output="screen", parameters=[ { @@ -44,30 +44,6 @@ def launch_setup( ], ) - image_cropper = Node( - package="image_proc", - executable="crop_decimate_node", - namespace="cam_1", - output="screen", - parameters=[ - { - "use_sim_time": False, - "decimation_x": 1, - "decimation_y": 1, - "width": 504, - "height": 378, - "offset_x": 10, - "offset_y": 51, - } - ], - remappings=[ - ("in/image_raw", "/cam_1/uncropped/image_raw"), - ("in/camera_info", "/cam_1/uncropped/camera_info"), - ("out/image_raw", "/cam_1/image_raw"), - ("out/camera_info", "/cam_1/camera_info"), - ], - ) - # Include common part of the demo launch files happypose_example_common_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -83,12 +59,13 @@ def launch_setup( ), launch_arguments={ "dataset_name": LaunchConfiguration("dataset_name"), + "model_type": LaunchConfiguration("model_type"), "device": LaunchConfiguration("device"), "use_rviz": LaunchConfiguration("use_rviz"), }.items(), ) - return [happypose_example_common_launch, image_publisher_node, image_cropper] + return [happypose_example_common_launch, image_publisher_node] def generate_launch_description(): @@ -96,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", From 0a0e77a0928d7466d85af4536b3ef52d41e42bc5 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 30 Oct 2024 13:06:12 +0000 Subject: [PATCH 3/4] Remove accidentally created publisher --- happypose_ros/test/happypose_testing_utils.py | 1 - 1 file changed, 1 deletion(-) diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py index 9295e64..b205692 100644 --- a/happypose_ros/test/happypose_testing_utils.py +++ b/happypose_ros/test/happypose_testing_utils.py @@ -109,7 +109,6 @@ def __init__( (f"{cam[0]}/camera_info"), 10, ), - self.create_publisher(CameraInfo, (f"{cam[0]}/camera_info"), 10), ) for cam in cameras } From 9029eb04ab1574a013873e8efe20d4981324f2cc Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 30 Oct 2024 13:10:32 +0000 Subject: [PATCH 4/4] Use reliable QoS in tests --- happypose_ros/test/happypose_testing_utils.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py index b205692..0938eb5 100644 --- a/happypose_ros/test/happypose_testing_utils.py +++ b/happypose_ros/test/happypose_testing_utils.py @@ -11,7 +11,7 @@ from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.time import Time -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 @@ -91,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]: ( @@ -102,12 +108,12 @@ def __init__( if isinstance(cam[1], Metaclass_Image) else f"{cam[0]}/image_raw/compressed" ), - 10, + qos_profile=reliable_qos, ), self.create_publisher( CameraInfo, (f"{cam[0]}/camera_info"), - 10, + qos_profile=reliable_qos, ), ) for cam in cameras