Added master exposure controller

spinnaker_camera_driver/include/spinnaker_camera_driver/exposure_controller.hpp

@@ -30,6 +30,10 @@ class ExposureController
   virtual ~ExposureController() {}
   virtual void update(Camera * cam, const std::shared_ptr<const Image> & img) = 0;
   virtual void addCamera(const std::shared_ptr<Camera> & cam) = 0;
+  virtual double getExposureTime() = 0;
+  virtual double getGain() = 0;
+  virtual void link(
+    const std::unordered_map<std::string, std::shared_ptr<ExposureController>> & map) = 0;
 };
 }  // namespace spinnaker_camera_driver
 #endif  // SPINNAKER_CAMERA_DRIVER__EXPOSURE_CONTROLLER_HPP_

spinnaker_synchronized_camera_driver/CMakeLists.txt

@@ -60,7 +60,8 @@ add_library(synchronized_camera_driver SHARED
   src/time_estimator.cpp
   src/time_keeper.cpp
   src/exposure_controller_factory.cpp
-  src/individual_exposure_controller.cpp)
+  src/master_exposure_controller.cpp
+  src/follower_exposure_controller.cpp)
 
 ament_target_dependencies(synchronized_camera_driver PUBLIC ${ROS_DEPENDENCIES})
 target_link_libraries(synchronized_camera_driver PUBLIC spinnaker_camera_driver::camera_driver PRIVATE yaml-cpp)
@@ -111,7 +112,7 @@ if(BUILD_TESTING)
   find_package(ament_cmake_copyright REQUIRED)
   find_package(ament_cmake_cppcheck REQUIRED)
   find_package(ament_cmake_cpplint REQUIRED)
-  find_package(ament_cmake_black REQUIRED)
+  find_package(ament_cmake_flake8 REQUIRED)
   find_package(ament_cmake_lint_cmake REQUIRED)
   find_package(ament_cmake_pep257 REQUIRED)
   find_package(ament_cmake_clang_format REQUIRED)
@@ -120,7 +121,7 @@ if(BUILD_TESTING)
   ament_copyright()
   ament_cppcheck(LANGUAGE c++)
   ament_cpplint(FILTERS "-build/include,-runtime/indentation_namespace")
-  ament_black()
+  ament_flake8()
   ament_lint_cmake()
   ament_pep257()
   ament_clang_format(CONFIG_FILE .clang-format)

spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/follower_exposure_controller.hpp

@@ -0,0 +1,61 @@
+// -*-c++-*--------------------------------------------------------------------
+// Copyright 2024 Bernd Pfrommer <bernd.pfrommer@gmail.com>
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__FOLLOWER_EXPOSURE_CONTROLLER_HPP_
+#define SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__FOLLOWER_EXPOSURE_CONTROLLER_HPP_
+
+#include <limits>
+#include <rclcpp/rclcpp.hpp>
+#include <spinnaker_camera_driver/exposure_controller.hpp>
+
+namespace spinnaker_synchronized_camera_driver
+{
+class FollowerExposureController : public spinnaker_camera_driver::ExposureController
+{
+public:
+  using Camera = spinnaker_camera_driver::Camera;
+  explicit FollowerExposureController(const std::string & name, rclcpp::Node * n);
+  void update(
+    Camera * cam, const std::shared_ptr<const spinnaker_camera_driver::Image> & img) final;
+  void addCamera(const std::shared_ptr<Camera> & cam) final;
+  double getExposureTime() final { return (currentExposureTime_); };
+  double getGain() final { return (currentGain_); }
+  void link(const std::unordered_map<std::string, std::shared_ptr<ExposureController>> & map) final;
+
+private:
+  rclcpp::Logger get_logger() { return (rclcpp::get_logger(cameraName_)); }
+
+  template <class T>
+  T declare_param(const std::string & n, const T & def)
+  {
+    return (node_->declare_parameter<T>(name_ + "." + n, def));
+  }
+
+  // ----------------- variables --------------------
+  std::string name_;
+  std::string cameraName_;
+  rclcpp::Node * node_{0};
+  std::string exposureParameterName_;
+  std::string gainParameterName_;
+  std::string masterControllerName_;
+  std::shared_ptr<spinnaker_camera_driver::ExposureController> masterController_;
+
+  double currentExposureTime_{0};
+  double currentGain_{std::numeric_limits<float>::lowest()};
+  int numFramesSkip_{0};
+  int maxFramesSkip_{10};
+};
+}  // namespace spinnaker_synchronized_camera_driver
+#endif  // SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__FOLLOWER_EXPOSURE_CONTROLLER_HPP_

spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/individual_exposure_controller.hpp → spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/master_exposure_controller.hpp

@@ -13,23 +13,26 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__INDIVIDUAL_EXPOSURE_CONTROLLER_HPP_
-#define SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__INDIVIDUAL_EXPOSURE_CONTROLLER_HPP_
+#ifndef SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__MASTER_EXPOSURE_CONTROLLER_HPP_
+#define SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__MASTER_EXPOSURE_CONTROLLER_HPP_
 
 #include <limits>
 #include <rclcpp/rclcpp.hpp>
 #include <spinnaker_camera_driver/exposure_controller.hpp>
 
 namespace spinnaker_synchronized_camera_driver
 {
-class IndividualExposureController : public spinnaker_camera_driver::ExposureController
+class MasterExposureController : public spinnaker_camera_driver::ExposureController
 {
 public:
-  explicit IndividualExposureController(const std::string & name, rclcpp::Node * n);
+  using Camera = spinnaker_camera_driver::Camera;
+  explicit MasterExposureController(const std::string & name, rclcpp::Node * n);
   void update(
-    spinnaker_camera_driver::Camera * cam,
-    const std::shared_ptr<const spinnaker_camera_driver::Image> & img) final;
-  void addCamera(const std::shared_ptr<spinnaker_camera_driver::Camera> & cam) final;
+    Camera * cam, const std::shared_ptr<const spinnaker_camera_driver::Image> & img) final;
+  void addCamera(const std::shared_ptr<Camera> & cam) final;
+  double getExposureTime() final { return (currentExposureTime_); };
+  double getGain() final { return (currentGain_); }
+  void link(const std::unordered_map<std::string, std::shared_ptr<ExposureController>> &) final {}
 
 private:
   double calculateGain(double brightRatio) const;
@@ -68,4 +71,4 @@ class IndividualExposureController : public spinnaker_camera_driver::ExposureCon
   bool gainPriority_{false};
 };
 }  // namespace spinnaker_synchronized_camera_driver
-#endif  // SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__INDIVIDUAL_EXPOSURE_CONTROLLER_HPP_
+#endif  // SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__MASTER_EXPOSURE_CONTROLLER_HPP_

spinnaker_synchronized_camera_driver/launch/follower_example.launch.py

