Draft implementation of custom frame id for cameras

happypose_ros/happypose_ros/camera_wrapper.py

@@ -50,7 +50,8 @@ def __init__(
         self._node = node
         self._camera_name = name
 
-        if params.get_entry(self._camera_name).compressed:
+        camera_params = params.get_entry(self._camera_name)
+        if camera_params.compressed:
             img_msg_type = CompressedImage
             topic_postfix = "/image_raw/compressed"
         else:
@@ -59,6 +60,7 @@ def __init__(
 
         self._image = None
         self._cvb = CvBridge()
+        self._estimated_tf_frame_id = camera_params.estimated_tf_frame_id
 
         sync_topics = [
             Subscriber(self._node, img_msg_type, self._camera_name + topic_postfix),
@@ -69,11 +71,20 @@ def __init__(
         self._image_approx_time_sync = ApproximateTimeSynchronizer(
             sync_topics,
             queue_size=5,
-            slop=params.get_entry(self._camera_name).time_sync_slop,
+            slop=camera_params.time_sync_slop,
         )
         # Register callback depending on the configuration
         self._image_approx_time_sync.registerCallback(self._on_image_data_cb)
 
+    def update_params(self, params: happypose_ros.Params.cameras) -> None:
+        """Updates internal parameters of given camera
+
+        :param params: ROS parameters created by generate_parameter_library.
+        :type params: happypose_ros.Params.cameras
+        """
+        camera_params = params.get_entry(self._camera_name)
+        self._estimated_tf_frame_id = camera_params.estimated_tf_frame_id
+
     def image_guarded(func: Callable[..., RetType]) -> Callable[..., RetType]:
         """Decorator, checks if image was already received.
 
@@ -164,13 +175,17 @@ def ready(self) -> bool:
 
     @image_guarded
     def get_last_image_frame_id(self) -> str:
-        """Returns frame id associated with the last received image.
+        """Returns frame id associated with the camera.
 
         :raises RuntimeError: No images were received yet.
         :return: String with a name of the frame id.
         :rtype: str
         """
-        return self._image.header.frame_id
+        return (
+            self._image.header.frame_id
+            if self._estimated_tf_frame_id == ""
+            else self._estimated_tf_frame_id
+        )
 
     @image_guarded
     def get_last_image_stamp(self) -> Time:

happypose_ros/happypose_ros/custom_validation.py

@@ -23,3 +23,19 @@ def torch_check_device(param: Parameter) -> str:
         f"Device {param.name} is not available on this machine."
         f" Available devices: {available_devices}."
     )
+
+
+def check_tf_valid_name(param: Parameter) -> str:
+    """Checks if passed string can be used as a valid frame ID.
+
+    :param param: ROS parameter with a string containing the name of frame ID.
+    :type param: rclpy.Parameter
+    :return: Error explanation. If empty string, everything is correct.
+    :rtype: str
+    """
+    if param.value.startswith("/"):
+        return (
+            f"Invalid param '{param.name}' with value '{param.value}'. "
+            "tf2 frame_ids cannot start with a '/'."
+        )
+    return ""

happypose_ros/happypose_ros/happypose_node.py

@@ -227,7 +227,9 @@ def __init__(self, node_name: str = "happypose_node", **kwargs) -> None:
             )
         )
 
-        if self._params.cameras.get_entry(self._leading_camera).publish_tf:
+        leading_cam_params = self._params.cameras.get_entry(self._leading_camera)
+
+        if leading_cam_params.publish_tf:
             e = ParameterException(
                 "Leading camera can not publish TF",
                 (
@@ -238,6 +240,17 @@ def __init__(self, node_name: str = "happypose_node", **kwargs) -> None:
             self.get_logger().error(str(e))
             raise e
 
+        if leading_cam_params.estimated_tf_frame_id != "":
+            e = ParameterException(
+                "Leading camera can not have `frame_id` overwritten",
+                (
+                    f"cameras.{self._leading_camera}.estimated_tf_frame_id",
+                    f"cameras.{self._leading_camera}.leading",
+                ),
+            )
+            self.get_logger().error(str(e))
+            raise e
+
         self._multiview = len(self._cameras) > 1
         if self._multiview:
             self.get_logger().info(
@@ -310,6 +323,10 @@ def _update_dynamic_params(self, on_init: bool = False) -> None:
         self._stamp_select_strategy = {"newest": max, "oldest": min, "average": mean}[
             self._params.time_stamp_strategy
         ]
+        # Update internal params of cameras
+        for cam in self._cameras.values():
+            cam.update_params(self._params.cameras)
+
         # Clear the queue from old data
         while not self._params_queue.empty():
             self._params_queue.get()

happypose_ros/happypose_ros/happypose_ros_parameters.yaml

@@ -148,10 +148,15 @@ happypose_ros:
         default_value: false
         description: "Publish TF of a given camera relative to the leading camera."
         read_only: true
+      estimated_tf_frame_id:
+        type: string
+        default_value: ""
+        description: "Name of frame_id published for estimated camera poses. If empty string, frame_id of camera is used. Leading camera can not have estimated frame_id."
+        validation:
+          custom_validators::check_tf_valid_name:
       time_sync_slop:
         type: double
         default_value: 0.04
         description: "Delay [seconds] with which incoming messages can be synchronized."
         validation:
           gt<>: [0.0]
-        read_only: true

happypose_ros/test/happypose_testing_utils.py

@@ -208,6 +208,25 @@ def get_transform(
             target_frame, source_frame, rclpy.time.Time(), Duration(seconds=timeout)
         ).transform
 
+    def can_transform(
+        self, target_frame: str, source_frame: str, timeout: float = 5.0
+    ) -> bool:
+        """Check if transformation is possible between source and target frame.
+
+        :param target_frame: Name of the frame to transform into.
+        :type target_frame: str
+        :param source_frame: Name of the input frame.
+        :type source_frame: str
+        :param timeout: Time in seconds to wait for the target
+            frame to become available, defaults to 5.0.
+        :type timeout: float, optional
+        :return: Transformation between source and target frames.
+        :rtype: geometry_msgs.msg.Transform
+        """
+        return self._tf_buffer.can_transform(
+            target_frame, source_frame, rclpy.time.Time(), Duration(seconds=timeout)
+        )
+
     def get_params(
         self, param_names: List[str], timeout: float = 5.0
     ) -> List[Parameter]:

happypose_ros/test/test_initialization_params.py

@@ -116,6 +116,26 @@ def test_leading_publish_tf(happypose_params: dict) -> None:
     assert all(par in str(excinfo.value) for par in params_to_check)
 
 
+def test_leading_estimated_tf_frame_id(happypose_params: dict) -> None:
+    with pytest.raises(ParameterException) as excinfo:
+        HappyPoseNode(
+            **happypose_params,
+            parameter_overrides=[
+                Parameter("cosypose.dataset_name", Parameter.Type.STRING, "ycbv"),
+                Parameter("camera_names", Parameter.Type.STRING_ARRAY, ["cam_1"]),
+                Parameter("cameras.cam_1.leading", Parameter.Type.BOOL, True),
+                Parameter(
+                    "cameras.cam_1.estimated_tf_frame_id",
+                    Parameter.Type.STRING,
+                    "arbitrary_frame_name",
+                ),
+            ],
+        )
+    # Check if both parameters are pointed in the exception
+    params_to_check = ("cameras.cam_1.leading", "cameras.cam_1.estimated_tf_frame_id")
+    assert all(par in str(excinfo.value) for par in params_to_check)
+
+
 def test_device_unknown(
     happypose_params: dict, minimal_overwrites: List[Parameter]
 ) -> None:

happypose_ros/test/test_multi_view_integration.py

@@ -218,6 +218,67 @@ def test_9_check_transforms_correct(self) -> None:
         cam_3_trans = self.node.get_transform("cam_1", "cam_3")
         assert_transform_equal(cam_3_trans, expected_translation)
 
+    def test_10_dynamic_params_change_frame_id(self) -> None:
+        # cam_3 frame_id
+        self.node.set_params(
+            [
+                Parameter(
+                    "cam_3.estimated_tf_frame_id",
+                    Parameter.Type.STRING,
+                    "custom_estimated_cam_3_frame_id",
+                ),
+            ],
+            10.0,
+        )
+
+        # Clear old messages
+        self.node.clear_msg_buffer()
+        # Wait more than the timeout
+        time.sleep(1.0)
+        # Publish three images and expect to pass
+        self.node.publish_image("cam_1", self.cam_1_image, self.K)
+        self.node.publish_image("cam_2", self.cam_2_image, self.K)
+        self.node.publish_image("cam_3", self.cam_3_image, self.K)
+        self.node.assert_message_received("happypose/detections", timeout=60.0)
+
+        self.assertFalse(self.node.can_transform("cam_1", "cam3"))
+
+        # Based on transformed ground truth
+        # Image 1874 camera pose transformed into image 629 camera pose reference frame
+        expected_translation = Transform(
+            translation=Vector3(**dict(zip("xyz", [-0.4790, -0.0166, 0.3517]))),
+            rotation=Quaternion(**dict(zip("xyzw", [0.0536, 0.3365, 0.1493, 0.9281]))),
+        )
+        cam_3_trans = self.node.get_transform(
+            "cam_1", "custom_estimated_cam_3_frame_id"
+        )
+        assert_transform_equal(cam_3_trans, expected_translation)
+
+    def test_11_dynamic_params_change_frame_id_to_empty(self) -> None:
+        # cam_3 frame_id
+        self.node.set_params(
+            [
+                Parameter(
+                    "cam_3.estimated_tf_frame_id",
+                    Parameter.Type.STRING,
+                    "",
+                ),
+            ],
+            10.0,
+        )
+
+        # Clear old messages
+        self.node.clear_msg_buffer()
+        # Wait more than the timeout
+        time.sleep(1.0)
+        # Publish three images and expect to pass
+        self.node.publish_image("cam_1", self.cam_1_image, self.K)
+        self.node.publish_image("cam_2", self.cam_2_image, self.K)
+        self.node.publish_image("cam_3", self.cam_3_image, self.K)
+        self.node.assert_message_received("happypose/detections", timeout=60.0)
+
+        self.assertTrue(self.node.can_transform("cam_1", "cam3"))
+
     def set_timeout(self, timeout: float) -> None:
         self.node.set_params(
             [
@@ -230,7 +291,7 @@ def set_timeout(self, timeout: float) -> None:
             timeout=10.0,
         )
 
-    def test_10_dynamic_params_camera_no_timeout(self) -> None:
+    def test_12_dynamic_params_camera_no_timeout(self) -> None:
         # Clear old messages
         self.node.clear_msg_buffer()
 
@@ -247,7 +308,7 @@ def expect_no_detection(self) -> None:
             msg="One image after timeout triggered the pipeline!",
         )
 
-    def test_11_dynamic_params_camera_timeout_one_camera(self) -> None:
+    def test_13_dynamic_params_camera_timeout_one_camera(self) -> None:
         # Clear old messages
         self.node.clear_msg_buffer()
         # Wait more than the timeout
@@ -258,7 +319,7 @@ def test_11_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_12_dynamic_params_camera_timeout_two_cameras(self) -> None:
+    def test_14_dynamic_params_camera_timeout_two_cameras(self) -> None:
         # Clear old messages
         self.node.clear_msg_buffer()
         # Wait more than the timeout
@@ -268,7 +329,7 @@ def test_12_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_13_dynamic_params_camera_timeout_three_cameras_ok(self) -> None:
+    def test_15_dynamic_params_camera_timeout_three_cameras_ok(self) -> None:
         # Clear old messages
         self.node.clear_msg_buffer()
         # Wait more than the timeout
@@ -279,7 +340,7 @@ def test_13_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_14_dynamic_params_camera_timeout_three_cameras_short(self) -> None:
+    def test_16_dynamic_params_camera_timeout_three_cameras_short(self) -> None:
         # Set timeout to a small value
         timeout = 0.05
         self.set_timeout(timeout)
@@ -297,7 +358,7 @@ def test_14_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_15_dynamic_params_camera_timeout_three_cameras_short_ok(self) -> None:
+    def test_17_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
@@ -354,16 +415,16 @@ def setup_timestamp_test(
             msg=f"Timestamp was not chosen to be '{strategy}'",
         )
 
-    def test_16_dynamic_params_timestamp_strategy(self) -> None:
+    def test_18_dynamic_params_timestamp_strategy(self) -> None:
         offsets = [0.0, 0.01, 0.02]
         self.setup_timestamp_test(offsets, 0.0, "newest")
 
-    def test_17_dynamic_params_timestamp_strategy(self) -> None:
+    def test_19_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_18_dynamic_params_timestamp_strategy(self) -> None:
+    def test_20_dynamic_params_timestamp_strategy(self) -> None:
         offsets = [0.0, 0.01, 0.05]
         self.setup_timestamp_test(offsets, -0.02, "average")