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

…or_info

Implement `get_sensor_info()` for drivers and refactor to reduce redundancy

CHANGELOG.md

@@ -8,11 +8,19 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Add method `get_debug_image` to `TriCameraObjectTrackerDriver` and a script
   that uses it to create debug images from the cameras (this makes diagnosing
   issues with the object detection easier).
+- Support `trifinger_cameras::Settings` for changing camera configuration (see
+  documentation of `trifinger_cameras`).
+- Provide information about frame rate and camera parameters via `get_sensor_info()`
+  like in `trifinger_cameras`.  For this, overloaded constructors are added to the
+  driver classes which accept camera info files instead of device IDs.
+- Add a "backend-only" mode to `demo_cameras`.  Use this if the front end is running in
+  a separate process.
 
 ### Removed
 - The `ProgramOptions` class has been moved to its own package
   ([cli_utils](https://github.com/MPI-IS/cli_utils)).  Thus it is removed from
   this package and the one from cli_utils is used instead.
+- Option `--multi-process` from `demo_cameras`.  Use `--frontend-only` instead.
 
 ### Fixed
 - pybind11 build error on Ubuntu 22.04

CMakeLists.txt

@@ -9,6 +9,9 @@ set(CMAKE_CXX_STANDARD_REQUIRED on)
 # fails
 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 " -Wall -Wextra -Wfatal-errors -Werror=return-type")
 
@@ -138,7 +141,7 @@ if (${HAS_PYLON_DRIVERS})
     )
     target_link_libraries(tricamera_object_tracking_driver
         robot_interfaces::robot_interfaces
-        trifinger_cameras::pylon_driver
+        trifinger_cameras::tricamera_driver
         cube_detector
     )
 
@@ -160,6 +163,7 @@ target_link_libraries(pybullet_tricamera_object_tracker_driver
     pybind11_opencv::pybind11_opencv
     robot_interfaces::robot_interfaces
     serialization_utils::serialization_utils
+    trifinger_cameras::pybullet_tricamera_driver
     cube_detector
 )
 # using pybind11 types, therefore visibility needs to be hidden

demos/demo_cameras.py

@@ -1,58 +1,48 @@
 #!/usr/bin/env python3
-"""
-Demo on how to access observations from the TriCameraObjectTrackerDriver.
-"""
+"""Demo on how to access observations from the TriCameraObjectTrackerDriver."""
+
 import argparse
+import pathlib
+import sys
 import time
 
 import cv2
 import numpy as np
 
-import trifinger_object_tracking.py_object_tracker
+import signal_handler
+import trifinger_object_tracking.py_object_tracker as object_tracker
 import trifinger_object_tracking.py_tricamera_types as tricamera
 from trifinger_cameras import utils
 
 
-def main():
-    argparser = argparse.ArgumentParser(description=__doc__)
-    argparser.add_argument(
-        "--multi-process",
-        action="store_true",
-        help="""If set, use multiprocess camera data to access backend in other
-            process.  Otherwise run the backend locally.
-        """,
-    )
-    argparser.add_argument(
-        "--object",
-        type=str,
-        default="cube_v2",
-        help="Name of the model used for object detection.  Default: %(default)s.",
-    )
-    args = argparser.parse_args()
+camera_names = ["camera60", "camera180", "camera300"]
+calib_files = [
+    pathlib.Path("/etc/trifingerpro/camera60_cropped_and_downsampled.yml"),
+    pathlib.Path("/etc/trifingerpro/camera180_cropped_and_downsampled.yml"),
+    pathlib.Path("/etc/trifingerpro/camera300_cropped_and_downsampled.yml"),
+]
 
-    camera_names = ["camera60", "camera180", "camera300"]
 
-    model = trifinger_object_tracking.py_object_tracker.get_model_by_name(
-        args.object
-    )
+def back_end_only_loop() -> None:
+    """Idle loop for the back-end-only mode."""
+    print("Backend is running.  Press Ctrl-C to stop.")
+    signal_handler.init()
+    while not signal_handler.has_received_sigint():
+        time.sleep(1)
 