@@ -0,0 +1,151 @@
+# -----------------------------------------------------------------------------
+# Copyright 2024 Bernd Pfrommer <bernd.pfrommer@gmail.com>
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+#
+
+
+#
+# Example file for two Blackfly S cameras that are *externally triggered*, i.e
+# you must provide an external hardware synchronization pulse to both cameras!
+#
+# One of them creates a master controller, the other one a follower. The exposure
+# parameters are determined by the master. This is a useful setup for e.g. a
+# synchronized stereo camera.
+#
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument as LaunchArg
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration as LaunchConfig
+from launch.substitutions import PathJoinSubstitution as PJoin
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
+
+camera_list = {
+    'cam0': '20435008',
+    'cam1': '20415937',
+}
+
+exposure_controller_parameters = {
+    'brightness_target': 120,  # from 0..255
+    'brightness_tolerance': 20,  # when to update exposure/gain
+    # watch that max_exposure_time is short enough
+    # to support the trigger frame rate!
+    'max_exposure_time': 15000,  # usec
+    'min_exposure_time': 5000,  # usec
+    'max_gain': 29.9,
+    'gain_priority': False,
+}
+
+cam_parameters = {
+    'debug': False,
+    'quiet': True,
+    'buffer_queue_size': 1,
+    'compute_brightness': True,
+    'exposure_auto': 'Off',
+    'exposure_time': 10000,  # not used under auto exposure
+    'trigger_mode': 'On',
+    'gain_auto': 'Off',
+    'trigger_source': 'Line3',
+    'trigger_selector': 'FrameStart',
+    'trigger_overlap': 'ReadOut',
+    'trigger_activation': 'RisingEdge',
+    'balance_white_auto': 'Continuous',
+    # You must enable chunk mode and chunks: frame_id, exposure_time, and gain
+    'chunk_mode_active': True,
+    'chunk_selector_frame_id': 'FrameID',
+    'chunk_enable_frame_id': True,
+    'chunk_selector_exposure_time': 'ExposureTime',
+    'chunk_enable_exposure_time': True,
+    'chunk_selector_gain': 'Gain',
+    'chunk_enable_gain': True,
+    # The Timestamp is not used at the moment
+    'chunk_selector_timestamp': 'Timestamp',
+    'chunk_enable_timestamp': True,
+}
+
+
+def make_parameters(context):
+    """Launch synchronized camera driver node."""
+    pd = LaunchConfig('camera_parameter_directory')
+    calib_url = 'file://' + LaunchConfig('calibration_directory').perform(context) + '/'
+
+    exp_ctrl_names = [cam + '.exposure_controller' for cam in camera_list.keys()]
+    driver_parameters = {
+        'cameras': list(camera_list.keys()),
+        'exposure_controllers': exp_ctrl_names,
+        'ffmpeg_image_transport.encoding': 'hevc_nvenc',  # only for ffmpeg image transport
+    }
+    # generate identical exposure controller parameters for all cameras
+    for exp in exp_ctrl_names:
+        driver_parameters.update(
+            {exp + '.' + k: v for k, v in exposure_controller_parameters.items()}
+        )
+    # now set cam0 to be master, cam1 to be follower
+    driver_parameters[exp_ctrl_names[0] + '.type'] = 'master'
+    driver_parameters[exp_ctrl_names[1] + '.type'] = 'follower'
+    # tell camera 1 that the master is (camera 0)
+    driver_parameters[exp_ctrl_names[1] + '.master'] = exp_ctrl_names[0]
+
+    # generate camera parameters
+    cam_parameters['parameter_file'] = PJoin([pd, 'blackfly_s.yaml'])
+    for cam, serial in camera_list.items():
+        cam_params = {cam + '.' + k: v for k, v in cam_parameters.items()}
+        cam_params[cam + '.serial_number'] = serial
+        cam_params[cam + '.camerainfo_url'] = calib_url + serial + '.yaml'
+        cam_params[cam + '.frame_id'] = cam
+        driver_parameters.update(cam_params)  # insert into main parameter list
+        # link the camera to its exposure controller. Each camera has its own controller
+        driver_parameters.update({cam + '.exposure_controller_name': cam + '.exposure_controller'})
+    return driver_parameters
+
+
+def launch_setup(context, *args, **kwargs):
+    container = ComposableNodeContainer(
+        name='cam_sync_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='spinnaker_synchronized_camera_driver',
+                plugin='spinnaker_synchronized_camera_driver::SynchronizedCameraDriver',
+                name='cam_sync',
+                parameters=[make_parameters(context)],
+                extra_arguments=[{'use_intra_process_comms': True}],
+            ),
+        ],
+        output='screen',
+    )  # end of container
+    return [container]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            LaunchArg(
+                'camera_parameter_directory',
+                default_value=PJoin([FindPackageShare('spinnaker_camera_driver'), 'config']),
+                description='root directory for camera parameter definitions',
+            ),
+            LaunchArg(
+                'calibration_directory',
+                default_value=['camera_calibrations'],
+                description='root directory for camera calibration files',
+            ),
+            OpaqueFunction(function=launch_setup),
+        ]
+    )