Add ROI and binning handling

happypose_ros/happypose_ros/camera_wrapper.py

@@ -5,10 +5,12 @@
 
 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
 
 from cv_bridge import CvBridge
+from image_geometry.cameramodels import PinholeCameraModel
 
 from message_filters import ApproximateTimeSynchronizer, Subscriber
 
@@ -58,11 +60,23 @@ 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),
-            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
@@ -74,43 +88,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,30 +134,26 @@ 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.
 
         :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 +163,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 +173,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,12 +194,13 @@ 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.
 
         :raises RuntimeError: No camera info messages were received yet.
         :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 np.array(self._cam_model.intrinsicMatrix())

happypose_ros/package.xml

@@ -22,6 +22,7 @@
   <depend>tf2_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>happypose_msgs</depend>
+  <depend>image_geometry</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>vision_msgs</depend>

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)

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)