Feature/estimated tf names (#18)

happypose_ros/happypose_ros/camera_wrapper.py

@@ -53,7 +53,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:
@@ -63,6 +64,7 @@ def __init__(
         self._image = None
         self._camera_info = None
         self._cvb = CvBridge()
+        self._estimated_tf_frame_id = camera_params.estimated_tf_frame_id
         self._cam_model = PinholeCameraModel()
 
         sync_topics = [
@@ -86,11 +88,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 data_received_guarded(func: Callable[..., RetType]) -> Callable[..., RetType]:
         """Decorator, checks if data was already received.
 
@@ -158,13 +169,17 @@ def ready(self) -> bool:
 
     @data_received_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
+        )
 
     @data_received_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

@@ -237,7 +237,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",
                 (
@@ -248,6 +250,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(
@@ -323,6 +336,11 @@ 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
+        if not on_init:
+            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

@@ -214,6 +214,31 @@ def get_transform(
             target_frame, source_frame, rclpy.time.Time(), Duration(seconds=timeout)
         ).transform
 
+    def can_transform(
+        self,
+        target_frame: str,
+        source_frame: str,
+        stamp: Time = rclpy.time.Time(),
+        timeout: float = 5.0,
+    ) -> bool:
+        """Check if transformation has been published between source and target frame
+        at a given time stamp. If no ``stamp`` was passed, use the latest transformation.
+
+        :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, stamp, 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,28 @@ 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),
+                # Clear state after previous test
+                Parameter("cameras.cam_1.publish_tf", Parameter.Type.BOOL, False),
+                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.yaml

@@ -10,7 +10,7 @@
         n_workers: 1
     camera_names: ["cam_1", "cam_2", "cam_3"]
     cameras:
-      timeout: 0.0
+      timeout: 1.5
       n_min_cameras: 3
       cam_1:
         leading: true

happypose_ros/test/test_multi_view_integration.py

@@ -12,7 +12,6 @@
 from rclpy.parameter import Parameter
 from rclpy.time import Time
 
-from tf2_ros import LookupException
 
 from geometry_msgs.msg import Point, Pose, Transform, Vector3, Quaternion
 from sensor_msgs.msg import Image, CompressedImage
@@ -205,25 +204,100 @@ def test_07_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_8_check_not_published_transforms(self) -> None:
-        with self.assertRaises(LookupException) as excinfo:
-            self.node.get_transform("cam_1", "cam_2")
-        self.assertTrue(
-            '"cam_2" passed to lookupTransform argument source_frame does not exist'
-            in str(excinfo.exception),
-            msg="'cam_2' is published even though it shouldn't!",
+    def test_08_check_not_published_transforms(self) -> None:
+        self.assertFalse(
+            self.node.can_transform("cam_1", "cam_2"),
+            msg="`cam_2` frame_id was was published even thought it shouldn't!",
         )
 
-    def test_9_check_transforms_correct(self) -> None:
+    def test_09_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(
             translation=Vector3(**dict(zip("xyz", [-0.4790, -0.0166, 0.3517]))),
             rotation=Quaternion(**dict(zip("xyzw", [0.0536, 0.3365, 0.1493, 0.9281]))),
         )
+        self.assertTrue(
+            self.node.can_transform("cam_1", "cam_3"),
+            msg="`cam_3` frame_id was not published!",
+        )
         cam_3_trans = self.node.get_transform("cam_1", "cam_3")
         assert_transform_equal(cam_3_trans, expected_translation)
 
+    def push_data(self, stamp: Time) -> None:
+        # Clear old messages
+        self.node.clear_msg_buffer()
+        # Publish three images and expect to pass
+        self.node.publish_image("cam_1", self.cam_1_image, self.K, stamp)
+        self.node.publish_image("cam_2", self.cam_2_image, self.K, stamp)
+        self.node.publish_image("cam_3", self.cam_3_image, self.K, stamp)
+        self.node.assert_message_received("happypose/detections", timeout=60.0)
+
+    def test_10_dynamic_params_change_frame_id(self) -> None:
+        # Set cam_3 frame_id
+        self.node.set_params(
+            [
+                Parameter(
+                    "cameras.cam_3.estimated_tf_frame_id",
+                    Parameter.Type.STRING,
+                    "custom_cam_3_frame_id",
+                ),
+            ],
+            10.0,
+        )
+
+        # Wait more than the timeout
+        time.sleep(2.0)
+        # Get fresh timestamp
+        stamp = self.node.get_clock().now()
+        self.push_data(stamp)
+
+        self.assertFalse(
+            self.node.can_transform("cam_1", "cam_3", stamp),
+            msg="`cam_3` frame_id was was published even thought it shouldn't!",
+        )
+        self.assertTrue(
+            self.node.can_transform("cam_1", "custom_cam_3_frame_id", stamp),
+            msg="`custom_cam_3_frame_id` frame_id was not published!",
+        )
+
+        # 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_cam_3_frame_id")
+        assert_transform_equal(cam_3_trans, expected_translation)
+
+    def test_11_dynamic_params_change_frame_id_to_empty(self) -> None:
+        # Use default frame_if for cam_3
+        self.node.set_params(
+            [
+                Parameter(
+                    "cameras.cam_3.estimated_tf_frame_id",
+                    Parameter.Type.STRING,
+                    "",
+                ),
+            ],
+            10.0,
+        )
+
+        # Wait more than the timeout
+        time.sleep(2.0)
+        # Get fresh timestamp
+        stamp = self.node.get_clock().now()
+        self.push_data(stamp)
+
+        self.assertFalse(
+            self.node.can_transform("cam_1", "custom_cam_3_frame_id", stamp),
+            msg="`custom_cam_3_frame_id` frame_id was was published even thought it shouldn't!",
+        )
+        self.assertTrue(
+            self.node.can_transform("cam_1", "cam_3", stamp),
+            msg="`cam_3` frame_id was not published!",
+        )
+
     def set_timeout(self, timeout: float) -> None:
         self.node.set_params(
             [
@@ -236,7 +310,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()
 
@@ -253,7 +327,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
@@ -264,7 +338,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
@@ -274,7 +348,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
@@ -285,7 +359,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)
@@ -303,7 +377,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
@@ -360,16 +434,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")