From 1fdbb1761b16fb0dd0a043296ff8e138d7e7d323 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Mon, 23 Sep 2024 13:40:28 +0000 Subject: [PATCH] Use synchronous callback for received images | Remove option for hardcoded k matrix --- happypose_examples/config/camera_info.yaml | 17 ++++- .../config/cosypose_params.yaml | 9 +-- .../config/cosypose_params_multiview.yaml | 13 ++-- happypose_ros/happypose_ros/camera_wrapper.py | 68 +++++++------------ .../happypose_ros_parameters.yaml | 13 ++-- happypose_ros/test/test_multi_view.yaml | 2 - .../test/test_multi_view_integration.py | 41 +++++------ 7 files changed, 72 insertions(+), 91 deletions(-) diff --git a/happypose_examples/config/camera_info.yaml b/happypose_examples/config/camera_info.yaml index 7358a70..a06bb1d 100644 --- a/happypose_examples/config/camera_info.yaml +++ b/happypose_examples/config/camera_info.yaml @@ -1,3 +1,4 @@ +# Camera parameters based on YCBV dataset camera parameters image_width: 640 image_height: 480 camera_name: webcam @@ -17,4 +18,18 @@ rectification_matrix: projection_matrix: rows: 3 cols: 4 - data: [1.0, 0.0, 320.0, 0.0, 0.0, 1.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0] + data: + [ + 1066.778, + 0.0, + 312.9869, + 0.0, + 0.0, + 1067.487, + 241.3109, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + ] diff --git a/happypose_examples/config/cosypose_params.yaml b/happypose_examples/config/cosypose_params.yaml index 8be0727..c403dc5 100644 --- a/happypose_examples/config/cosypose_params.yaml +++ b/happypose_examples/config/cosypose_params.yaml @@ -53,9 +53,6 @@ publish_tf: false # Expect compressed image messages from given camera compressed: false - # Camera intrinsic matrix. If values are valid intrinsic matrix, - # overwrites values from info ROS topic. - # Normally K matrix should be obtained from /camera_info. - # Currently hard-coded due to bug in image_publisher_node - # Do not use hard-coded K matrix unless needed! - k_matrix: [1066.778, 0.0, 312.9869, 0.0, 1067.487, 241.3109, 0.0, 0.0, 1.0] + # Delay [seconds] with which incoming messages can be synchronized. + # Value corresponds to synchronization of messages at 23 Hz. + time_sync_slop: 0.04 diff --git a/happypose_examples/config/cosypose_params_multiview.yaml b/happypose_examples/config/cosypose_params_multiview.yaml index ee2498c..b18aa07 100644 --- a/happypose_examples/config/cosypose_params_multiview.yaml +++ b/happypose_examples/config/cosypose_params_multiview.yaml @@ -67,19 +67,16 @@ publish_tf: false # Expect compressed image messages from given camera compressed: false - # Camera intrinsic matrix. If values are valid intrinsic matrix, - # overwrites values from info ROS topic. - # Normally K matrix should be obtained from /camera_info. - # Currently hard-coded due to bug in image_publisher_node - # Do not use hard-coded K matrix unless needed! - k_matrix: [1066.778, 0.0, 312.9869, 0.0, 1067.487, 241.3109, 0.0, 0.0, 1.0] + # Delay [seconds] with which incoming messages can be synchronized. + # Value corresponds to synchronization of messages at 10 Hz. + time_sync_slop: 0.05 cam_2: leading: false publish_tf: true compressed: false - k_matrix: [1066.778, 0.0, 312.9869, 0.0, 1067.487, 241.3109, 0.0, 0.0, 1.0] + time_sync_slop: 0.05 cam_3: leading: false publish_tf: true compressed: false - k_matrix: [1066.778, 0.0, 312.9869, 0.0, 1067.487, 241.3109, 0.0, 0.0, 1.0] + time_sync_slop: 0.05 diff --git a/happypose_ros/happypose_ros/camera_wrapper.py b/happypose_ros/happypose_ros/camera_wrapper.py index 01cc39d..f150032 100644 --- a/happypose_ros/happypose_ros/camera_wrapper.py +++ b/happypose_ros/happypose_ros/camera_wrapper.py @@ -3,7 +3,6 @@ import numpy.typing as npt from typing import Callable, Union, TypeVar -from rclpy.exceptions import ParameterException from rclpy.node import Node from rclpy.time import Time @@ -11,6 +10,8 @@ from cv_bridge import CvBridge +from message_filters import ApproximateTimeSynchronizer, Subscriber + # Automatically generated file from happypose_ros.happypose_ros_parameters import happypose_ros @@ -56,38 +57,22 @@ def __init__( img_msg_type = Image topic_postfix = "/image_color" - self._camera_k = None - # If param with K matrix is correct, assume it is fixed - param_k_matrix = np.array(params.get_entry(self._camera_name).k_matrix) - self._fixed_k = self._validate_k_matrix(param_k_matrix) - if self._fixed_k: - self._camera_k = param_k_matrix - self._node.get_logger().warn( - f"Camera '{self._camera_name}' uses fixed K matrix." - f" Camera info topic is not subscribed." - ) - # If non zero, but incorrect - elif np.any(np.nonzero(param_k_matrix)): - e = ParameterException( - f"K matrix for '{self._camera_name}' is incorrect", - (f"cameras.{self._camera_name}.k_matrix"), - ) - self._node.get_logger().error(str(e)) - raise e - self._image = None self._cvb = CvBridge() - image_topic = self._camera_name + topic_postfix - self._image_sub = node.create_subscription( - img_msg_type, image_topic, self._image_cb, 5 - ) + sync_topics = [ + Subscriber(self._node, img_msg_type, self._camera_name + topic_postfix), + Subscriber(self._node, CameraInfo, self._camera_name + "/camera_info"), + ] - if not self._fixed_k: - info_topic = self._camera_name + "/camera_info" - self._info_sub = node.create_subscription( - CameraInfo, info_topic, self._camera_info_cb, 5 - ) + # Create time approximate time synchronization + self._image_approx_time_sync = ApproximateTimeSynchronizer( + sync_topics, + queue_size=5, + slop=params.get_entry(self._camera_name).time_sync_slop, + ) + # 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. @@ -140,22 +125,15 @@ def _validate_k_matrix(self, k_arr: npt.NDArray[np.float64]) -> bool: keep_vals = [True, False, True, False, True, True, False, False, True] return np.all(k_arr[keep_vals] > 0.0) and math.isclose(k_arr[-1], 1.0) - def _image_cb(self, image: Union[Image, CompressedImage]) -> None: - """Called on every received image. Saves the image. If intrinsic matrix - already received calls :func:`_image_sync_hook` + def _on_image_data_cb( + self, image: Union[Image, CompressedImage], info: CameraInfo + ) -> None: + """Called on every time synchronized image and camera info are received. + Saves the image and checks if intrinsics are correct. If all checks pass + calls :func:`_image_sync_hook`. :param image: Image received from the camera :type image: Union[sensor_msgs.msg.Image, sensor_msgs.msg.CompressedImage] - """ - - self._image = image - # Fire the callback only if camera info is available - if self._camera_k is not None: - self._image_sync_hook() - - def _camera_info_cb(self, info: CameraInfo) -> None: - """Receives camera info messages and extracts intrinsic matrices from them. - :param info: Camera info message :type info: sensor_msgs.msg.CameraInfo """ @@ -169,6 +147,12 @@ def _camera_info_cb(self, info: CameraInfo) -> None: f" param for the camera '{self._camera_name}'.", 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() def ready(self) -> bool: """Checks if the camera has all the data needed to use. diff --git a/happypose_ros/happypose_ros/happypose_ros_parameters.yaml b/happypose_ros/happypose_ros/happypose_ros_parameters.yaml index 7d3f488..cecae97 100644 --- a/happypose_ros/happypose_ros/happypose_ros_parameters.yaml +++ b/happypose_ros/happypose_ros/happypose_ros_parameters.yaml @@ -189,12 +189,9 @@ happypose_ros: description: "Publish TF of a given camera relative to the leading camera.", read_only: true, } - k_matrix: { - type: double_array, - default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - description: "Camera intrinsic matrix. If values are valid intrinsic matrix, overwrites values from info ROS topic.", - validation: { - fixed_size<>: [9], - lower_element_bounds<>: [0.0] - } + time_sync_slop: { + type: double, + default_value: 0.04, + description: "Delay [seconds] with which incoming messages can be synchronized.", + read_only: true } diff --git a/happypose_ros/test/test_multi_view.yaml b/happypose_ros/test/test_multi_view.yaml index d445d40..30dd5a6 100644 --- a/happypose_ros/test/test_multi_view.yaml +++ b/happypose_ros/test/test_multi_view.yaml @@ -16,8 +16,6 @@ leading: true publish_tf: false compressed: false - k_matrix: - [1066.778, 0.0, 312.9869, 0.0, 1067.487, 241.3109, 0.0, 0.0, 1.0] cam_2: compressed: true cam_3: diff --git a/happypose_ros/test/test_multi_view_integration.py b/happypose_ros/test/test_multi_view_integration.py index d36a1e5..ccd1c92 100644 --- a/happypose_ros/test/test_multi_view_integration.py +++ b/happypose_ros/test/test_multi_view_integration.py @@ -95,6 +95,7 @@ def test_01_node_startup(self, proc_output: ActiveIoHandler) -> None: def test_02_check_topics(self) -> None: # Check if node subscribes to correct topics self.node.assert_node_is_subscriber("cam_1/image_color", timeout=3.0) + self.node.assert_node_is_subscriber("cam_1/camera_info", timeout=3.0) self.node.assert_node_is_subscriber("cam_2/image_color/compressed", timeout=3.0) self.node.assert_node_is_subscriber("cam_2/camera_info", timeout=3.0) self.node.assert_node_is_subscriber("cam_3/image_color", timeout=3.0) @@ -103,15 +104,7 @@ def test_02_check_topics(self) -> None: self.node.assert_node_is_publisher("happypose/vision_info", timeout=3.0) self.node.assert_node_is_publisher("/tf", timeout=3.0) - def test_03_check_not_subscribed_cam_info(self) -> None: - with self.assertRaises(AssertionError) as excinfo: - self.node.assert_node_is_subscriber("cam_1/camera_info", timeout=3.0) - self.assertTrue( - "node is not a subscriber of" in str(excinfo.exception), - msg="One image after timeout triggered the pipeline!", - ) - - def test_04_check_not_publish_markers(self) -> None: + def test_03_check_not_publish_markers(self) -> None: with self.assertRaises(AssertionError) as excinfo: self.node.assert_node_is_publisher("happypose/markers", timeout=3.0) self.assertTrue( @@ -119,7 +112,7 @@ def test_04_check_not_publish_markers(self) -> None: msg="One image after timeout triggered the pipeline!", ) - def test_05_trigger_pipeline(self, proc_output: ActiveIoHandler) -> None: + def test_04_trigger_pipeline(self, proc_output: ActiveIoHandler) -> None: # Clear buffer before expecting any messages self.node.clear_msg_buffer() @@ -136,16 +129,16 @@ def test_05_trigger_pipeline(self, proc_output: ActiveIoHandler) -> None: if not ready: self.fail("Failed to trigger the pipeline!") - def test_06_receive_messages(self) -> None: + def test_05_receive_messages(self) -> None: self.node.assert_message_received("happypose/detections", timeout=180.0) self.node.assert_message_received("happypose/vision_info", timeout=8.0) - def test_07_check_vision_info(self) -> None: + def test_06_check_vision_info(self) -> None: vision_info = self.node.get_received_message("happypose/vision_info") self.assertEqual(vision_info.method, "cosypose") self.assertTrue("ycbv" in vision_info.database_location) - def test_08_check_detection(self) -> None: + def test_07_check_detection(self) -> None: detections = self.node.get_received_message("happypose/detections") # Ensure none of the detections have bounding box for detection in detections.detections: @@ -206,7 +199,7 @@ def test_08_check_detection(self) -> None: assert_pose_equal(ycbv_05.results[0].pose.pose, ycbv_05_pose) assert_pose_equal(ycbv_15.results[0].pose.pose, ycbv_15_pose) - def test_10_check_not_published_transforms(self) -> None: + def test_8_check_not_published_transforms(self) -> None: with self.assertRaises(LookupException) as excinfo: self.node.get_transform("cam_1", "cam_2") self.assertTrue( @@ -215,7 +208,7 @@ def test_10_check_not_published_transforms(self) -> None: msg="'cam_2' is published even though it shouldn't!", ) - def test_11_check_transforms_correct(self) -> None: + def test_9_check_transforms_correct(self) -> None: # Based on transformed ground truth # Image 1874 camera pose transformed into image 629 camera pose reference frame expected_translation = Transform( @@ -237,7 +230,7 @@ def set_timeout(self, timeout: float) -> None: timeout=10.0, ) - def test_12_dynamic_params_camera_no_timeout(self) -> None: + def test_10_dynamic_params_camera_no_timeout(self) -> None: # Clear old messages self.node.clear_msg_buffer() @@ -254,7 +247,7 @@ def expect_no_detection(self) -> None: msg="One image after timeout triggered the pipeline!", ) - def test_13_dynamic_params_camera_timeout_one_camera(self) -> None: + def test_11_dynamic_params_camera_timeout_one_camera(self) -> None: # Clear old messages self.node.clear_msg_buffer() # Wait more than the timeout @@ -265,7 +258,7 @@ def test_13_dynamic_params_camera_timeout_one_camera(self) -> None: self.node.publish_image("cam_1", self.cam_1_image, self.K) self.expect_no_detection() - def test_14_dynamic_params_camera_timeout_two_cameras(self) -> None: + def test_12_dynamic_params_camera_timeout_two_cameras(self) -> None: # Clear old messages self.node.clear_msg_buffer() # Wait more than the timeout @@ -275,7 +268,7 @@ def test_14_dynamic_params_camera_timeout_two_cameras(self) -> None: self.node.publish_image("cam_2", self.cam_2_image, self.K) self.expect_no_detection() - def test_15_dynamic_params_camera_timeout_three_cameras_ok(self) -> None: + def test_13_dynamic_params_camera_timeout_three_cameras_ok(self) -> None: # Clear old messages self.node.clear_msg_buffer() # Wait more than the timeout @@ -286,7 +279,7 @@ def test_15_dynamic_params_camera_timeout_three_cameras_ok(self) -> None: self.node.publish_image("cam_3", self.cam_3_image, self.K) self.node.assert_message_received("happypose/detections", timeout=60.0) - def test_16_dynamic_params_camera_timeout_three_cameras_short(self) -> None: + def test_14_dynamic_params_camera_timeout_three_cameras_short(self) -> None: # Set timeout to a small value timeout = 0.05 self.set_timeout(timeout) @@ -304,7 +297,7 @@ def test_16_dynamic_params_camera_timeout_three_cameras_short(self) -> None: self.node.publish_image("cam_3", self.cam_3_image, self.K) self.expect_no_detection() - def test_17_dynamic_params_camera_timeout_three_cameras_short_ok(self) -> None: + def test_15_dynamic_params_camera_timeout_three_cameras_short_ok(self) -> None: # Set timeout to a small value self.set_timeout(0.05) # Wait more than the timeout @@ -361,16 +354,16 @@ def setup_timestamp_test( msg=f"Timestamp was not chosen to be '{strategy}'", ) - def test_18_dynamic_params_timestamp_strategy(self) -> None: + def test_16_dynamic_params_timestamp_strategy(self) -> None: offsets = [0.0, 0.01, 0.02] self.setup_timestamp_test(offsets, 0.0, "newest") - def test_19_dynamic_params_timestamp_strategy(self) -> None: + def test_17_dynamic_params_timestamp_strategy(self) -> None: offsets = [0.0, 0.01, 0.05] # Offestes are subtracted from time current time so # the result has to be expected in the past self.setup_timestamp_test(offsets, -0.05, "oldest") - def test_20_dynamic_params_timestamp_strategy(self) -> None: + def test_18_dynamic_params_timestamp_strategy(self) -> None: offsets = [0.0, 0.01, 0.05] self.setup_timestamp_test(offsets, -0.02, "average")