Use synchronous callback for received images | Remove option for hard…

…coded k matrix

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,
+    ]

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_name>/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

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_name>/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

happypose_ros/happypose_ros/camera_wrapper.py

@@ -3,14 +3,15 @@
 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
 
 from sensor_msgs.msg import CameraInfo, CompressedImage, Image
 
 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.

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
       }

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:

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,23 +104,15 @@ 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(
             "node is not a publisher of" in str(excinfo.exception),
             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")