berndpfrommer / flir_spinnaker_ros2

ros2 driver for flir spinnaker SDK
26 stars 27 forks source link

Running multiple flir spinnaker cameras #24

Closed FranzAlbers closed 1 year ago

FranzAlbers commented 1 year ago

Hello,

first of all, thanks for developing and releasing this driver! We currently use it with ROS2 Humble and two Chameleon3 Cameras mounted on our research vehicle and it runs nicely if we use a slightly adapted version of stereo_synced.launch.py.

If we try to start both cameras with different launch files (both slightly adapted from chameleon.launch.py) each camera runs fine if we only start one camera. Whenever we start up the second camera, the first camera stops publishing immediately and only the second camera publishes its images:

[camera_driver_node-1] [INFO] [1662551425.549895681] [sensing.camera.camera_rear]: frame rate in: 20.005 Hz, out:20 Hz, drop: 0%
[camera_driver_node-1] [INFO] [1662551430.549893745] [sensing.camera.camera_rear]: frame rate in: 20.0055 Hz, out:15 Hz, drop: 0%
[camera_driver_node-1] [INFO] [1662551435.549899662] [sensing.camera.camera_rear]: frame rate in: 20.0055 Hz, out:0 Hz, drop: 0%
[camera_driver_node-1] [INFO] [1662551429.873198944] [sensing.camera.camera_front]: camera_driver_node started up!
[camera_driver_node-1] [INFO] [1662551433.085710646] [sensing.camera.camera_front]: frame rate in: 19.94 Hz, out:0 Hz, drop: 0%
... (after subscribing) ...
[camera_driver_node-1] [INFO] [1662551453.085705482] [sensing.camera.camera_front]: frame rate in: 20.0062 Hz, out:20 Hz, drop: 0%

As mentioned, both cameras run fine together if we start them using composable nodes in the same process. However, we experienced some message rate drops and delays during rectification with the image_proc package if we run both cameras in the same composable node container at 20 Hz, so I figured running both cameras in separate processes might fix the issue.

Is this known and is there maybe a known fix for this issue?

berndpfrommer commented 1 year ago

First, this is not a known issue and should work. Are you doing a hardware sync? If yes, is the master sending a sync pulse? I'll try to replicate your setup (stereo, but separate processes) with a pair of blackfly-s.

FranzAlbers commented 1 year ago

Thanks for the quick answer!

For the working setup for both cameras, we are not using the exposure control or the hardware sync, as we wanted to use the hardware triggering signal from our lidar in the near future. This is the launch file we use:

# -----------------------------------------------------------------------------
# Copyright 2022 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.
#
#

import launch
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch.substitutions import LaunchConfiguration as LaunchConfig
from launch.actions import DeclareLaunchArgument as LaunchArg
from ament_index_python.packages import get_package_share_directory

camera_params_front = {
    'debug': False,
    'compute_brightness': True,
    'dump_node_map': False,
    # set parameters defined in chameleon.cfg    
    'image_width': 2048,
    'image_height': 1152,
    'offset_x': 0,
    'offset_y': 384,
    'pixel_format': 'RGB8', # 'BayerRG8, 'RGB8' or 'Mono8'
    'gain_auto': 'Continuous',
    'frame_rate_auto': 'Off', # 'Off' or 'Continuous'
    'frame_rate_enable': True,
    'frame_rate': 20.0,
    'trigger_mode': 'Off',
    '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,
    }

# Crop other side of the image on rear camera because it is rotated
camera_params_rear = camera_params_front.copy()
camera_params_rear['offset_y'] = 100

