Merge pull request #40 from open-dynamic-robot-initiative/fkloss/sens…

…or_info

Implement get_sensor_info() for Pylon (single and tri) and PyBullet drivers

CHANGELOG.md

@@ -15,6 +15,11 @@
   [documentation](https://open-dynamic-robot-initiative.github.io/trifinger_cameras/doc/configuration.html)
 - Executable `tricamera_monitor_rate` for checking the actual frame rate (most meant for
   debugging and testing during development)
+- Executables `single_camera_backend` and `tricamera_backend` to run camera back ends in
+  a separate process (mostly for testing).  Also added `--multi-process` option to
+  `demo_camera.py` accordingly.
+- Implement `get_sensor_info()` for Pylon and PyBullet drivers.  See `demo_camera` and
+  `demo_tricamera` on how to use it.
 
 ### Removed
 - Obsolete script `verify_calibration.py`

CMakeLists.txt

@@ -7,6 +7,12 @@ set(CMAKE_CXX_STANDARD_REQUIRED on)
 # libraries need to be position independent for building Python modules
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
+# generate compile_commands.json by default (needed for LSP)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# stop build on first error
+string(APPEND CMAKE_CXX_FLAGS " -Wfatal-errors -Werror=return-type")
+
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake)
 
 # pybind11 needs to be first, otherwise other packages which also search for
