From 0529ed9fd8b72f38adb9a04115e1e358c36a5583 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski Date: Tue, 29 Oct 2024 17:39:36 +0000 Subject: [PATCH] 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())