Use new capabilities of image_publisher

README.md

@@ -27,7 +27,7 @@ colcon build --symlink-install
 ## Launch
 
 > [!NOTE]
-> Intrinsic parameters of the camera are approximate in the demos and may cause inaccurate results! You can change them by modifying the `k_matrix` param in [cosypose_params.yaml](./happypose_examples/config/cosypose_params.yaml) file.
+> Default intrinsic parameters in demos are based on those from YCBV dataset. In you are running demos on a webcam with unknown intrinsics parameters you can set `field_of_view` that will approximate them for you. Another option is calibrate camera with [camera_calibration](https://docs.ros.org/en/rolling/p/camera_calibration/) ROS package. Path to resulting YAML with camera parameters can be passed with parameter `camera_info_url`. Example of such file can be found in [camera_info.yaml](./happypose_examples/config/camera_info.yaml) file.
 
 > [!TIP]
 > When running the demos, make sure to change `device` parameter according to your hardware configuration!

happypose_examples/config/camera_info.yaml

@@ -17,4 +17,4 @@ 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/launch/multi_view_demo.launch.py

@@ -15,6 +15,15 @@
 def launch_setup(
     context: LaunchContext, *args, **kwargs
 ) -> list[LaunchDescriptionEntity]:
+    # Obtain value for FoV
+    field_of_view = LaunchConfiguration("field_of_view")
+    # Obtain URL of camera calibration. If FoV is non zero overwrite the calibration
+    camera_info_url = (
+        ""
+        if float(field_of_view.perform(context)) > 0.0
+        else LaunchConfiguration("camera_info_url")
+    )
+
     # Register image publishers in loop
     image_publishers = []
     for i in range(1, 4):
@@ -33,9 +42,8 @@ def launch_setup(
                         "publish_rate": 10.0,
                         "frame_id": f"camera_{i}",
                         "filename": image_path,
-                        # Camera info is ignored by the node on startup.
-                        # Waiting for https://github.com/ros-perception/image_pipeline/issues/965
-                        "camera_info_url": "package://happypose_examples/config/camera_info.yaml",
+                        "field_of_view": field_of_view,
+                        "camera_info_url": camera_info_url,
                     }
                 ],
                 remappings=[
@@ -137,6 +145,18 @@ def generate_launch_description():
             description="Path to the third image to be published "
             + "as an input for happypose_ros node.",
         ),
+        DeclareLaunchArgument(
+            "field_of_view",
+            default_value="0.0",
+            description="Field of view of the camera taking images "
+            + "used to approximate intrinsic parameters. "
+            + "Overwritten by param `camera_info_url`",
+        ),
+        DeclareLaunchArgument(
+            "camera_info_url",
+            default_value="package://happypose_examples/config/camera_info.yaml",
+            description="URL of the calibrated camera params. Overwrites param `field_of_view`.",
+        ),
         DeclareLaunchArgument(
             "use_rviz",
             default_value="false",

happypose_examples/launch/single_view_demo.launch.py

@@ -17,6 +17,14 @@ def launch_setup(
 ) -> list[LaunchDescriptionEntity]:
     # Obtain argument value for image path
     image_path = LaunchConfiguration("image_path")
+    # Obtain value for FoV
+    field_of_view = LaunchConfiguration("field_of_view")
+    # Obtain URL of camera calibration. If FoV is non zero overwrite the calibration
+    camera_info_url = (
+        ""
+        if float(field_of_view.perform(context)) > 0.0
+        else LaunchConfiguration("camera_info_url")
+    )
 
     # Start ROS node for image publishing
     image_publisher_node = Node(
@@ -30,9 +38,8 @@ def launch_setup(
                 "publish_rate": 23.0,
                 "frame_id": "camera_1",
                 "filename": image_path,
-                # Camera info is ignored by the node on startup.
-                # Waiting for https://github.com/ros-perception/image_pipeline/issues/965
-                "camera_info_url": "package://happypose_examples/config/camera_info.yaml",
+                "field_of_view": field_of_view,
+                "camera_info_url": camera_info_url,
             }
         ],
         remappings=[
@@ -88,6 +95,18 @@ def generate_launch_description():
             ),
             description="Path to image or webcam to be published as an input for happypose_ros node.",
         ),
+        DeclareLaunchArgument(
+            "field_of_view",
+            default_value="0.0",
+            description="Field of view of the camera taking images "
+            + "used to approximate intrinsic parameters. "
+            + "Overwrites `camera_info_url` parameter",
+        ),
+        DeclareLaunchArgument(
+            "camera_info_url",
+            default_value="package://happypose_examples/config/camera_info.yaml",
+            description="URL of the calibrated camera params. Is overwritten by param `field_of_view`.",
+        ),
         DeclareLaunchArgument(
             "use_rviz",
             default_value="false",