@@ -78,7 +84,10 @@ target_link_libraries(camera_observations
 list(APPEND install_targets camera_observations)
 
 
-add_library(opencv_driver src/opencv_driver.cpp)
+add_library(opencv_driver
+    src/opencv_driver.cpp
+    src/camera_parameters.cpp
+)
 target_include_directories(opencv_driver PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
@@ -87,13 +96,17 @@ target_include_directories(opencv_driver PUBLIC
 target_link_libraries(opencv_driver
     ${OpenCV_LIBRARIES}
     robot_interfaces::robot_interfaces
+    serialization_utils::serialization_utils
     camera_observations
 )
 list(APPEND install_targets opencv_driver)
 
 
 if (${Pylon_FOUND})
-    add_library(pylon_driver src/pylon_driver.cpp)
+    add_library(pylon_driver
+        src/pylon_driver.cpp
+        src/camera_parameters.cpp
+    )
     target_include_directories(pylon_driver PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
@@ -105,6 +118,8 @@ if (${Pylon_FOUND})
         ${Pylon_LIBRARIES}
         fmt::fmt
         robot_interfaces::robot_interfaces
+        serialization_utils::serialization_utils
+        camera_calibration_parser
         camera_observations
         settings
     )
@@ -199,7 +214,10 @@ target_include_directories(camera_calibration_parser PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(camera_calibration_parser yaml_utils::yaml_utils)
+target_link_libraries(camera_calibration_parser
+    serialization_utils::serialization_utils
+    yaml_utils::yaml_utils
+)
 ament_target_dependencies(camera_calibration_parser
     sensor_msgs
 )
@@ -263,11 +281,12 @@ install_scripts(
     scripts/overlay_camera_stream.py
     scripts/overlay_real_and_rendered_images.py
     scripts/record_image_dataset.py
+    scripts/single_camera_backend.py
+    scripts/tricamera_backend.py
     scripts/tricamera_log_converter.py
     scripts/tricamera_log_viewer.py
     scripts/tricamera_monitor_rate.py
     scripts/tricamera_test_connection.py
-
     DESTINATION lib/${PROJECT_NAME}
 )
 

demos/demo_camera.py

@@ -6,16 +6,19 @@
 Basically illustrates what objects to create to interact with the
 camera, and the available methods for that.
 """
+
 import argparse
+import pathlib
 import sys
 
 import cv2
+import numpy as np
 
 import trifinger_cameras
 from trifinger_cameras import utils
 
 
-def main() -> int:
+def main() -> int:  # noqa: D103
     argparser = argparse.ArgumentParser(description=__doc__)
     argparser.add_argument(
         "--pylon",
@@ -30,6 +33,20 @@ def main() -> int:
             to the DeviceUserId, otherwise it is the index of the device.
         """,
     )
+    argparser.add_argument(
+        "--camera-info",
+        type=pathlib.Path,
+        metavar="<file>",
+        dest="camera_info_file",
+        help="Path to YAML file with camera calibration data.",
+    )
+    argparser.add_argument(
+        "--multi-process",
+        action="store_true",
+        help="""If set, run only front end with multi-process robot data.  Otherwise run
+            everything within a single process.
+        """,
+    )
     argparser.add_argument(
         "--record",
         type=str,
@@ -38,31 +55,54 @@ def main() -> int:
 
     args = argparser.parse_args()
 
-    camera_data = trifinger_cameras.camera.SingleProcessData()
-    # camera_data = trifinger_cameras.camera.MultiProcessData("cam", True, 10)
+    if args.multi_process:
+        # In multi-process case assume that the backend is running in a
+        # separate process and only set up the frontend here.
+        camera_data = trifinger_cameras.camera.MultiProcessData("camera", False)
+    else:
+        camera_data = trifinger_cameras.camera.SingleProcessData()
+
+        try:
+            if args.pylon:
+                if args.camera_info_file:
+                    camera_driver = trifinger_cameras.camera.PylonDriver(
+                        args.camera_info_file
+                    )
+                else:
+                    camera_driver = trifinger_cameras.camera.PylonDriver(args.camera_id)
+            else:
+                camera_id = int(args.camera_id) if args.camera_id else 0
+                camera_driver = trifinger_cameras.camera.OpenCVDriver(camera_id)
+        except Exception as e:
+            print("Failed to initialise driver:", e)
+            return 1
+
+        camera_backend = trifinger_cameras.camera.Backend(  # noqa: F841
+            camera_driver, camera_data
+        )
 
-    try:
-        if args.pylon:
-            camera_driver = trifinger_cameras.camera.PylonDriver(args.camera_id)
-        else:
-            camera_id = int(args.camera_id) if args.camera_id else 0
-            camera_driver = trifinger_cameras.camera.OpenCVDriver(camera_id)
-    except Exception as e:
-        print("Failed to initialise driver:", e)
-        return 1
-
-    camera_backend = trifinger_cameras.camera.Backend(  # noqa: F841
-        camera_driver, camera_data
-    )
     camera_frontend = trifinger_cameras.camera.Frontend(camera_data)
 
     if args.record:
         logger = trifinger_cameras.camera.Logger(camera_data, 10000)
         logger.start()
 
+    np.set_printoptions(precision=3, suppress=True)
+    print("--- Camera Info: ----------------------")
+    sinfo = camera_frontend.get_sensor_info()
+    print(f"fps: {sinfo.frame_rate_fps}")
+    print(f"width x height: {sinfo.image_width}x{sinfo.image_height}")
+    print(sinfo.camera_matrix)
+    print(sinfo.distortion_coefficients)
+    print(sinfo.tf_world_to_camera)
+    print("---------------------------------------")
+
     while True:
         observation = camera_frontend.get_latest_observation()
-        image = utils.convert_image(observation.image)
+        if args.pylon:
+            image = utils.convert_image(observation.image)
+        else:
+            image = observation.image
         window_name = "Image Stream"
         cv2.imshow(window_name, image)
 

demos/demo_tricamera.py

@@ -6,9 +6,11 @@
 """
 
 import argparse
-import cv2
 import pickle
 
+import cv2
+import numpy as np
+
 import trifinger_cameras
 from trifinger_cameras import utils
 
@@ -46,6 +48,18 @@ def main() -> None:  # noqa: D103
     camera_frontend = trifinger_cameras.tricamera.Frontend(camera_data)
     observations_timestamps_list = []
 
+    np.set_printoptions(precision=3, suppress=True)
+    print("=== Camera Info: ======================")
+    for i, info in enumerate(camera_frontend.get_sensor_info().camera):
+        print(f"--- Camera {i}: ----------------------")
+        print(f"fps: {info.frame_rate_fps}")
+        print(f"width x height: {info.image_width}x{info.image_height}")
+        print(info.camera_matrix)
+        print(info.distortion_coefficients)
+        print(info.tf_world_to_camera)
+        print("---------------------------------------")
+    print("=======================================")
+
     while True:
         observation = camera_frontend.get_latest_observation()
         for i, name in enumerate(camera_names):

doc/executables.rst

@@ -142,3 +142,11 @@ Usage (saving the settings to a file "camera_settings.txt":
 .. code-block:: sh
 
    $ pylon_dump_camera_settings "device_user_id" > camera_settings.txt
+
+
+single_camera_backend / tricamera_backend
+=========================================
+
+Run a back end instance for a single camera / the TriCamera setup.  Multi-process sensor
+data is used so other processes can connect with a front end to acquire the images (see
+`demo_camera` / `demo_tricamera`).

include/trifinger_cameras/camera_parameters.hpp

@@ -5,8 +5,12 @@
  */
 #pragma once
 
-#include <Eigen/Eigen>
 #include <ostream>
+#include <string>
+
+#include <Eigen/Eigen>
+
+#include <serialization_utils/cereal_eigen.hpp>
 
 namespace trifinger_cameras
 {
@@ -19,8 +23,48 @@ struct CameraParameters
     Eigen::Matrix<double, 1, 5> distortion_coefficients;
 
     Eigen::Matrix4d tf_world_to_camera;
+
+    template <class Archive>
+    void serialize(Archive& archive)
+    {
+        archive(CEREAL_NVP(image_width),
+                CEREAL_NVP(image_height),
+                CEREAL_NVP(camera_matrix),
+                CEREAL_NVP(distortion_coefficients),
+                CEREAL_NVP(tf_world_to_camera));
+    }
 };
 
 std::ostream& operator<<(std::ostream& os, const CameraParameters& cp);
 
+struct CameraInfo : public CameraParameters
+{
+    float frame_rate_fps;
+
+    template <class Archive>
+    void serialize(Archive& archive)
+    {
+        CameraParameters::serialize(archive);
+        archive(CEREAL_NVP(frame_rate_fps));
+    }
+};
+
+struct TriCameraInfo
+{
+    std::array<CameraInfo, 3> camera;
+
+    TriCameraInfo() = default;
+
+    TriCameraInfo(CameraInfo camera1, CameraInfo camera2, CameraInfo camera3)
+        : camera{camera1, camera2, camera3}
+    {
+    }
+
+    template <class Archive>
+    void serialize(Archive& archive)
+    {
+        archive(CEREAL_NVP(camera));
+    }
+};
+
 }  // namespace trifinger_cameras

include/trifinger_cameras/opencv_driver.hpp

@@ -8,13 +8,15 @@
 
 #include <robot_interfaces/sensors/sensor_driver.hpp>
 #include <trifinger_cameras/camera_observation.hpp>
+#include <trifinger_cameras/camera_parameters.hpp>
 
 namespace trifinger_cameras
 {
 /**
  * @brief Driver for interacting with any camera using OpenCV.
  */
-class OpenCVDriver : public robot_interfaces::SensorDriver<CameraObservation>
+class OpenCVDriver
+    : public robot_interfaces::SensorDriver<CameraObservation, CameraInfo>
 {
 public:
     OpenCVDriver(int device_id);

include/trifinger_cameras/pybullet_tricamera_driver.hpp

@@ -10,6 +10,7 @@
 
 #include <robot_interfaces/finger_types.hpp>
 #include <robot_interfaces/sensors/sensor_driver.hpp>
+#include <trifinger_cameras/camera_parameters.hpp>
 #include <trifinger_cameras/settings.hpp>
 #include <trifinger_cameras/tricamera_observation.hpp>
 
@@ -18,20 +19,30 @@ namespace trifinger_cameras
 /**
  * @brief Driver to get rendered camera images from pyBullet.
  */
-class PyBulletTriCameraDriver : public robot_interfaces::SensorDriver<
-                                    trifinger_cameras::TriCameraObservation>
+class PyBulletTriCameraDriver
+    : public robot_interfaces::SensorDriver<TriCameraObservation, TriCameraInfo>
 {
 public:
     PyBulletTriCameraDriver(
         robot_interfaces::TriFingerTypes::BaseDataPtr robot_data,
         bool render_images = true,
         Settings settings = Settings());
 
+    /**
+     * @brief Get the camera parameters (image size and calibration
+     * coefficients).
+     *
+     * **Important:**  The calibration coefficients are only set if the driver
+     * is initialized with a calibration file (see constructor).  Otherwise,
+     * they will be empty.
+     */
+    virtual TriCameraInfo get_sensor_info() override;
+
     /**
      * @brief Get the latest observation from the three cameras
      * @return TricameraObservation
      */
-    trifinger_cameras::TriCameraObservation get_observation();
+    virtual trifinger_cameras::TriCameraObservation get_observation() override;
 
 private:
     //! @brief Python object to access cameras in pyBullet.
@@ -52,6 +63,9 @@ class PyBulletTriCameraDriver : public robot_interfaces::SensorDriver<
 
     //! Number of robot time steps after which the next frame should be fetched.
     int frame_rate_in_robot_steps_;
+
+    //! Sensor info for the cameras.
+    TriCameraInfo sensor_info_ = {};
 };
 
 }  // namespace trifinger_cameras