-    if args.multi_process:
-        camera_data = tricamera.MultiProcessData("tricamera", False)
-    else:
-        camera_data = tricamera.SingleProcessData()
-        camera_driver = tricamera.TriCameraObjectTrackerDriver(
-            *camera_names, cube_model=model
-        )
-        camera_backend = tricamera.Backend(camera_driver, camera_data)
 
+def front_end_loop(
+    camera_data: tricamera.BaseData, model: object_tracker.BaseCuboidModel
+) -> None:
+    """Run the front end to display camera images with visualized object pose."""
     camera_frontend = tricamera.Frontend(camera_data)
 
-    calib_files = [
-        "/etc/trifingerpro/camera60_cropped_and_downsampled.yml",
-        "/etc/trifingerpro/camera180_cropped_and_downsampled.yml",
-        "/etc/trifingerpro/camera300_cropped_and_downsampled.yml",
-    ]
-    cube_visualizer = tricamera.CubeVisualizer(model, calib_files)
+    cube_visualizer = tricamera.CubeVisualizer(
+        model, [str(f) for f in calib_files]
+    )
+
+    print("=== Camera Info: ===")
+    utils.print_tricamera_sensor_info(camera_frontend.get_sensor_info())
 
     last_print = time.time()
     while True:
@@ -81,9 +71,71 @@ def main():
         if cv2.waitKey(3) in [ord("q"), 27]:  # 27 = ESC
             break
 
-    if not args.multi_process:
+
+def main() -> int:  # noqa: D103
+    parser = argparse.ArgumentParser(description=__doc__)
+
+    mode_group = parser.add_mutually_exclusive_group()
+    mode_group.add_argument(
+        "--frontend-only",
+        action="store_const",
+        dest="mode",
+        const="frontend-only",
+        help="""Run only the front end, using multi-process camera data.  In this case,
+            the back end has to be run in a separate process.
+        """,
+    )
+    mode_group.add_argument(
+        "--backend-only",
+        action="store_const",
+        dest="mode",
+        const="backend-only",
+        help="""Run only the back end, using multi-process camera data.  In this case,
+            the front end has to be run in a separate process.
+        """,
+    )
+    mode_group.add_argument(
+        "--single-process",
+        action="store_const",
+        dest="mode",
+        const="single-process",
+        help="Run both front and back end, using single-process camera data.",
+    )
+    mode_group.set_defaults(mode="single-process")
+
+    parser.add_argument(
+        "--object",
+        type=str,
+        default="cube_v2",
+        help="Name of the model used for object detection.  Default: %(default)s.",
+    )
+    args = parser.parse_args()
+
+    model = object_tracker.get_model_by_name(args.object)
+
+    if args.mode == "single-process":
+        camera_data = tricamera.SingleProcessData()
+    else:
+        camera_data = tricamera.MultiProcessData("tricamera", False)
+
+    if args.mode != "frontend-only":
+        camera_driver = tricamera.TriCameraObjectTrackerDriver(
+            *calib_files, cube_model=model
+        )
+        camera_backend = tricamera.Backend(camera_driver, camera_data)
+    else:
+        camera_backend = None
+
+    if args.mode == "backend-only":
+        back_end_only_loop()
+    else:
+        front_end_loop(camera_data, model)
+
+    if camera_backend is not None:
         camera_backend.shutdown()
 
+    return 0
+
 
 if __name__ == "__main__":
-    main()
+    sys.exit(main())

include/trifinger_object_tracking/pybullet_tricamera_object_tracker_driver.hpp

@@ -11,6 +11,9 @@
 
 #include <robot_interfaces/finger_types.hpp>
 #include <robot_interfaces/sensors/sensor_driver.hpp>
+#include <trifinger_cameras/camera_parameters.hpp>
+#include <trifinger_cameras/pybullet_tricamera_driver.hpp>
+
 #include <trifinger_object_tracking/tricamera_object_observation.hpp>
 
 namespace trifinger_object_tracking
@@ -22,7 +25,8 @@ namespace trifinger_object_tracking
  */
 class PyBulletTriCameraObjectTrackerDriver
     : public robot_interfaces::SensorDriver<