def generate_launch_description():
    """Create synchronized stereo camera."""
    flir_dir = get_package_share_directory('flir_spinnaker_ros2')
    config_dir = flir_dir + '/config/'
    container = ComposableNodeContainer(
            name='stereo_camera_container',
            namespace='camera',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='flir_spinnaker_ros2',
                    plugin='flir_spinnaker_ros2::CameraDriver',
                    name=LaunchConfig('camera_front'),
                    namespace='/sensing/camera',
                    parameters=[camera_params_front,
                                {'parameter_file': config_dir + 'chameleon_rst.cfg',
                                 'frame_id': 'camera_front/camera_optical_link',
                                 'serial_number': '18497292'}],
                    remappings=[('~/control', 'camera_front/exposure_control/control'),],
                    extra_arguments=[{'use_intra_process_comms': True}],
                ),
                ComposableNode(
                    package='flir_spinnaker_ros2',
                    plugin='flir_spinnaker_ros2::CameraDriver',
                    name=LaunchConfig('camera_rear'),
                    namespace='/sensing/camera',
                    parameters=[camera_params_rear,
                                {'parameter_file': config_dir + 'chameleon_rst.cfg',
                                 'frame_id': 'camera_rear/camera_optical_link',
                                 'serial_number': '18497287'}],
                    remappings=[('~/control', 'camera_rear/exposure_control/control'), ],
                    extra_arguments=[{'use_intra_process_comms': True}],
                ),
                ComposableNode(
                    package='image_proc',
                    plugin='image_proc::RectifyNode',
                    name='rectify_front_cam_node',
                    namespace='/sensing/camera/camera_front',
                    remappings=[('image', 'image_raw'),],
                    #parameters=[{'queue_size': 1}], # unused
                    extra_arguments=[{'use_intra_process_comms': True}],
                ),
                ComposableNode(
                    package='image_proc',
                    plugin='image_proc::RectifyNode',
                    name='rectify_rear_cam_node',
                    namespace='/sensing/camera/camera_rear',
                    remappings=[('image', 'image_raw'),],
                    #parameters=[{'queue_size': 1}], # unused
                    extra_arguments=[{'use_intra_process_comms': True}],
                ),
                ComposableNode(
                    package='image_flip',
                    plugin='image_flip::ImageFlipNode',
                    name='rotate_rear_cam_node',
                    namespace='/sensing/camera/camera_rear',
                    parameters=[{'rotation_steps': 2},
                                {'use_camera_info': False}],
                    remappings=[('image', 'image_rect'),],
                    #extra_arguments=[{'use_intra_process_comms': True}],
                ),
            ],
            output='screen',
    )

    name_0_arg = LaunchArg('camera_front', default_value=['camera_front'],
                           description='name of camera 0')
    name_1_arg = LaunchArg('camera_rear', default_value=['camera_rear'],
                           description='name of camera 1')
    return launch.LaunchDescription([name_0_arg, name_1_arg, container])

This is one of the launch files we use for running both cameras separately:

from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration as LaunchConfig
from launch.actions import DeclareLaunchArgument as LaunchArg
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory

camera_params = {
    'serial_number': '18497292',
    'frame_id': 'camera_front/camera_optical_link',
    'debug': False,
    'compute_brightness': False,
    'dump_node_map': False,
    # set parameters defined in chameleon.cfg    
    'image_width': 2048,
    'image_height': 1152,
    'offset_x': 0,
    'offset_y': 384,
    'pixel_format': 'RGB8', # 'BayerRG8, 'RGB8' or 'Mono8'
    'gain_auto': 'Continuous',
    'frame_rate_auto': 'Off', # 'Off' or 'Continuous'
    'frame_rate_enable': True,
    'frame_rate': 20.0,
    'trigger_mode': 'Off',
    '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 generate_launch_description():
    """launch chameleon camera node."""
    flir_dir = get_package_share_directory('flir_spinnaker_ros2')
    config_dir = flir_dir + '/config/'
    name_arg = LaunchArg('camera_name', default_value='camera_front',
                         description='camera name')
    serial_arg = LaunchArg('serial', default_value="'18497292'",
                           description='serial number')
    print([LaunchConfig('serial'),'_'])
    node = Node(package='flir_spinnaker_ros2',
                executable='camera_driver_node',
                output='screen',
                name='camera_front',
                namespace='/sensing/camera',
                parameters=[camera_params,
                            {'parameter_file': config_dir + 'chameleon_rst.cfg',
                             'serial_number': [LaunchConfig('serial')],
                            }],
                #remappings=[('~/control', '/exposure_control/control'),],
    )
    return LaunchDescription([name_arg, serial_arg, node])
berndpfrommer commented 1 year ago

Just tested with my two blackflies using my own launch files, but no sync signal: works fine when running two separate nodes (each at 100hz!). Will try your launch files now (adapted to blackfly).

berndpfrommer commented 1 year ago

The launch file below works for me for dual startup for the blackfly cameras. It is derived from your launch file with only minimal changes (removing the parameters that the blackfly does not support). One more thing: can you double and triple check that you are really starting the two nodes with different serial numbers? Because opening the same camera twice could stop the first one.

from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration as LaunchConfig
from launch.actions import DeclareLaunchArgument as LaunchArg
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory

camera_params = {
    'serial_number': '20435008',
    'frame_id': 'camera_front/camera_optical_link',
    'debug': False,
    'compute_brightness': False,
    'dump_node_map': False,
    # set parameters defined in chameleon.cfg    
#    'image_width': 2048,
#    'image_height': 1152,
#    'offset_x': 0,
#    'offset_y': 384,
#    'pixel_format': 'RGB8', # 'BayerRG8, 'RGB8' or 'Mono8'
    'gain_auto': 'Continuous',
#    'frame_rate_auto': 'Off', # 'Off' or 'Continuous'
    'frame_rate_enable': True,
    'frame_rate': 20.0,
    'trigger_mode': 'Off',
    '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 generate_launch_description():
    """launch chameleon camera node."""
    flir_dir = get_package_share_directory('flir_spinnaker_ros2')
    config_dir = flir_dir + '/config/'
    name_arg = LaunchArg('camera_name', default_value='camera_front',
                         description='camera name')
    serial_arg = LaunchArg('serial', default_value="'20435008'",
                           description='serial number')
    print([LaunchConfig('serial'),'_'])
    node = Node(package='flir_spinnaker_ros2',
                executable='camera_driver_node',
                output='screen',
                name='camera_front',
                namespace='/sensing/camera',
                parameters=[camera_params,
                            {'parameter_file': config_dir + 'blackfly_s.cfg',
                             'serial_number': [LaunchConfig('serial')],
                            }],
                #remappings=[('~/control', '/exposure_control/control'),],
    )
    return LaunchDescription([name_arg, serial_arg, node])
FranzAlbers commented 1 year ago

This is strange, seems to be more or less exactly the same launch file. I've also tried using two different config files, but that has no influence. We use the cameras with USB3.

The serial numbers are set correctly in both occurrences in both our launch files. This is also stated by the driver during launch:

[camera_driver_node-1] [INFO] [1662558435.575136806] [sensing.camera.camera_front]:  serial: 18497292
[camera_driver_node-1] [INFO] [1662558441.507088485] [sensing.camera.camera_rear]:  serial: 18497287

What is also strange is that the nodes remain in the node graph after closing the driver or the whole terminal, but I am not sure if this can have an effect:

albers@leaf-pc:~$ ros2 node list
WARNING: Be aware that are nodes in the graph that share an exact name, this can have unintended side effects.
/camera/stereo_camera_container
/camera/stereo_camera_container
/camera/stereo_camera_container
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_front
/sensing/camera/camera_rear
/sensing/camera/camera_rear
/sensing/camera/camera_rear
/sensing/camera/camera_rear
/sensing/camera/camera_rear
/sensing/camera/camera_rear
/sensing/camera/camera_rear
/sensing/camera/camera_rear
/sensing/camera/camera_rear
berndpfrommer commented 1 year ago

Alright, this feels more like a ROS2 issue rather than a FLIR problem. One more check (at your discretion) would be to start up one camera with the ROS node, then start spinview and see if you can pull images from the second camera without affecting the ROS node. However spinview could give you a hard time when trying to do that.

The ROS2 message about nodes sharing the same name is definitely a point of concern. How about some shot-in-the-dark approaches like killing the ros2 daemon? It's happened once or twice to me that ROS2 went into a funky state and killing the daemon helped. More silly suggestions along these lines: rebooting the machine.

FranzAlbers commented 1 year ago

I just checked and can confirm that it is possible to pull images with spinview for one camera and with the ros2 driver for the second camera at the same time. Of course, starting spinview and starting the ros2 driver for the same camera stops the image stream for the other method.

In our setup, we currently run around 20 different nodes. After stopping them with Ctrl+C, only the following persist:

albers@leaf-pc:~$ ros2 node list
/sensing/camera/camera/stereo_camera_container
/sensing/camera/camera_front
/sensing/camera/camera_rear

The nodes for rectifying and flipping images in the stereo_camera_container are also correctly closed. Killing the ros2 daemon with killall -9 ros2 doesn't change this, the nodes are persisting. They persist until a reboot is performed, but this issue is reproducable. So I think the driver is probably somehow not correctly closed on Ctrl+C.

berndpfrommer commented 1 year ago

Strange. If I ctrl-C the ROS driver for my blackfly cameras they exit cleanly. Only now do I understand your log files correctly: the callbacks keep coming from the spinnaker SDK, so definitely this problem is entirely in ROS land. Based on my improved understanding I did the test again but I cannot replicate your problems: even with two nodes running I get good data simultaneously on both cameras. I don't know how to clean the ROS2 state but that should not require a reboot. Have you tried to kill all node processes and the daemon with -9, checked that they are gone, then restarted them? Starting from a clean slate if you bring up both both camera nodes, do you get any warnings about duplicate node names? Can you double check that the nodes are indeed started with different names? If all fails: go into the camera_driver.cpp code and bypass the publication check for count_subscribers(), see if it then publishes. Because evidently that check says there are no subscribers or else your "out" statistics in the log wouldn't be zero.

FranzAlbers commented 1 year ago

Thanks for your very quick help! This is really great support that you don't even get for many commercial products.

I did some more research and apparently the order of doing things matters: Open spinview -> launch ros2 driver for front cam -> run stream for rear cam in spinnaker -> Both streams run as they should Open spinview -> run stream for rear cam in spinnaker -> launch ros2 driver for front cam -> spinview crashes with a device not open error: QIODevice::write (QFile, "SpinView_QT_log"): device not open

Bypassing the publication check does not fix the issue. The CameraDriver::publishImage callback function just stops being called if I start the second camera either in ROS2 or spinview. So they obviously somehow access the same resource, even though they shouldn't. I've checked the serial numbers multiple times.

There is no warning regarding duplicate node names during the startup of the driver. The only warning I've seen so far is coming from ros2 node list or ros2 node info /sensing/camera/camera_front. Interestingly, sometimes the node is closed as it should after ctrl+c, but this happens not often (in much less than half of my tries). Killing the ros2 daemon in htop removes the zombie nodes from the node graph, but even when starting from a clean slate we cannot access both cameras from different processes at the same time. So I don't think these issues are connected.

I am currently running the ubuntu 20.04 spinnaker SDK on ubuntu 22.04 since the appropriate packages are not available yet. Could this cause this issue? I've tried updating spinnaker SDK from version 2.6.0.160 to 2.7.0.128. With 2.7.0.128, the camera driver node dies right after startup, so I've reverted spinnaker SDK back to 2.6.0.160:

[camera_driver_node-1] [INFO] [1662713932.179317821] [sensing.camera.camera_front]: camera_driver_node started up!
[ERROR] [camera_driver_node-1]: process has died [pid 24674, exit code -11, cmd '/home/albers/autoware/install/flir_spinnaker_ros2/lib/flir_spinnaker_ros2/camera_driver_node --ros-args -r __node:=camera_front -r __ns:=/sensing/camera --params-file /tmp/launch_params_6vzmswvb --params-file /tmp/launch_params_bi9b75ug'].
berndpfrommer commented 1 year ago

You are welcome, although I am starting to reach the point where it gets difficult to support your particular configuration: I don't have a machine running Ubuntu 22.04 yet and don't have the chameleon cameras.

I fear you have already covered this, but check your USB3 linux memory limit configuration in case this is resource contention at the kernel level. I could imagine if you run two separate processes it may consume more kernel resources: https://www.flir.com/support-center/iis/machine-vision/application-note/using-linux-with-usb-3.1/

What I don't understand is that your very first post shows that "frame rate in" does not drop to zero, only frame rate out. This would indicate that there are still callbacks coming from the SDK. How can this be a SDK issue then?

FranzAlbers commented 1 year ago

I've already set the USB3 memory limit during the installation of the SDK to 1000MB, but also setting it to 2000MB for testing does not change the behavior.

I don't think that the frame rate in really shows a valid number in the output after the second camera is launched. It is usually fluctuating minimally around the desired frame rate when the driver is running fine, but I've checked and it is stuck at the exact last value once the second camera is launched and does not change afterward. There are definitely no callbacks called anymore in this case.

Moving the rectifying and flipping nodes for one camera out of the camera component container or running all camera and image processing nodes in a multi-threaded component_container_mt fixes the frame rate drops and delays me, so I've found a possible workaround for our use case. However, we lose the option to use_intra_process_comms with this workaround (not sure for component_container_mt).

berndpfrommer commented 1 year ago

After upgrading the OS on my desktop to Jammy and installing the latest Spinnaker SDK v2.7.0 I find exactly the same problem as you: when I start the second camera, the first one stops.

The moment I start the second camera I get this in the syslog:

Sep 9 22:25:06 enz kernel: [ 2644.063995] usb 2-2.4: reset SuperSpeed USB device number 7 using xhci_hcd Sep 9 22:25:07 enz kernel: [ 2644.188014] usb 2-2.2: reset SuperSpeed USB device number 6 using xhci_hcd

Looks like something resets the first camera when the second one starts. Alas there is something funky with my desktop such that it does not recognize the devices as USB3, just USB2, so I have the cameras both hanging off a cheap USB3 hub.

Could it be that the Spinnaker SDK resets both USB links when the second camera starts such that they can do some bandwidth allocation scheme between the cameras? Do you have a way of plugging it into a different port such that in "lsusb -t" they show up on different busses? Your computer may not have the hardware for that...

berndpfrommer commented 1 year ago

Please check if this workaround is acceptable. I have also put in a support ticket with FLIR

FranzAlbers commented 1 year ago

Thanks for your work!

The workaround works well in most cases, but occasionally, one or two cameras are not found:

[component_container-2] [INFO] [1662981823.842932070] [sensing.camera.camera_rear]: using spinnaker lib version: 2.6.0.160
[component_container-2] [WARN] [1662981823.843022853] [sensing.camera.camera_rear]: driver found no cameras!
[component_container-2] [ERROR] [1662981823.843042229] [sensing.camera.camera_rear]: no camera found with serial number:18497287
[component_container-2] [ERROR] [1662981823.843054540] [sensing.camera.camera_rear]: startup failed!
[component_container-1] [INFO] [1662981823.843132043] [sensing.camera.camera_front]: using spinnaker lib version: 2.6.0.160
[component_container-1] [WARN] [1662981823.843201931] [sensing.camera.camera_front]: driver found no cameras!
[component_container-1] [ERROR] [1662981823.843224254] [sensing.camera.camera_front]: no camera found with serial number:18497292
[component_container-1] [ERROR] [1662981823.843245249] [sensing.camera.camera_front]: startup failed!
[component_container-1] [ERROR] [1662981824.070594517] [camera.camera_front_container]: Component constructor threw an exception: startup of CameraDriver node failed!
[component_container-2] [ERROR] [1662981824.070688787] [camera.camera_rear_proc_container]: Component constructor threw an exception: startup of CameraDriver node failed!
[ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'camera_rear' of type 'flir_spinnaker_ros2::CameraDriver' in container '/camera/camera_rear_proc_container': Component constructor threw an exception: startup of CameraDriver node failed!
[ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'camera_front' of type 'flir_spinnaker_ros2::CameraDriver' in container '/camera/camera_front_container': Component constructor threw an exception: startup of CameraDriver node failed!

And sometimes the cameras are found, but the serial number seems to be unknown:

[component_container-1] [INFO] [1662982658.317526333] [sensing.camera.camera_front]: using spinnaker lib version: 2.6.0.160
[component_container-1] [ERROR] [1662982658.317703327] [sensing.camera.camera_front]: no camera found with serial number:18497292
[component_container-1] [WARN] [1662982658.317730867] [sensing.camera.camera_front]: found cameras: 18497287
[component_container-1] [WARN] [1662982658.317748178] [sensing.camera.camera_front]: found cameras: unknown
[component_container-1] [ERROR] [1662982658.317764438] [sensing.camera.camera_front]: startup failed!

These errors occur in roughly estimated around 1/3 of all tries. Restarting the launch file usually works.

We currently already have attached the cameras to different busses:

Bus 006 Device 012: ID 1e10:3300 Point Grey Research, Inc. Chameleon3 CM3-U3-31S4C
Bus 004 Device 012: ID 1e10:3300 Point Grey Research, Inc. Chameleon3 CM3-U3-31S4C

We use a PCI USB Card for the cameras, so I can try some different ports next time I'm at the vehicle.

To me, it seems like this is an issue introduced by the spinnaker SDK, so I think waiting on a fix in the SDK or on the proper SDK version for ubuntu 22.04 might be the best option. Nevertheless, I think using the acquisition timeout is a nice addition to the driver.

berndpfrommer commented 1 year ago

Have you updated the camera to the latest firmware? I do recall there were some substantial fixes with respect to the original firmware. Will have a look if I can make the startup more robust. I started the two cameras in separate launch files, by hand, i.e. well separated in time. Did you start the two cameras in the same launch script? That may cause some race conditions within the SDK.

FranzAlbers commented 1 year ago

Yes, we're using firmware 1.12.3, which is the latest for our Chameleon3-31S4 cameras.

Race conditions are most likely the issue here. I indeed used only a single launch file and I can not reproduce the error with two separate launch files.

berndpfrommer commented 1 year ago

Even using a single launch script I was unable to reproduce the race condition in more than 20 attempts. So the fix I just merged in is speculative. Could you please test? Thanks!

FranzAlbers commented 1 year ago

The race condition only occurs when I start two different camera driver nodes in two different containers, like this:

# -----------------------------------------------------------------------------
# Copyright 2022 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.
#
#

import launch
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch.substitutions import LaunchConfiguration as LaunchConfig
from launch.actions import DeclareLaunchArgument as LaunchArg
from ament_index_python.packages import get_package_share_directory

camera_params_front = {
    'debug': False,
    'compute_brightness': True,
    'dump_node_map': False,
    # set parameters defined in chameleon.cfg    
    #'video_mode': 1,
    'image_width': 2048,
    'image_height': 1152,
    'offset_x': 0,
    'offset_y': 384,
    #'image_width': 2048,
    #'image_height': 1536,
    #'offset_x': 0,
    #'offset_y': 0,
    'pixel_format': 'RGB8', # 'BayerRG8, 'RGB8' or 'Mono8'
    #'pixel_coding': 'RGBPacked',
    'gain_auto': 'Continuous',
    #'exposure_auto': 'Continuous',
    'frame_rate_auto': 'Off', # 'Off' or 'Continuous'
    'frame_rate_enable': True,
    #'frame_rate_continous': True,
    'frame_rate': 20.0,
    'trigger_mode': 'Off',
    '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,
    }

# Crop other side of the image on rear camera because it is rotated
camera_params_rear = camera_params_front.copy()
camera_params_rear['offset_y'] = 100

def generate_launch_description():
    """Create synchronized stereo camera."""
    flir_dir = get_package_share_directory('flir_spinnaker_ros2')
    config_dir = flir_dir + '/config/'
    camera_front_container = ComposableNodeContainer(
            name='camera_front_container',
            namespace='camera',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='flir_spinnaker_ros2',
                    plugin='flir_spinnaker_ros2::CameraDriver',
                    name=LaunchConfig('camera_front'),
                    namespace='/sensing/camera',
                    parameters=[camera_params_front,
                                {'parameter_file': config_dir + 'chameleon_rst.cfg',
                                 'frame_id': 'camera_front/camera_optical_link',
                                 'serial_number': '18497292'}],
                    remappings=[('~/control', 'camera_front/exposure_control/control'),],
                    extra_arguments=[{'use_intra_process_comms': True}],
                ),
                ComposableNode(
                    package='image_proc',
                    plugin='image_proc::RectifyNode',
                    name='rectify_front_cam_node',
                    namespace='/sensing/camera/camera_front',
                    remappings=[('image', 'image_raw'),],
                    extra_arguments=[{'use_intra_process_comms': True}],
                ),
            ],
            output='screen',
    )

    # Workaround for keeping rate up and delay low. Should also contain the camera_rear driver node, but this is not possible for now
    # https://github.com/berndpfrommer/flir_spinnaker_ros2/issues/24
    camera_rear_container = ComposableNodeContainer(
        name='camera_rear_container',
        namespace='camera',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[
            ComposableNode(
                package='flir_spinnaker_ros2',
                plugin='flir_spinnaker_ros2::CameraDriver',
                name=LaunchConfig('camera_rear'),
                namespace='/sensing/camera',
                parameters=[camera_params_rear,
                            {'parameter_file': config_dir + 'chameleon_rst.cfg',
                                'frame_id': 'camera_rear/camera_optical_link',
                                'serial_number': '18497287'}],
                remappings=[('~/control', 'camera_rear/exposure_control/control'), ],
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
           ComposableNode(
                package='image_proc',
                plugin='image_proc::RectifyNode',
                name='rectify_rear_cam_node',
                namespace='/sensing/camera/camera_rear',
                remappings=[('image', 'image_raw'),],
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
        ],
        output='screen',
    )

    name_0_arg = LaunchArg('camera_front', default_value=['camera_front'],
                           description='name of camera 0')
    name_1_arg = LaunchArg('camera_rear', default_value=['camera_rear'],
                           description='name of camera 1')
    return launch.LaunchDescription([name_0_arg, name_1_arg, camera_front_container, camera_rear_container])

Unfortunately, the cameras are still not found with retrying:

[component_container-1] [INFO] [1663057248.667714341] [sensing.camera.camera_front]: using spinnaker lib version: 2.6.0.160
[component_container-2] [INFO] [1663057248.668189055] [sensing.camera.camera_rear]: using spinnaker lib version: 2.6.0.160
[component_container-1] [WARN] [1663057248.668712076] [sensing.camera.camera_front]: no camera found with serial: 18497292 on try # 1
[component_container-2] [WARN] [1663057248.669598766] [sensing.camera.camera_rear]: no camera found with serial: 18497287 on try # 1
[component_container-1] [WARN] [1663057249.768442707] [sensing.camera.camera_front]: no camera found with serial: 18497292 on try # 2
[component_container-2] [WARN] [1663057249.770156179] [sensing.camera.camera_rear]: no camera found with serial: 18497287 on try # 2
[component_container-1] [WARN] [1663057250.769669368] [sensing.camera.camera_front]: no camera found with serial: 18497292 on try # 3
[component_container-2] [WARN] [1663057250.771303759] [sensing.camera.camera_rear]: no camera found with serial: 18497287 on try # 3
[component_container-1] [WARN] [1663057251.862655476] [sensing.camera.camera_front]: no camera found with serial: 18497292 on try # 4
[component_container-2] [WARN] [1663057251.864169251] [sensing.camera.camera_rear]: no camera found with serial: 18497287 on try # 4
[component_container-1] [WARN] [1663057252.864031267] [sensing.camera.camera_front]: no camera found with serial: 18497292 on try # 5
[component_container-2] [WARN] [1663057252.865358517] [sensing.camera.camera_rear]: no camera found with serial: 18497287 on try # 5
[component_container-1] [ERROR] [1663057253.864204505] [sensing.camera.camera_front]: giving up, camera 18497292 not found!
[component_container-1] [ERROR] [1663057253.864289647] [sensing.camera.camera_front]: startup failed!
[component_container-2] [ERROR] [1663057253.865486239] [sensing.camera.camera_rear]: giving up, camera 18497287 not found!
[component_container-2] [ERROR] [1663057253.865537566] [sensing.camera.camera_rear]: startup failed!
[component_container-2] [ERROR] [1663057254.657287385] [camera.camera_rear_container]: Component constructor threw an exception: startup of CameraDriver node failed!
[component_container-1] [ERROR] [1663057254.659451059] [camera.camera_front_container]: Component constructor threw an exception: startup of CameraDriver node failed!
[ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'camera_front' of type 'flir_spinnaker_ros2::CameraDriver' in container '/camera/camera_front_container': Component constructor threw an exception: startup of CameraDriver node failed!
[ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'camera_rear' of type 'flir_spinnaker_ros2::CameraDriver' in container '/camera/camera_rear_container': Component constructor threw an exception: startup of CameraDriver node failed!

Similar results if only one camera is not found. Retrying in this case also does not find the second camera. I've also tried different USB ports, e.g., one on the main board and one on the PCI USB Card. This seems like it makes no difference.

It might be possible to delay the launch of one of the containers with a TimerAction, but when doing this it seems like the second container isn't really starting either (not reaching using spinnaker lib version .. until the second node is also started). So I had no success so far with this approach.

Hopefully, this behavior is fixed in the next Spinnaker SDK release. Until then, we have working workarounds.

berndpfrommer commented 1 year ago

Your launch file (with minimal edits to serial number and camera parameters) works just fine with two blackfly S cameras running on Galactic / Ubuntu 20.04. Started up correctly every single time. Unfortunately my Jammy/Humble machine died and may need a while to get back up and running.

berndpfrommer commented 1 year ago

By some miracle the jammy/humble desktop booted up: works there as well. So this is specific to either your host hardware or the chameleons.

berndpfrommer commented 1 year ago

Here is what I got back from FLIR:

Hi Bernd,

My name is William, I am a Support Engineer for Teledyne FLIR Machine Vision, and I will be assisting you with your questions.

First off, apologies for the delay; I'm not sure how the response didn't go through, but I will follow up in a couple of days in case you don't respond to this one by then. I have brought up your issue with our software engineers here. Officially, we do not support using multiple instances of spinnaker running. Running multiple cameras in a single instance of a spinnaker-based application is supported and tested for, but having multiple instances of spinnaker running (each with their own camera) does not. Although it is possible to have multiple instances of Spinnaker running with multiple cameras, and have no issues occurring, its not something we test for, or support ourselves, meaning issues could occur, regardless of using ROS2 or not in the spinnaker application.

Best regards,

William Gallego Support Engineer Teledyne FLIR

berndpfrommer commented 1 year ago

Closing this as unfixab