From d2c88c9b8dce9fdbd235e98a39b70c1a965fb563 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Thu, 30 May 2024 13:43:39 -0400 Subject: [PATCH 1/2] renamed stereo_synced file and added doc --- spinnaker_camera_driver/doc/index.rst | 14 ++ .../launch/multiple_cameras.launch.py | 131 ++++++++++++++++ .../launch/stereo_synced.launch.py | 145 ------------------ 3 files changed, 145 insertions(+), 145 deletions(-) create mode 100755 spinnaker_camera_driver/launch/multiple_cameras.launch.py delete mode 100755 spinnaker_camera_driver/launch/stereo_synced.launch.py diff --git a/spinnaker_camera_driver/doc/index.rst b/spinnaker_camera_driver/doc/index.rst index a1816dfd..93248372 100644 --- a/spinnaker_camera_driver/doc/index.rst +++ b/spinnaker_camera_driver/doc/index.rst @@ -184,6 +184,20 @@ that you can customize as needed. ros2 launch spinnaker_camera_driver driver_node.launch.py camera_type:=blackfly_s serial:="'20435008'" +Using multiple cameras at the same time +======================================= + +The FLIR Spinnaker does not support two programs accessing the Spinnaker SDK at the same time, +even if two different cameras are accessed. Strange things happen, in particular with USB3 cameras. +You can however run multiple cameras in the same +address space using ROS2's Composable Node concept, see the example +launch file ``multiple_cameras.launch.py``. + +If you are using hardware synchronized cameras please use the ``spinnaker_synchronized_camera_driver``, +which will associate the images triggered by the same sync pulse with each other +and give them identical time stamps. + + About time stamps ================= diff --git a/spinnaker_camera_driver/launch/multiple_cameras.launch.py b/spinnaker_camera_driver/launch/multiple_cameras.launch.py new file mode 100755 index 00000000..742cd5b8 --- /dev/null +++ b/spinnaker_camera_driver/launch/multiple_cameras.launch.py @@ -0,0 +1,131 @@ +# ----------------------------------------------------------------------------- +# Copyright 2022 Bernd Pfrommer +# +# 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. +# +# + +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 +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare + +camera_params = { + 'debug': False, + 'compute_brightness': True, + 'dump_node_map': False, + 'adjust_timestamp': True, + 'gain_auto': 'Off', + 'gain': 0, + 'exposure_auto': 'Off', + 'exposure_time': 9000, + 'line2_selector': 'Line2', + 'line2_v33enable': False, + 'line3_selector': 'Line3', + 'line3_linemode': 'Input', + 'trigger_selector': 'FrameStart', + 'trigger_mode': 'On', + 'trigger_source': 'Line3', + 'trigger_delay': 9, + 'trigger_overlap': 'ReadOut', + '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, + 'chunk_selector_timestamp': 'Timestamp', + 'chunk_enable_timestamp': True, +} + + +def make_camera_node(name, camera_type, serial): + parameter_file = PathJoinSubstitution( + [FindPackageShare('spinnaker_camera_driver'), 'config', camera_type + '.yaml'] + ) + + node = ComposableNode( + package='spinnaker_camera_driver', + plugin='spinnaker_camera_driver::CameraDriver', + name=name, + parameters=[camera_params, {'parameter_file': parameter_file, 'serial_number': serial}], + remappings=[ + ('~/control', '/exposure_control/control'), + ], + extra_arguments=[{'use_intra_process_comms': True}], + ) + return node + + +def launch_setup(context, *args, **kwargs): + """Create multiple camera.""" + container = ComposableNodeContainer( + name='stereo_camera_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # + # These two camera nodes run independently from each other, + # but in the same address space + # + make_camera_node( + LaunchConfig('cam_0_name'), + LaunchConfig('cam_0_type').perform(context), + LaunchConfig('cam_0_serial'), + ), + make_camera_node( + LaunchConfig('cam_1_name'), + LaunchConfig('cam_1_type').perform(context), + LaunchConfig('cam_1_serial'), + ), + ], + output='screen', + ) # end of container + return [container] + + +def generate_launch_description(): + """Create composable node by calling opaque function.""" + return LaunchDescription( + [ + LaunchArg( + 'cam_0_name', + default_value=['cam_0'], + description='camera name (ros node name) of camera 0', + ), + LaunchArg( + 'cam_1_name', + default_value=['cam_1'], + description='camera name (ros node name) of camera 1', + ), + LaunchArg('cam_0_type', default_value='blackfly_s', description='type of camera 0'), + LaunchArg('cam_1_type', default_value='blackfly_s', description='type of camera 1'), + LaunchArg( + 'cam_0_serial', + default_value="'20435008'", + description='FLIR serial number of camera 0 (in quotes!!)', + ), + LaunchArg( + 'cam_1_serial', + default_value="'20415937'", + description='FLIR serial number of camera 1 (in quotes!!)', + ), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/spinnaker_camera_driver/launch/stereo_synced.launch.py b/spinnaker_camera_driver/launch/stereo_synced.launch.py deleted file mode 100755 index a593c9ef..00000000 --- a/spinnaker_camera_driver/launch/stereo_synced.launch.py +++ /dev/null @@ -1,145 +0,0 @@ -# ----------------------------------------------------------------------------- -# Copyright 2022 Bernd Pfrommer -# -# 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. -# -# - -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 - -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare - - -camera_params = { - 'debug': False, - 'compute_brightness': True, - 'dump_node_map': False, - 'adjust_timestamp': True, - 'gain_auto': 'Off', - 'gain': 0, - 'exposure_auto': 'Off', - 'exposure_time': 9000, - 'line2_selector': 'Line2', - 'line2_v33enable': False, - 'line3_selector': 'Line3', - 'line3_linemode': 'Input', - 'trigger_selector': 'FrameStart', - 'trigger_mode': 'On', - 'trigger_source': 'Line3', - 'trigger_delay': 9, - 'trigger_overlap': 'ReadOut', - '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, - 'chunk_selector_timestamp': 'Timestamp', - 'chunk_enable_timestamp': True, - } - - -def make_camera_node(name, camera_type, serial): - parameter_file = PathJoinSubstitution( - [FindPackageShare('spinnaker_camera_driver'), 'config', - camera_type + '.yaml']) - - node = ComposableNode( - package='spinnaker_camera_driver', - plugin='spinnaker_camera_driver::CameraDriver', - name=name, - parameters=[camera_params, - {'parameter_file': parameter_file, - 'serial_number': serial}], - remappings=[('~/control', '/exposure_control/control'), ], - extra_arguments=[{'use_intra_process_comms': True}]) - return node - - -def launch_setup(context, *args, **kwargs): - """Create synchronized stereo camera.""" - container = ComposableNodeContainer( - name='stereo_camera_container', - namespace='', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - # - # These two camera nodes run independently from each other, - # but in the same address space - # - make_camera_node(LaunchConfig('cam_0_name'), - LaunchConfig('cam_0_type').perform(context), - LaunchConfig('cam_0_serial')), - make_camera_node(LaunchConfig('cam_1_name'), - LaunchConfig('cam_1_type').perform(context), - LaunchConfig('cam_1_serial')), - # - # This node forces the ros header stamps to be identical - # across the two cameras. Remove if you don't need it. - # - ComposableNode( - package='cam_sync_ros2', - plugin='cam_sync_ros2::CamSync', - name='sync', - parameters=[], - extra_arguments=[{'use_intra_process_comms': True}], - ), - # - # This node is for external exposure control. Remove - # if you don't need it, and switch on auto exposure. - # - ComposableNode( - package='exposure_control_ros2', - plugin='exposure_control_ros2::ExposureControl', - name='exposure_control', - parameters=[{'cam_name': LaunchConfig('cam_0_name'), - 'max_gain': 20.0, - 'gain_priority': False, - 'brightness_target': 100, - 'max_exposure_time': 9500.0, - 'min_exposure_time': 1000.0}], - remappings=[('~/meta', ['/', LaunchConfig('cam_0_name'), - '/meta']), ], - extra_arguments=[{'use_intra_process_comms': True}], - ), - ], - output='screen', - ) # end of container - return [container] - - -def generate_launch_description(): - """Create composable node by calling opaque function.""" - return LaunchDescription([ - LaunchArg('cam_0_name', default_value=['cam_0'], - description='camera name (ros node name) of camera 0'), - LaunchArg('cam_1_name', default_value=['cam_1'], - description='camera name (ros node name) of camera 1'), - LaunchArg('cam_0_type', default_value='blackfly_s', - description='type of camera 0'), - LaunchArg('cam_1_type', default_value='blackfly_s', - description='type of camera 1'), - LaunchArg('cam_0_serial', default_value="'20435008'", - description='FLIR serial number of camera 0 (in quotes!!)'), - LaunchArg('cam_1_serial', default_value="'20415937'", - description='FLIR serial number of camera 1 (in quotes!!)'), - OpaqueFunction(function=launch_setup) - ]) From eaac252b633ebbf3cb891c7b155662d2bfbca8f5 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Thu, 30 May 2024 13:44:29 -0400 Subject: [PATCH 2/2] added primary_secondary launch file --- .../doc/index.rst | 6 +- .../launch/primary_secondary.launch.py | 164 ++++++++++++++++++ 2 files changed, 169 insertions(+), 1 deletion(-) create mode 100644 spinnaker_synchronized_camera_driver/launch/primary_secondary.launch.py diff --git a/spinnaker_synchronized_camera_driver/doc/index.rst b/spinnaker_synchronized_camera_driver/doc/index.rst index 95f29c11..630b438c 100644 --- a/spinnaker_synchronized_camera_driver/doc/index.rst +++ b/spinnaker_synchronized_camera_driver/doc/index.rst @@ -119,7 +119,7 @@ Exposure controller parameters Example usage ------------- -The driver comes with two example launch files that need to be modified for your purposes (update serial numbers etc). +The driver comes with several example launch files that need to be modified for your purposes (update serial numbers etc). The ``follower_example.launch.py`` can be used as template for stereo cameras, the ``master_example.launch.py`` for situations where each camera should run their own exposure control. @@ -133,6 +133,10 @@ Carefully examine the camera parameters that are set in the launch file, in part - ``exposure_auto`` must be off (disable the individual camera controller). - ``chunk_mode_active`` must be true, and chunk exposure time, gain and frame id must be enabled +For the special case where one camera (primary) triggers the sync pulse of a secondary camera, see +the example launch file ``primary_secondary.launch.py``. If you don't use Blackfly S cameras you will have +to adjust the digital I/O parameters. + Known issues ============ diff --git a/spinnaker_synchronized_camera_driver/launch/primary_secondary.launch.py b/spinnaker_synchronized_camera_driver/launch/primary_secondary.launch.py new file mode 100644 index 00000000..f235d674 --- /dev/null +++ b/spinnaker_synchronized_camera_driver/launch/primary_secondary.launch.py @@ -0,0 +1,164 @@ +# ----------------------------------------------------------------------------- +# Copyright 2024 Bernd Pfrommer and Li Dahua +# +# 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 where the primary camera triggers +# the secondary via its 3.3V signaling interface. +# +# 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 with serial numbers +camera_list = { + 'cam0': '22092309', + 'cam1': '22092307', +} + +# Exposure controller parameters +exposure_controller_parameters = { + 'brightness_target': 50, # from 0..255 + 'brightness_tolerance': 20, # when to update exposure/gain + 'max_exposure_time': 15000, # usec + 'min_exposure_time': 5000, # usec + 'max_gain': 29.9, + 'gain_priority': False, +} + +# Parameters shared by all cameras +shared_cam_parameters = { + 'debug': False, + 'quiet': True, + 'buffer_queue_size': 1, + 'compute_brightness': True, + 'pixel_format': 'BGR8', + 'exposure_auto': 'Off', + 'exposure_time': 10000, # not used under auto exposure + 'gain_auto': 'Off', + 'balance_white_auto': 'Continuous', + '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, + 'chunk_selector_timestamp': 'Timestamp', + 'chunk_enable_timestamp': True, +} + +# Parameters for the primary camera +primary_cam_parameters = { + **shared_cam_parameters, + 'trigger_mode': 'Off', + 'line1_selector': 'Line1', + 'line1_linemode': 'Output', + 'line2_selector': 'Line2', + 'line2_v33enable': True, +} + +# Parameters for the secondary camera +secondary_cam_parameters = { + **shared_cam_parameters, + 'trigger_mode': 'On', + 'trigger_source': 'Line3', + 'trigger_selector': 'FrameStart', + 'trigger_overlap': 'ReadOut', +} + + +def make_parameters(context): + """Generate camera parameters for the 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', + } + # 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()} + ) + # Set cam0 as master and cam1 as follower + driver_parameters[exp_ctrl_names[0] + '.type'] = 'master' + driver_parameters[exp_ctrl_names[1] + '.type'] = 'follower' + driver_parameters[exp_ctrl_names[1] + '.master'] = exp_ctrl_names[0] + + # Generate camera parameters + primary_cam_parameters['parameter_file'] = PJoin([pd, 'blackfly_s.yaml']) + secondary_cam_parameters['parameter_file'] = PJoin([pd, 'blackfly_s.yaml']) + for cam, serial in camera_list.items(): + if cam == 'cam0': + cam_params = {cam + '.' + k: v for k, v in primary_cam_parameters.items()} + else: + cam_params = {cam + '.' + k: v for k, v in secondary_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) + 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', + ) + 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), + ] + )