-          trifinger_object_tracking::TriCameraObjectObservation>
+          trifinger_object_tracking::TriCameraObjectObservation,
+          trifinger_cameras::TriCameraInfo>
 {
 public:
     // For some reason the constructor needs to be in the header file to avoid a
@@ -41,52 +45,30 @@ class PyBulletTriCameraObjectTrackerDriver
     PyBulletTriCameraObjectTrackerDriver(
         pybind11::object tracking_object,
         robot_interfaces::TriFingerTypes::BaseDataPtr robot_data,
-        bool render_images = true)
-        : tracking_object_(tracking_object),
-          robot_data_(robot_data),
-          render_images_(render_images),
-          last_update_robot_time_index_(0)
-    {
-        // initialize Python interpreter if not already done
-        if (!Py_IsInitialized())
-        {
-            pybind11::initialize_interpreter();
-        }
-
-        pybind11::gil_scoped_acquire acquire;
-
-        if (render_images)
-        {
-            // some imports that are needed later for converting images (numpy
-            // array -> cv::Mat)
-            numpy_ = pybind11::module::import("numpy");
+        bool render_images = true,
+        trifinger_cameras::Settings settings = trifinger_cameras::Settings());
 
-            // TriFingerCameras gives access to the cameras in simulation
-            pybind11::module mod_camera =
-                pybind11::module::import("trifinger_simulation.camera");
-            cameras_ = mod_camera.attr("TriFingerCameras")();
-        }
-    }
+    /**
+     * @brief Get the camera parameters.
+     *
+     * @verbatim embed:rst:leading-asterisk
+     * Internally, this calls
+     * :cpp:func:`trifinger_cameras::PyBulletTriCameraDriver::get_sensor_info`,
+     * so see there fore more information.
+     * @endverbatim
+     */
+    trifinger_cameras::TriCameraInfo get_sensor_info() override;
 
     //! @brief Get the latest observation.
-    trifinger_object_tracking::TriCameraObjectObservation get_observation();
+    trifinger_object_tracking::TriCameraObjectObservation get_observation()
+        override;
 
 private:
-    //! @brief Python object to access cameras in pyBullet.
-    pybind11::object cameras_;
+    //! @brief PyBullet driver instance for rendering images.
+    trifinger_cameras::PyBulletTriCameraDriver camera_driver_;
 
     //! @brief Python object of the tracked object.
     pybind11::object tracking_object_;
-
-    robot_interfaces::TriFingerTypes::BaseDataPtr robot_data_;
-
-    //! @brief If false, no actual images are rendered.
-    bool render_images_;
-
-    //! @brief The numpy module.
-    pybind11::module numpy_;
-
-    time_series::Index last_update_robot_time_index_;
 };
 
 }  // namespace trifinger_object_tracking

include/trifinger_object_tracking/tricamera_object_observation.hpp

@@ -7,7 +7,7 @@
 
 #include <array>
 
-#include <trifinger_cameras/camera_observation.hpp>
+#include <trifinger_cameras/tricamera_observation.hpp>
 #include <trifinger_object_tracking/object_pose.hpp>
 
 namespace trifinger_object_tracking
@@ -16,16 +16,24 @@ namespace trifinger_object_tracking
  * @brief Observation of three cameras + object tracking
  */
 struct TriCameraObjectObservation
+    : public trifinger_cameras::TriCameraObservation
 {
-    std::array<trifinger_cameras::CameraObservation, 3> cameras;
+    trifinger_object_tracking::ObjectPose object_pose = {};
+    trifinger_object_tracking::ObjectPose filtered_object_pose = {};
 
-    trifinger_object_tracking::ObjectPose object_pose;
-    trifinger_object_tracking::ObjectPose filtered_object_pose;
+    TriCameraObjectObservation() = default;
+
+    TriCameraObjectObservation(
+        const trifinger_cameras::TriCameraObservation& observation)
+        : trifinger_cameras::TriCameraObservation(observation)
+    {
+    }
 
     template <class Archive>
     void serialize(Archive& archive)
     {
-        archive(cameras, object_pose, filtered_object_pose);
+        trifinger_cameras::TriCameraObservation::serialize(archive);
+        archive(object_pose, filtered_object_pose);
     }
 };