diff --git a/spinnaker_camera_driver/CMakeLists.txt b/spinnaker_camera_driver/CMakeLists.txt index 3385190e..86828bd1 100644 --- a/spinnaker_camera_driver/CMakeLists.txt +++ b/spinnaker_camera_driver/CMakeLists.txt @@ -67,6 +67,7 @@ find_package(SPINNAKER REQUIRED) find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(camera_info_manager REQUIRED) +find_package(diagnostic_updater REQUIRED) find_package(flir_camera_msgs REQUIRED) find_package(image_transport REQUIRED) find_package(rclcpp REQUIRED) @@ -82,6 +83,7 @@ include_directories(SYSTEM set(ROS_DEPENDENCIES camera_info_manager::camera_info_manager + diagnostic_updater::diagnostic_updater ${flir_camera_msgs_TARGETS} image_transport::image_transport rclcpp::rclcpp diff --git a/spinnaker_camera_driver/cmake/spinnaker_camera_driverConfig.cmake.in b/spinnaker_camera_driver/cmake/spinnaker_camera_driverConfig.cmake.in index 6a5600cf..8be4f3f1 100644 --- a/spinnaker_camera_driver/cmake/spinnaker_camera_driverConfig.cmake.in +++ b/spinnaker_camera_driver/cmake/spinnaker_camera_driverConfig.cmake.in @@ -1,12 +1,13 @@ include(CMakeFindDependencyMacro) # find_dependency(SPINNAKER) -find_dependency(rclcpp) -find_dependency(rclcpp_components) find_dependency(camera_info_manager) -find_dependency(image_transport) +find_dependency(diagnostic_updater) find_dependency(flir_camera_msgs) +find_dependency(image_transport) find_dependency(sensor_msgs) +find_dependency(rclcpp) +find_dependency(rclcpp_components) find_dependency(std_msgs) # Add the targets file diff --git a/spinnaker_camera_driver/doc/index.rst b/spinnaker_camera_driver/doc/index.rst index 0063627b..18b2fa85 100644 --- a/spinnaker_camera_driver/doc/index.rst +++ b/spinnaker_camera_driver/doc/index.rst @@ -179,6 +179,17 @@ files*, the driver has the following ROS parameters: - ``use_ieee_1588``: use PTP (IEEE 1588) to set the ``header.stamp`` time stamp instead of system time. Note that you will still need to enable IEEE 1588 at the camera level, and enable time stamp "chunks". Default: false. +- ``diagnostic_incompletes_warn``: number of incomplete frames per diagnostic update period + before changing status to ``warning``. +- ``diagnostic_incompletes_error``: number of incomplete frames per diagnostic update period + before changing status to ``error``. +- ``diagnostic_drop_warn``: number of SDK-dropped frames per diagnostic update period + before changing status to ``warning``. +- ``diagnostic_drop_error``: number of SDK-dropped frames per diagnostic update period + before changing status to ``error``. +- ``diagnostic_min_freq``: minimum incoming message frequency before failing diagnostics. +- ``diagnostic_max_freq``: max incoming message frequency before failing diagnostics. +- ``diagnostic_window``: number of updates to maintain for diagnostic window.o The parameters listed above *cannot* be dynamically updated at runtime but require a driver restart to modify. @@ -203,9 +214,8 @@ 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``. +You can however run multiple cameras in the same program (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 @@ -296,6 +306,8 @@ Note that this will *not* work if you ping the camera: you will find a MTU size of 1500, even if both camera and host NIC have correct MTU setting. Beware that some network cards have `bugs that mess up the MTU when running ptp4l `__. +At least for the Intel igb cards, installing the 5.19.12 drivers `from here `__ +fixes these issues. For more tips on GigE setup look at FLIR’s support pages `here `__ @@ -315,14 +327,14 @@ Only FLIR GigE cameras are PTP capable, but not all of them. For example the Bla Each camera has a built-in PTP hardware clock (PHC). To use it, you need to: - configure the camera to use PTP by enabling it either in SpinView or via the ROS parameter `gev_ieee_1588`` (set to true/false) -- run the camera in ``Auto`` or ``SlaveOnly`` mode by setting the corresponding parameter in SpinView or using the ROS parameter ``gev_ieee_1588_mode``. +- run the camera in ``Auto`` or ``SlaveOnly`` mode by setting the corresponding parameter in SpinView or using the ROS parameter ``gev_ieee_1588_mode``. Running in ``Auto`` rarely makes sense though because one of the cameras may become a master clock without really having an absolute time reference (like ntp) to the ROS host clock. - enable time stamps being sent via chunks by setting ``chunk_mode_active`` to ``True``, ``chunk_selector_timestamp`` to ``Timestamp`` followed by ``chunk_enable_timestamp`` to True. - tell the driver to actually use the time stamps instead of the system clock by setting ``use_ieee_1588`` to True. If this flag is set, the driver will populate the ROS image message header stamp with the PTP time stamp. -The launch file section for the Blackfly S has some commented out example settings. +For an example launch file, see ``multiple_cameras_ptp.launch.py``. -When the camera boots up it will start its PHC time at zero rather than at the correct time (TAI). The driver log will have warning messages and indicate a huge offset. +When the camera boots up it will start its PHC time at zero rather than at the correct time. The driver log will have warning messages and indicate a huge offset. :: diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp index 86e4f30f..e33f4b8e 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include #include @@ -27,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -40,6 +43,9 @@ class Camera { public: using ImageConstPtr = spinnaker_camera_driver::ImageConstPtr; + using CompositeDiagnosticsTask = diagnostic_updater::CompositeDiagnosticTask; + using FunctionDiagnosticsTask = diagnostic_updater::FunctionDiagnosticTask; + using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper; explicit Camera( rclcpp::Node * node, image_transport::ImageTransport * it, const std::string & prefix, bool useStatus = true); @@ -77,6 +83,8 @@ class Camera bool setBool(const std::string & nodeName, bool v); bool execute(const std::string & nodeName); bool readParameterDefinitionFile(); + void startDiagnostics(); + void stopDiagnostics(); rclcpp::Time getAdjustedTimeStamp(uint64_t t, int64_t sensorTime); @@ -120,6 +128,11 @@ class Camera // do nothing } } + void updateDiagnosticsStatus( + DiagnosticStatusWrapper & status, const std::string & label, const int val, + const DiagnosticLevels & levels); + void incompleteDiagnostics(DiagnosticStatusWrapper & status); + void droppedDiagnostics(DiagnosticStatusWrapper & status); // ----- variables -- std::string prefix_; @@ -178,6 +191,20 @@ class Camera std::shared_ptr synchronizer_; std::shared_ptr exposureController_; bool firstSynchronizedFrame_{true}; + // --------- related to diagnostics + std::shared_ptr updater_; + std::shared_ptr topicDiagnostic_; + std::shared_ptr imageArrivalDiagnostic_; + double maxFreqDiag_{0}; + double minFreqDiag_{0}; + CompositeDiagnosticsTask imageTask_; + FunctionDiagnosticsTask incompleteFrameTask_; + uint64_t numIncompletes_{0}; + DiagnosticLevels incompleteLevels_; + FunctionDiagnosticsTask dropFrameTask_; + DiagnosticLevels dropLevels_; + uint64_t lastFrameId_{0}; + uint64_t numDrops_{0}; }; } // namespace spinnaker_camera_driver #endif // SPINNAKER_CAMERA_DRIVER__CAMERA_HPP_ diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostic_levels.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostic_levels.hpp new file mode 100644 index 00000000..fa8785d7 --- /dev/null +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostic_levels.hpp @@ -0,0 +1,30 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2023 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. + +#ifndef SPINNAKER_CAMERA_DRIVER__DIAGNOSTIC_LEVELS_HPP_ +#define SPINNAKER_CAMERA_DRIVER__DIAGNOSTIC_LEVELS_HPP_ + +namespace spinnaker_camera_driver +{ +template +struct DiagnosticLevels +{ +public: + explicit DiagnosticLevels(T w = 0, T e = 0) : warning(w), error(e) {} + T warning{0}; + T error{0}; +}; +} // namespace spinnaker_camera_driver +#endif // SPINNAKER_CAMERA_DRIVER__DIAGNOSTIC_LEVELS_HPP_ diff --git a/spinnaker_camera_driver/launch/driver_node.launch.py b/spinnaker_camera_driver/launch/driver_node.launch.py index a1eb2eb3..ac3248d2 100644 --- a/spinnaker_camera_driver/launch/driver_node.launch.py +++ b/spinnaker_camera_driver/launch/driver_node.launch.py @@ -67,6 +67,9 @@ 'chunk_enable_gain': True, 'chunk_selector_timestamp': 'Timestamp', 'chunk_enable_timestamp': True, + 'diagnostic_period': 1.0, + 'diagnostic_min_freq': 39.0, + 'diagnostic_max_freq': 41.0 }, 'blackfly': { 'debug': False, diff --git a/spinnaker_camera_driver/launch/gige_node.launch.py b/spinnaker_camera_driver/launch/gige_node.launch.py new file mode 100644 index 00000000..9db6c24b --- /dev/null +++ b/spinnaker_camera_driver/launch/gige_node.launch.py @@ -0,0 +1,222 @@ +# ----------------------------------------------------------------------------- +# 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 Node +from launch_ros.substitutions import FindPackageShare + +example_parameters = { + 'blackfly_s': { + 'debug': False, + 'compute_brightness': False, + 'adjust_timestamp': True, + 'dump_node_map': False, + # set parameters defined in blackfly_s.yaml + 'gain_auto': 'Continuous', + # 'pixel_format': 'BayerRG8', + 'exposure_auto': 'Continuous', + # to use a user set, do this: + # 'user_set_selector': 'UserSet0', + # 'user_set_load': 'Yes', + # These are useful for GigE cameras + # 'device_link_throughput_limit': 380000000, + 'gev_scps_packet_size': 1500, + # PTP for GigE cameras + 'gev_ieee_1588': True, + 'gev_ieee_1588_mode': 'SlaveOnly', # 'SlaveOnly', #'Auto', + # 'use_ieee_1588' : True, + # ---- to reduce the sensor width and shift the crop + # 'image_width': 1408, + # 'image_height': 1080, + # 'offset_x': 16, + # 'offset_y': 0, + # 'binning_x': 1, + # 'binning_y': 1, + # 'connect_while_subscribed': True, + # 'reverse_x': True, + # 'reverse_y': True, + 'frame_rate_auto': 'Off', + 'frame_rate': 40.0, + 'frame_rate_enable': True, + 'buffer_queue_size': 10, + '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, + 'diagnostic_period': 1.0, + 'diagnostic_min_freq': 39.0, + 'diagnostic_max_freq': 41.0, + 'diagnostic_incompletes_warn': 1, + 'diagnostic_incompletes_error': 2, + 'diagnostic_drops_warn': 1, + 'diagnostic_drops_error': 2, + }, + 'blackfly': { + 'debug': False, + 'dump_node_map': False, + 'gain_auto': 'Continuous', + 'pixel_format': 'BayerRG8', + 'exposure_auto': 'Continuous', + 'frame_rate_auto': 'Off', + 'frame_rate': 40.0, + 'frame_rate_enable': True, + 'buffer_queue_size': 10, + 'trigger_mode': 'Off', + # 'stream_buffer_handling_mode': 'NewestFirst', + # 'multicast_monitor_mode': False + }, + 'chameleon': { + 'debug': False, + 'compute_brightness': False, + 'dump_node_map': False, + # set parameters defined in chameleon.yaml + 'gain_auto': 'Continuous', + 'exposure_auto': 'Continuous', + 'offset_x': 0, + 'offset_y': 0, + 'image_width': 2048, + 'image_height': 1536, + 'pixel_format': 'RGB8', # 'BayerRG8, 'RGB8' or 'Mono8' + 'frame_rate_continous': True, + 'frame_rate': 100.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, + }, + 'grasshopper': { + 'debug': False, + 'compute_brightness': False, + 'dump_node_map': False, + # set parameters defined in grasshopper.yaml + 'gain_auto': 'Continuous', + 'exposure_auto': 'Continuous', + 'frame_rate_auto': 'Off', + 'frame_rate': 100.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, + }, + 'flir_ax5': { + 'debug': False, + 'compute_brightness': False, + 'adjust_timestamp': False, + 'dump_node_map': False, + # --- Set parameters defined in flir_ax5.yaml + 'pixel_format': 'Mono8', + 'gev_scps_packet_size': 576, + 'image_width': 640, + 'image_height': 512, + 'offset_x': 0, + 'offset_y': 0, + 'sensor_gain_mode': 'HighGainMode', # "HighGainMode" "LowGainMode" + 'nuc_mode': 'Automatic', # "Automatic" "External" "Manual" + 'sensor_dde_mode': 'Automatic', # "Automatic" "Manual" + 'sensor_video_standard': 'NTSC30HZ', # "NTSC30HZ" "PAL25Hz" "NTSC60HZ" "PAL50HZ" + # valid values: "PlateauHistogram" "OnceBright" "AutoBright" "Manual" "Linear" + 'image_adjust_method': 'PlateauHistogram', + 'video_orientation': 'Normal', # "Normal" "Invert" "Revert" "InvertRevert" + }, +} + + +def launch_setup(context, *args, **kwargs): + """Launch camera driver node.""" + parameter_file = LaunchConfig('parameter_file').perform(context) + camera_type = LaunchConfig('camera_type').perform(context) + if not parameter_file: + parameter_file = PathJoinSubstitution( + [ + FindPackageShare('spinnaker_camera_driver'), + 'config', + camera_type + '.yaml', + ] + ) + if camera_type not in example_parameters: + raise Exception('no example parameters available for type ' + camera_type) + + node = Node( + package='spinnaker_camera_driver', + executable='camera_driver_node', + output='screen', + name=[LaunchConfig('camera_name')], + parameters=[ + example_parameters[camera_type], + { + 'ffmpeg_image_transport.encoding': 'hevc_nvenc', + 'parameter_file': parameter_file, + 'serial_number': [LaunchConfig('serial')], + }, + ], + remappings=[ + ('~/control', '/exposure_control/control'), + ], + ) + + return [node] + + +def generate_launch_description(): + """Create composable node by calling opaque function.""" + return LaunchDescription( + [ + LaunchArg( + 'camera_name', + default_value=['flir_camera'], + description='camera name (ros node name)', + ), + LaunchArg( + 'camera_type', + default_value='blackfly_s', + description='type of camera (blackfly_s, chameleon...)', + ), + LaunchArg( + 'serial', + default_value="'20435008'", + description='FLIR serial number of camera (in quotes!!)', + ), + LaunchArg( + 'parameter_file', + default_value='', + description='path to ros parameter definition file (override camera type)', + ), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/spinnaker_camera_driver/launch/multiple_cameras_ptp.launch.py b/spinnaker_camera_driver/launch/multiple_cameras_ptp.launch.py new file mode 100755 index 00000000..f2bbe870 --- /dev/null +++ b/spinnaker_camera_driver/launch/multiple_cameras_ptp.launch.py @@ -0,0 +1,142 @@ +# ----------------------------------------------------------------------------- +# 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. +# +# + +# +# Example launch script for using PTP with two Blackfly S GigE cameras connected +# via shared 1Gb connection to the ROS host. +# +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, + # 'device_link_throughput_limit': 125000000, # 1Gbit + # The two cameras are connected through a single gigabit link, + # so cut the bandwidth by half to avoid incomplete messages + 'device_link_throughput_limit': 62500000, # in MB/sec + 'gev_scps_packet_size': 1400, # intel nics have bug that prevents 9000 + 'gev_ieee_1588': True, # enable PTP on the camera cameras + 'gev_ieee_1588_mode': 'SlaveOnly', # 'SlaveOnly', 'Auto', + 'use_ieee_1588': True, # actually use the PTP time as header stamp + 'compute_brightness': True, + 'dump_node_map': False, + 'adjust_timestamp': False, + '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': 'Off', # Switch this to "On" to use external hardware trigger + 'trigger_source': 'Line3', + '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="'20255407'", + description='FLIR serial number of camera 0 (in quotes!!)', + ), + LaunchArg( + 'cam_1_serial', + default_value="'20255396'", + description='FLIR serial number of camera 1 (in quotes!!)', + ), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/spinnaker_camera_driver/package.xml b/spinnaker_camera_driver/package.xml index 226f8b08..26c9edbb 100644 --- a/spinnaker_camera_driver/package.xml +++ b/spinnaker_camera_driver/package.xml @@ -29,6 +29,7 @@ yaml-cpp camera_info_manager + diagnostic_updater flir_camera_msgs image_transport rclcpp diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp index 1553744b..0447b4ae 100644 --- a/spinnaker_camera_driver/src/camera.cpp +++ b/spinnaker_camera_driver/src/camera.cpp @@ -32,6 +32,7 @@ namespace spinnaker_camera_driver { namespace chrono = std::chrono; +using Status = diagnostic_msgs::msg::DiagnosticStatus; // // this complicated code is to detect an interface change // between foxy and galactic @@ -112,9 +113,14 @@ Camera::NodeInfo::NodeInfo(const std::string & n, const std::string & nodeType) Camera::Camera( rclcpp::Node * node, image_transport::ImageTransport * it, const std::string & prefix, bool useStatus) +: node_(node), + name_(prefix), + imageTask_("image"), + incompleteFrameTask_( + "incomplete frames", std::bind(&Camera::incompleteDiagnostics, this, std::placeholders::_1)), + dropFrameTask_( + "dropped frames", std::bind(&Camera::droppedDiagnostics, this, std::placeholders::_1)) { - node_ = node; - name_ = prefix; imageTransport_ = it; prefix_ = prefix.empty() ? std::string("") : (prefix + "."); topicPrefix_ = prefix.empty() ? std::string("") : (prefix + "/"); @@ -123,6 +129,8 @@ Camera::Camera( statusTimer_ = rclcpp::create_timer( node_, node_->get_clock(), rclcpp::Duration(5, 0), std::bind(&Camera::printStatus, this)); } + imageTask_.addTask(&incompleteFrameTask_); + imageTask_.addTask(&dropFrameTask_); } Camera::~Camera() @@ -133,6 +141,7 @@ Camera::~Camera() bool Camera::stop() { + stopDiagnostics(); stopCamera(); if (wrapper_) { wrapper_->deInitCamera(); @@ -231,6 +240,12 @@ void Camera::readParameters() if (!quiet_) { LOG_INFO("reading ros parameters for camera with serial: " << serial_); } + incompleteLevels_ = DiagnosticLevels( + safe_declare(prefix_ + "diagnostic_incompletes_warn", 1), + safe_declare(prefix_ + "diagnostic_incompletes_error", 2)); + dropLevels_ = DiagnosticLevels( + safe_declare(prefix_ + "diagnostic_drops_warn", 1), + safe_declare(prefix_ + "diagnostic_drops_error", 2)); debug_ = safe_declare(prefix_ + "debug", false); adjustTimeStamp_ = safe_declare(prefix_ + "adjust_timestamp", false); useIEEE1588_ = safe_declare(prefix_ + "use_ieee_1588", false); @@ -519,6 +534,9 @@ void Camera::processImage(const ImageConstPtr & im) } else { droppedCount_++; } + if (imageArrivalDiagnostic_) { + imageArrivalDiagnostic_->tick(); + } } } @@ -659,6 +677,9 @@ void Camera::doPublish(const ImageConstPtr & im) } else { // const auto t0 = node_->now(); pub_.publish(std::move(img), std::move(cinfo)); + if (topicDiagnostic_) { + topicDiagnostic_->tick(t); + } // const auto t1 = node_->now(); // std::cout << "dt: " << (t1 - t0).nanoseconds() * 1e-9 << std::endl; publishedCount_++; @@ -673,6 +694,14 @@ void Camera::doPublish(const ImageConstPtr & im) metaMsg_.camera_time = im->imageTime_; metaPub_->publish(metaMsg_); } + numIncompletes_ += im->numIncomplete_; + if (lastFrameId_ == 0) { + lastFrameId_ = im->frameId_; + } + if (im->frameId_ != 0) { + numDrops_ += (im->frameId_ > lastFrameId_) ? (im->frameId_ - lastFrameId_ - 1) : 0; + } + lastFrameId_ = im->frameId_; } void Camera::printCameraInfo() @@ -748,6 +777,8 @@ bool Camera::start() break; } } + startDiagnostics(); // not clear exactly when diagnostics should be started + if (!foundCamera) { LOG_ERROR("giving up, camera " << serial_ << " not found!"); return (false); @@ -777,4 +808,64 @@ bool Camera::start() } return (true); } + +void Camera::startDiagnostics() +{ + const double period = safe_declare("diagnostic_period", -1.0); + if (period <= 0) { + return; + } + updater_ = std::make_shared(node_, period); + updater_->setHardwareID(serial_); + minFreqDiag_ = safe_declare("diagnostic_min_freq", -1.0); + maxFreqDiag_ = safe_declare("diagnostic_max_freq", -1.0); + if (minFreqDiag_ < 0 || maxFreqDiag_ < 0) { + BOMB_OUT("must set diagnostic_min_freq and diagnostic_max_freq parameters!"); + } + const int window_size = safe_declare("diagnostic_window", 10); + const double min_ts_diff = -1.0 / minFreqDiag_; + const double max_ts_diff = 1.0 / minFreqDiag_; + topicDiagnostic_ = std::make_shared( + topicPrefix_ + "image_raw", *updater_, + diagnostic_updater::FrequencyStatusParam(&minFreqDiag_, &maxFreqDiag_, 0, window_size), + diagnostic_updater::TimeStampStatusParam(min_ts_diff, max_ts_diff)); + imageArrivalDiagnostic_ = std::make_shared( + diagnostic_updater::FrequencyStatusParam(&minFreqDiag_, &maxFreqDiag_, 0, window_size), + "image_arrival"); + updater_->add(*imageArrivalDiagnostic_); + updater_->add(imageTask_); +} + +void Camera::updateDiagnosticsStatus( + DiagnosticStatusWrapper & status, const std::string & label, const int val, + const DiagnosticLevels & levels) +{ + if (val < levels.warning) { + status.summary(Status::OK, label + " is ok."); + } else if (val < levels.error) { + status.summary(Status::WARN, label + " is high!"); + } else { + status.summary(Status::ERROR, label + " too high!"); + } +} + +void Camera::incompleteDiagnostics(DiagnosticStatusWrapper & status) +{ + updateDiagnosticsStatus( + status, "incompletes", static_cast(numIncompletes_), incompleteLevels_); + numIncompletes_ = 0; +} + +void Camera::droppedDiagnostics(DiagnosticStatusWrapper & status) +{ + updateDiagnosticsStatus(status, "drops", static_cast(numDrops_), dropLevels_); + numDrops_ = 0; +} + +void Camera::stopDiagnostics() +{ + updater_.reset(); + topicDiagnostic_.reset(); + imageArrivalDiagnostic_.reset(); +} } // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp index ed1ba638..27be8bff 100644 --- a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp +++ b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp @@ -53,6 +53,8 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler double getReceiveFrameRate() const; double getIncompleteRate(); + size_t getNumIncompleteImages() const { return (numIncompleteImagesTotal_); } + size_t getNumDroppedImages() const { return (numDroppedImagesTotal_); } std::string getNodeMapAsString(); // methods for setting camera params @@ -95,6 +97,7 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler size_t numIncompleteImages_{0}; size_t numImagesTotal_{0}; size_t numIncompleteImagesTotal_{0}; + size_t numDroppedImagesTotal_{0}; Spinnaker::GevIEEE1588StatusEnums ptpStatus_{Spinnaker::GevIEEE1588Status_Disabled}; }; } // namespace spinnaker_camera_driver