From e92590c0944e6c670e74b861fb04535cf753c570 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 29 Oct 2024 16:59:31 +0000 Subject: [PATCH 01/11] Use `image_geometry` to crop ROI --- happypose_ros/happypose_ros/camera_wrapper.py | 59 +++++++------------ happypose_ros/package.xml | 1 + 2 files changed, 21 insertions(+), 39 deletions(-) diff --git a/happypose_ros/happypose_ros/camera_wrapper.py b/happypose_ros/happypose_ros/camera_wrapper.py index 2eada75..25cdaa1 100644 --- a/happypose_ros/happypose_ros/camera_wrapper.py +++ b/happypose_ros/happypose_ros/camera_wrapper.py @@ -9,6 +9,7 @@ from sensor_msgs.msg import CameraInfo, CompressedImage, Image from cv_bridge import CvBridge +from image_geometry.cameramodels import PinholeCameraModel from message_filters import ApproximateTimeSynchronizer, Subscriber @@ -58,7 +59,9 @@ def __init__( topic_postfix = "/image_raw" self._image = None + self._camera_info = None self._cvb = CvBridge() + self._cam_model = PinholeCameraModel() sync_topics = [ Subscriber(self._node, img_msg_type, self._camera_name + topic_postfix), @@ -74,43 +77,24 @@ def __init__( # Register callback depending on the configuration self._image_approx_time_sync.registerCallback(self._on_image_data_cb) - def image_guarded(func: Callable[..., RetType]) -> Callable[..., RetType]: - """Decorator, checks if image was already received. + def data_recieved_guarded(func: Callable[..., RetType]) -> Callable[..., RetType]: + """Decorator, checks if data was already received. :param func: Function to wrap. :type func: Callable[..., RetType] - :raises RuntimeError: No images were received yet. - :return: Wrapped function. - :rtype: Callable[..., RetType] - """ - - def _image_guarded_inner(self, *args, **kwargs) -> RetType: - if self._image is None: - raise RuntimeError( - f"No images received yet from camera '{self._camera_name}'!" - ) - return func(self, *args, **kwargs) - - return _image_guarded_inner - - def k_matrix_guarded(func: Callable[..., RetType]) -> Callable[..., RetType]: - """Decorator, checks if an intrinsic matrix was already received. - - :param func: Function to wrap. - :type func: Callable[..., RetType] - :raises RuntimeError: No images were received yet. + :raises RuntimeError: No data was received yet. :return: Wrapped function. :rtype: Callable[..., RetType] """ - def _k_matrix_guarded_inner(self, *args, **kwargs) -> RetType: - if self._camera_k is None: + def _data_recieved_guarded_inner(self, *args, **kwargs) -> RetType: + if self._image is None and self._camera_info is None: raise RuntimeError( - f"Camera info was not received yet from camera '{self._camera_name}'!" + f"No data received yet from the camera '{self._camera_name}'!" ) return func(self, *args, **kwargs) - return _k_matrix_guarded_inner + return _data_recieved_guarded_inner def _validate_k_matrix(self, k_arr: npt.NDArray[np.float64]) -> bool: """Performs basic check of the structure of intrinsic matrix. @@ -139,20 +123,16 @@ def _on_image_data_cb( """ if self._validate_k_matrix(info.k): - self._camera_k = info.k + self._camera_info = info else: self._node.get_logger().warn( - f"K matrix from topic '{self._info_sub.topic_name()}' is incorrect!" - f" Fix it or set 'cameras.{self._camera_name}.k_matrix'" - f" param for the camera '{self._camera_name}'.", + f"K matrix from topic '{self._info_sub.topic_name()}' is incorrect!", throttle_duration_sec=5.0, ) return self._image = image - # Fire the callback only if camera info is available - if self._camera_k is not None: - self._image_sync_hook() + self._image_sync_hook() def ready(self) -> bool: """Checks if the camera has all the data needed to use. @@ -160,9 +140,9 @@ def ready(self) -> bool: :return: Camera image and intrinsic matrix are available :rtype: bool """ - return self._image is not None and self._camera_k is not None + return self._image is not None - @image_guarded + @data_recieved_guarded def get_last_image_frame_id(self) -> str: """Returns frame id associated with the last received image. @@ -172,7 +152,7 @@ def get_last_image_frame_id(self) -> str: """ return self._image.header.frame_id - @image_guarded + @data_recieved_guarded def get_last_image_stamp(self) -> Time: """Returns time stamp associated with last received image. @@ -182,7 +162,7 @@ def get_last_image_stamp(self) -> Time: """ return Time.from_msg(self._image.header.stamp) - @image_guarded + @data_recieved_guarded def get_last_rgb_image(self) -> npt.NDArray[np.uint8]: """Returns last received color image. @@ -203,7 +183,7 @@ def get_last_rgb_image(self) -> npt.NDArray[np.uint8]: ) return encoder(self._image, desired_encoding) - @k_matrix_guarded + @data_recieved_guarded def get_last_k_matrix(self) -> npt.NDArray[np.float64]: """Returns intrinsic matrix associated with last received camera info message. @@ -211,4 +191,5 @@ def get_last_k_matrix(self) -> npt.NDArray[np.float64]: :return: 3x3 Numpy array with intrinsic matrix. :rtype: numpy.typing.NDArray[numpy.float64] """ - return self._camera_k.reshape((3, 3)) + self._cam_model.fromCameraInfo(self._camera_info) + return self._cam_model.full_K diff --git a/happypose_ros/package.xml b/happypose_ros/package.xml index 0d69a3a..fac23f6 100644 --- a/happypose_ros/package.xml +++ b/happypose_ros/package.xml @@ -22,6 +22,7 @@ tf2_ros geometry_msgs happypose_msgs + image_geometry sensor_msgs std_msgs vision_msgs From 3adef6d3f59ef5eff2be0fcc8539b618f62015a4 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 29 Oct 2024 17:05:41 +0000 Subject: [PATCH 02/11] Fix type casting --- happypose_ros/happypose_ros/camera_wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/happypose_ros/happypose_ros/camera_wrapper.py b/happypose_ros/happypose_ros/camera_wrapper.py index 25cdaa1..9c7e287 100644 --- a/happypose_ros/happypose_ros/camera_wrapper.py +++ b/happypose_ros/happypose_ros/camera_wrapper.py @@ -192,4 +192,4 @@ def get_last_k_matrix(self) -> npt.NDArray[np.float64]: :rtype: numpy.typing.NDArray[numpy.float64] """ self._cam_model.fromCameraInfo(self._camera_info) - return self._cam_model.full_K + return np.array(self._cam_model.full_K) From 0529ed9fd8b72f38adb9a04115e1e358c36a5583 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 29 Oct 2024 17:39:36 +0000 Subject: [PATCH 03/11] Fix QOS and expected matrix type --- happypose_ros/happypose_ros/camera_wrapper.py | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/happypose_ros/happypose_ros/camera_wrapper.py b/happypose_ros/happypose_ros/camera_wrapper.py index 9c7e287..94cd177 100644 --- a/happypose_ros/happypose_ros/camera_wrapper.py +++ b/happypose_ros/happypose_ros/camera_wrapper.py @@ -5,6 +5,7 @@ from rclpy.node import Node from rclpy.time import Time +from rclpy.qos import qos_profile_sensor_data from sensor_msgs.msg import CameraInfo, CompressedImage, Image @@ -64,8 +65,18 @@ def __init__( self._cam_model = PinholeCameraModel() 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, + ), + Subscriber( + self._node, + CameraInfo, + self._camera_name + "/camera_info", + qos_profile=qos_profile_sensor_data, + ), ] # Create time approximate time synchronization @@ -192,4 +203,4 @@ def get_last_k_matrix(self) -> npt.NDArray[np.float64]: :rtype: numpy.typing.NDArray[numpy.float64] """ self._cam_model.fromCameraInfo(self._camera_info) - return np.array(self._cam_model.full_K) + return np.array(self._cam_model.intrinsicMatrix()) From fc3f7381aab41f1d91ea9501265daadad58b3bdc Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 29 Oct 2024 22:27:15 +0000 Subject: [PATCH 04/11] Add base for image cropping --- happypose_ros/test/happypose_testing_utils.py | 38 ++++++++++++++++--- happypose_ros/test/single_view_base.py | 34 +++++++++++++++++ 2 files changed, 67 insertions(+), 5 deletions(-) diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py index 7a506aa..cf01147 100644 --- a/happypose_ros/test/happypose_testing_utils.py +++ b/happypose_ros/test/happypose_testing_utils.py @@ -11,6 +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 tf2_ros.buffer import Buffer @@ -21,7 +22,7 @@ from cv_bridge import CvBridge from geometry_msgs.msg import Pose, Transform -from sensor_msgs.msg import CameraInfo, Image, CompressedImage +from sensor_msgs.msg import CameraInfo, Image, CompressedImage, RegionOfInterest from sensor_msgs.msg._image import Metaclass_Image from std_msgs.msg import Header from vision_msgs.msg import Detection2D, Detection2DArray, BoundingBox2D, VisionInfo @@ -102,12 +103,12 @@ def __init__( if isinstance(cam[1], Metaclass_Image) else f"{cam[0]}/image_raw/compressed" ), - 10, + qos_profile=qos_profile_sensor_data, ), self.create_publisher( CameraInfo, (f"{cam[0]}/camera_info"), - 10, + qos_profile=qos_profile_sensor_data, ), ) for cam in cameras @@ -296,6 +297,10 @@ def publish_image( bgr: npt.NDArray[np.uint8], K: npt.NDArray[np.float32], stamp: Optional[Time] = None, + width: int = 0, + height: int = 0, + x_offset: int = 0, + y_offset: int = 0, ) -> None: """Publish image for a given camera. @@ -307,24 +312,47 @@ def publish_image( :type K: numpy.typing.NDArray[numpy.float32] :param stamp: Time stamp of the message. If None uses the current time, defaults to None. :type stamp: Optional[rclpy.Time], optional + :param width: Final cropped image width. 0 means no cropping. + :type width: int, optional + :param height: Final cropped image height. 0 means no cropping. + :type height: int, optional + :param x_offset: X offset used for cropping. + :type x_offset: int, optional + :param y_offset: Y offset used for cropping. + :type y_offset: int, optional """ if stamp is None: stamp = self.get_clock().now() + height = bgr.shape[0] if height == 0 else height + width = bgr.shape[1] if width == 0 else width + cropped_bgr = bgr[ + y_offset : y_offset + height, + x_offset : x_offset + width, + ] + if isinstance(self._cam_pubs[cam][0].msg_type, Metaclass_Image): - img_msg = self._cvb.cv2_to_imgmsg(bgr, encoding="rgb8") + img_msg = self._cvb.cv2_to_imgmsg(cropped_bgr, encoding="rgb8") else: # Convert BGR to RGB - rgb = bgr[:, :, ::-1] + rgb = cropped_bgr[:, :, ::-1] img_msg = self._cvb.cv2_to_compressed_imgmsg(rgb, dst_format="jpg") header = Header(frame_id=cam, stamp=stamp.to_msg()) img_msg.header = header info_msg = CameraInfo( header=header, + # Resized dimensions are specified in ROI message. + # Here shape before resizing is expected. height=bgr.shape[0], width=bgr.shape[1], k=K.reshape(-1), + roi=RegionOfInterest( + width=width, + height=height, + x_offset=x_offset, + y_offset=y_offset, + ), ) # Publish messages self._cam_pubs[cam][0].publish(img_msg) diff --git a/happypose_ros/test/single_view_base.py b/happypose_ros/test/single_view_base.py index 5049b95..63c2c83 100644 --- a/happypose_ros/test/single_view_base.py +++ b/happypose_ros/test/single_view_base.py @@ -425,3 +425,37 @@ def test_13_dynamic_params_labels_to_keep_empty_message( 0, "After filtering labels published message is not empty!", ) + + def test_14_region_of_interest(self: ActiveIoHandler) -> None: + # Clear old messages + self.node.clear_msg_buffer() + + # Publish new to trigger parameter change + self.node.publish_image( + "cam_1", + self.rgb, + self.K, + width=504, + height=378, + x_offset=10, + y_offset=51, + ) + + self.node.assert_message_received("happypose/detections", timeout=20.0) + detections = self.node.get_received_message("happypose/detections") + + self.assertGreaterEqual( + len(detections.detections), 1, "Incorrect number of detected objects!" + ) + + ycbv_15 = assert_and_find_detection(detections, "ycbv-obj_000015") + + # Based on ground truth, object poses for image 629 + ycbv_15_pose = Pose( + position=Point(**dict(zip("xyz", [-0.1013, 0.0329, 0.9138]))), + orientation=Quaternion( + **dict(zip("xyzw", [0.2526, 0.4850, 0.7653, -0.3392])) + ), + ) + + assert_pose_equal(ycbv_15.results[0].pose.pose, ycbv_15_pose) From 19641f7687e58a320d6fb84da0c0eba9c45030a7 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 30 Oct 2024 13:01:44 +0000 Subject: [PATCH 05/11] 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 06/11] 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 07/11] 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 08/11] 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 From 4c0f666c254a5e34061e045b5d73f3de8393257b Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 30 Oct 2024 13:33:54 +0000 Subject: [PATCH 09/11] Fix unittests --- happypose_ros/test/happypose_testing_utils.py | 9 ++++++--- happypose_ros/test/single_view_base.py | 12 +++++++++++- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py index e3b4a34..a024148 100644 --- a/happypose_ros/test/happypose_testing_utils.py +++ b/happypose_ros/test/happypose_testing_utils.py @@ -352,13 +352,16 @@ def publish_image( height=bgr.shape[0], width=bgr.shape[1], k=K.reshape(-1), - roi=RegionOfInterest( + ) + if bgr.shape != cropped_bgr.shape: + info_msg.roi = RegionOfInterest( width=width, height=height, x_offset=x_offset, y_offset=y_offset, - ), - ) + do_rectify=True, + ) + # Publish messages self._cam_pubs[cam][0].publish(img_msg) self._cam_pubs[cam][1].publish(info_msg) diff --git a/happypose_ros/test/single_view_base.py b/happypose_ros/test/single_view_base.py index 63c2c83..7796020 100644 --- a/happypose_ros/test/single_view_base.py +++ b/happypose_ros/test/single_view_base.py @@ -445,12 +445,21 @@ def test_14_region_of_interest(self: ActiveIoHandler) -> None: detections = self.node.get_received_message("happypose/detections") self.assertGreaterEqual( - len(detections.detections), 1, "Incorrect number of detected objects!" + len(detections.detections), 2, "Incorrect number of detected objects!" ) + # In compressed case pose of 05 object is not reliable + ycbv_02 = assert_and_find_detection(detections, "ycbv-obj_000002") ycbv_15 = assert_and_find_detection(detections, "ycbv-obj_000015") # Based on ground truth, object poses for image 629 + ycbv_02_pose = Pose( + position=Point(**dict(zip("xyz", [0.0552, -0.0913, 1.0283]))), + orientation=Quaternion( + **dict(zip("xyzw", [0.2279, 0.1563, 0.0245, 0.9607])) + ), + ) + ycbv_15_pose = Pose( position=Point(**dict(zip("xyz", [-0.1013, 0.0329, 0.9138]))), orientation=Quaternion( @@ -458,4 +467,5 @@ def test_14_region_of_interest(self: ActiveIoHandler) -> None: ), ) + assert_pose_equal(ycbv_02.results[0].pose.pose, ycbv_02_pose) assert_pose_equal(ycbv_15.results[0].pose.pose, ycbv_15_pose) From 097585295ed6c6491c9eec64844696b4d18989a5 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 30 Oct 2024 13:36:06 +0000 Subject: [PATCH 10/11] Fix `received` typos --- happypose_ros/happypose_ros/camera_wrapper.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/happypose_ros/happypose_ros/camera_wrapper.py b/happypose_ros/happypose_ros/camera_wrapper.py index f176f1e..3fa8c40 100644 --- a/happypose_ros/happypose_ros/camera_wrapper.py +++ b/happypose_ros/happypose_ros/camera_wrapper.py @@ -91,7 +91,7 @@ def __init__( # Register callback depending on the configuration self._image_approx_time_sync.registerCallback(self._on_image_data_cb) - def data_recieved_guarded(func: Callable[..., RetType]) -> Callable[..., RetType]: + def data_received_guarded(func: Callable[..., RetType]) -> Callable[..., RetType]: """Decorator, checks if data was already received. :param func: Function to wrap. @@ -101,14 +101,14 @@ def data_recieved_guarded(func: Callable[..., RetType]) -> Callable[..., RetType :rtype: Callable[..., RetType] """ - def _data_recieved_guarded_inner(self, *args, **kwargs) -> RetType: + def _data_received_guarded_inner(self, *args, **kwargs) -> RetType: if self._image is None and self._camera_info is None: raise RuntimeError( f"No data received yet from the camera '{self._camera_name}'!" ) return func(self, *args, **kwargs) - return _data_recieved_guarded_inner + return _data_received_guarded_inner def _validate_k_matrix(self, k_arr: npt.NDArray[np.float64]) -> bool: """Performs basic check of the structure of intrinsic matrix. @@ -156,7 +156,7 @@ def ready(self) -> bool: """ return self._image is not None - @data_recieved_guarded + @data_received_guarded def get_last_image_frame_id(self) -> str: """Returns frame id associated with the last received image. @@ -166,7 +166,7 @@ def get_last_image_frame_id(self) -> str: """ return self._image.header.frame_id - @data_recieved_guarded + @data_received_guarded def get_last_image_stamp(self) -> Time: """Returns time stamp associated with last received image. @@ -176,7 +176,7 @@ def get_last_image_stamp(self) -> Time: """ return Time.from_msg(self._image.header.stamp) - @data_recieved_guarded + @data_received_guarded def get_last_rgb_image(self) -> npt.NDArray[np.uint8]: """Returns last received color image. @@ -197,7 +197,7 @@ def get_last_rgb_image(self) -> npt.NDArray[np.uint8]: ) return encoder(self._image, desired_encoding) - @data_recieved_guarded + @data_received_guarded def get_last_k_matrix(self) -> npt.NDArray[np.float64]: """Returns intrinsic matrix associated with last received camera info message. From 167c9f880c746e8bebf14d3a8ffd7b50fb5b1a8d Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Wed, 30 Oct 2024 16:14:34 +0000 Subject: [PATCH 11/11] Fix unittest | Increase quality of compressed images --- happypose_ros/test/happypose_testing_utils.py | 2 +- happypose_ros/test/single_view_base.py | 30 +------------------ 2 files changed, 2 insertions(+), 30 deletions(-) diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py index a024148..d5e1a68 100644 --- a/happypose_ros/test/happypose_testing_utils.py +++ b/happypose_ros/test/happypose_testing_utils.py @@ -341,7 +341,7 @@ def publish_image( else: # Convert BGR to RGB rgb = cropped_bgr[:, :, ::-1] - img_msg = self._cvb.cv2_to_compressed_imgmsg(rgb, dst_format="jpg") + img_msg = self._cvb.cv2_to_compressed_imgmsg(rgb, dst_format="png") header = Header(frame_id=cam, stamp=stamp.to_msg()) img_msg.header = header diff --git a/happypose_ros/test/single_view_base.py b/happypose_ros/test/single_view_base.py index 7796020..23d3a4f 100644 --- a/happypose_ros/test/single_view_base.py +++ b/happypose_ros/test/single_view_base.py @@ -398,35 +398,7 @@ def test_12_dynamic_params_labels_to_keep_reset(self: ActiveIoHandler) -> None: msg="Number of objects with symmetries is not equal to full YCBV dataset!", ) - def test_13_dynamic_params_labels_to_keep_empty_message( - self: ActiveIoHandler, - ) -> None: - # Undo filtering of the labels - self.node.set_params( - [ - Parameter( - "cosypose.inference.labels_to_keep", - Parameter.Type.STRING_ARRAY, - ["ycbv-obj_000020"], - ) - ] - ) - - # Clear old messages - self.node.clear_msg_buffer() - - # Publish new to trigger parameter change - self.node.publish_image("cam_1", self.rgb, self.K) - self.node.assert_message_received("happypose/detections", timeout=20.0) - detections = self.node.get_received_message("happypose/detections") - - self.assertGreaterEqual( - len(detections.detections), - 0, - "After filtering labels published message is not empty!", - ) - - def test_14_region_of_interest(self: ActiveIoHandler) -> None: + def test_13_region_of_interest(self: ActiveIoHandler) -> None: # Clear old messages self.node.clear_msg_buffer()