diff --git a/spinnaker_camera_driver/CMakeLists.txt b/spinnaker_camera_driver/CMakeLists.txt index 86828bd1..522da368 100644 --- a/spinnaker_camera_driver/CMakeLists.txt +++ b/spinnaker_camera_driver/CMakeLists.txt @@ -26,7 +26,7 @@ set(CMAKE_CXX_EXTENSIONS OFF) set(CMAKE_POSITION_INDEPENDENT_CODE ON) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() if(NOT CMAKE_BUILD_TYPE) @@ -76,6 +76,11 @@ find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(yaml-cpp REQUIRED) +if(${image_transport_VERSION} VERSION_GREATER_EQUAL "6.4.0") + add_definitions(-DIMAGE_TRANSPORT_SUPPORTS_NODE_INTERFACES) + add_definitions(-DIMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE) +endif() + include_directories(SYSTEM ${SPINNAKER_INCLUDE_DIRS}) @@ -103,6 +108,7 @@ add_library(camera_driver SHARED src/image.cpp src/pixel_format.cpp src/genicam_utils.cpp + src/utils.cpp ) target_link_libraries(camera_driver @@ -112,6 +118,7 @@ target_link_libraries(camera_driver PRIVATE Spinnaker::Spinnaker) + target_include_directories(camera_driver PUBLIC $ $ diff --git a/spinnaker_camera_driver/doc/index.rst b/spinnaker_camera_driver/doc/index.rst index 18b2fa85..196c7c80 100644 --- a/spinnaker_camera_driver/doc/index.rst +++ b/spinnaker_camera_driver/doc/index.rst @@ -144,6 +144,8 @@ spinview program. *In addition to the parameters defined in the .yaml files*, the driver has the following ROS parameters: +- ``auto_start``: automatically configure and activate the driver when starting + as a lifecycle node. Default: true. - ``adjust_timestamp``: see `About time stamps`_ below for more documentation. Default: false. - ``acquisition_timeout``: timeout for expecting frames (in seconds). If no frame is received for this time, the driver restarts. Default @@ -161,6 +163,17 @@ files*, the driver has the following ROS parameters: while no ROS subscribers are present, but adds latency on subscription: after a subscription the first image will be published up to 1s later than without this option. +- ``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 - ``dump_node_map``: set this to true to get a dump of the node map. Default: false. - ``enable_external_control``: set this to true to enable external exposure control. @@ -179,17 +192,6 @@ 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. diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp index e33f4b8e..aa0dc61e 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include @@ -46,13 +47,21 @@ class Camera using CompositeDiagnosticsTask = diagnostic_updater::CompositeDiagnosticTask; using FunctionDiagnosticsTask = diagnostic_updater::FunctionDiagnosticTask; using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper; + /* + The Constructor takes a long list of interfaces to support older distros like Humble + */ explicit Camera( - rclcpp::Node * node, image_transport::ImageTransport * it, const std::string & prefix, - bool useStatus = true); + const std::shared_ptr &, + const std::shared_ptr &, + const std::shared_ptr &, + const std::shared_ptr &, + const std::shared_ptr &, + const std::shared_ptr &, + const std::shared_ptr &, + image_transport::ImageTransport * it, camera_info_manager::CameraInfoManager * im, + const std::string & logName, const std::string & prefix, bool useStatus = true); ~Camera(); - bool start(); - bool stop(); void setSynchronizer(const std::shared_ptr & s) { synchronizer_ = s; } void setExposureController(const std::shared_ptr & e) { @@ -61,6 +70,11 @@ class Camera const std::string & getName() const { return (name_); } const std::string & getPrefix() const { return (prefix_); } + bool configure(); // corresponds to lifecycle configure + bool activate(); // corresponds to lifecycle activate + bool deactivate(); // corresponds to lifecycle deactivate + bool deconfigure(); // corresponds to lifecycle cleanup + private: struct NodeInfo { @@ -73,8 +87,8 @@ class Camera void processImage(const ImageConstPtr & image); void readParameters(); void printCameraInfo(); - void startCamera(); - bool stopCamera(); + bool startStreaming(); + bool stopStreaming(); void createCameraParameters(); void setParameter(const NodeInfo & ni, const rclcpp::Parameter & p); bool setEnum(const std::string & nodeName, const std::string & v = ""); @@ -85,6 +99,14 @@ class Camera bool readParameterDefinitionFile(); void startDiagnostics(); void stopDiagnostics(); + void makePublishers(); + void destroyPublishers(); + void makeSubscribers(); + void destroySubscribers(); + void startTimers(); + void stopTimers(); + void openDevice(); + void closeDevice(); rclcpp::Time getAdjustedTimeStamp(uint64_t t, int64_t sensorTime); @@ -93,37 +115,41 @@ class Camera rcl_interfaces::msg::SetParametersResult parameterChanged( const std::vector & params); void controlCallback(const flir_camera_msgs::msg::CameraControl::UniquePtr msg); - void printStatus(); + void updateStatus(); void checkSubscriptions(); void doPublish(const ImageConstPtr & im); - rclcpp::Logger get_logger() - { - return rclcpp::get_logger( - !name_.empty() ? name_ : (serial_.empty() ? std::string("camera") : serial_)); - } + rclcpp::Logger get_logger() { return (rclcpp::get_logger(logName_)); } template T safe_declare(const std::string & name, const T & def) { try { - return (node_->declare_parameter(name, def)); + return ( + node_parameters_interface_->declare_parameter(name, rclcpp::ParameterValue(def)).get()); } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) { - T value(def); - (void)node_->get_parameter_or(name, value, def); - return (value); + const rclcpp::Parameter p = node_parameters_interface_->get_parameter(name); + return (p.get_parameter_value().get()); } } + template + T safePrefixedDeclare(const std::string & name, const T & def) + { + return (safe_declare(prefix_ + name, def)); + } + + auto clock() { return node_clock_interface_->get_clock(); } + auto name() { return node_base_interface_->get_name(); } void safe_declare( const std::string & name, const rclcpp::ParameterValue & pv, const rcl_interfaces::msg::ParameterDescriptor & desc) { try { - node_->declare_parameter(name, pv, desc, false); + node_parameters_interface_->declare_parameter(name, pv, desc, false); } catch (rclcpp::exceptions::InvalidParameterTypeException & e) { RCLCPP_WARN_STREAM( get_logger(), "overwriting bad param with default: " + std::string(e.what())); - node_->declare_parameter(name, pv, desc, true); + node_parameters_interface_->declare_parameter(name, pv, desc, true); } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) { // do nothing } @@ -133,15 +159,24 @@ class Camera const DiagnosticLevels & levels); void incompleteDiagnostics(DiagnosticStatusWrapper & status); void droppedDiagnostics(DiagnosticStatusWrapper & status); + void acquisitionDiagnostics(DiagnosticStatusWrapper & status); // ----- variables -- std::string prefix_; std::string topicPrefix_; - rclcpp::Node * node_; - image_transport::ImageTransport * imageTransport_; + std::shared_ptr node_base_interface_; + std::shared_ptr node_parameters_interface_; + std::shared_ptr node_logging_interface_; + std::shared_ptr node_timers_interface_; + std::shared_ptr node_clock_interface_; + std::shared_ptr node_topics_interface_; + std::shared_ptr node_services_interface_; + image_transport::ImageTransport * imageTransport_{nullptr}; + camera_info_manager::CameraInfoManager * infoManager_{nullptr}; image_transport::CameraPublisher pub_; rclcpp::Publisher::SharedPtr metaPub_; std::string serial_; + std::string logName_; std::string name_; std::string cameraInfoURL_; std::string frameId_; @@ -158,22 +193,21 @@ class Camera bool useIEEE1588_{false}; double minIEEE1588Offset_{0}; double maxIEEE1588Offset_{0.1}; - double ptpOffset_{0}; // in seconds - bool connectWhileSubscribed_{false}; // if true, connects to SDK when subscription happens + double ptpOffset_{0}; // in seconds + bool streamOnlyWhileSubscribed_{false}; // if true, streams from camera only while subscribed bool enableExternalControl_{false}; uint32_t currentExposureTime_{0}; double averageTimeDifference_{std::numeric_limits::quiet_NaN()}; int64_t baseTimeOffset_{0}; float currentGain_{std::numeric_limits::lowest()}; std::shared_ptr wrapper_; - std::shared_ptr infoManager_; sensor_msgs::msg::Image imageMsg_; sensor_msgs::msg::CameraInfo cameraInfoMsg_; flir_camera_msgs::msg::ImageMetaData metaMsg_; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr callbackHandle_; // keep alive callbacks rclcpp::TimerBase::SharedPtr statusTimer_; rclcpp::TimerBase::SharedPtr checkSubscriptionsTimer_; - bool cameraRunning_{false}; + bool cameraStreaming_{false}; std::mutex mutex_; std::condition_variable cv_; std::deque bufferQueue_; @@ -191,6 +225,7 @@ class Camera std::shared_ptr synchronizer_; std::shared_ptr exposureController_; bool firstSynchronizedFrame_{true}; + bool runStatusTimer_{false}; // --------- related to diagnostics std::shared_ptr updater_; std::shared_ptr topicDiagnostic_; @@ -205,6 +240,9 @@ class Camera DiagnosticLevels dropLevels_; uint64_t lastFrameId_{0}; uint64_t numDrops_{0}; + FunctionDiagnosticsTask acquisitionErrorTask_; + uint64_t acquisitionTimeouts_{0}; + bool acquisitionError_{false}; }; } // namespace spinnaker_camera_driver #endif // SPINNAKER_CAMERA_DRIVER__CAMERA_HPP_ diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera_driver.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera_driver.hpp index 19651a4b..b1afda2e 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera_driver.hpp +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera_driver.hpp @@ -16,22 +16,69 @@ #ifndef SPINNAKER_CAMERA_DRIVER__CAMERA_DRIVER_HPP_ #define SPINNAKER_CAMERA_DRIVER__CAMERA_DRIVER_HPP_ +#include #include #include #include #include +#include namespace spinnaker_camera_driver { -class CameraDriver : public rclcpp::Node +class CameraDriver : public NodeType { public: - explicit CameraDriver(const rclcpp::NodeOptions & options); - ~CameraDriver(); + using CameraInfoManager = camera_info_manager::CameraInfoManager; + using ImageTransport = image_transport::ImageTransport; + explicit CameraDriver(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~CameraDriver() override; +#ifdef IMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE +protected: + void preShutdown(); + CbReturn on_configure(const LCState & state) override; + CbReturn on_activate(const LCState & state) override; + CbReturn on_deactivate(const LCState & state) override; + CbReturn on_cleanup(const LCState & state) override; + CbReturn on_shutdown(const LCState & state) override; + CbReturn on_error(const LCState & state) override; + // helper methods + using CameraFnPtr = bool (Camera::*)(); + template + CbReturn changeState(F && f, const CameraFnPtr & cameraFn, const std::string & trans) + { + RCLCPP_INFO_STREAM(get_logger(), "requested transition: " << trans); + const auto ret = f(*this); + if (ret != CbReturn::SUCCESS) { + return (ret); + } + if (!camera_) { + return (CbReturn::FAILURE); + } + const bool v = (camera_.get()->*cameraFn)(); + return (v ? CbReturn::SUCCESS : CbReturn::FAILURE); + } +#endif private: - std::shared_ptr imageTransport_; + template + T safeDeclare(const std::string & name, const T & def) + { + try { + return (get_node_parameters_interface() + ->declare_parameter(name, rclcpp::ParameterValue(def)) + .get()); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) { + const rclcpp::Parameter p = get_node_parameters_interface()->get_parameter(name); + return (p.get_parameter_value().get()); + } + } + + void shutdown(); + // --------------- variables + std::shared_ptr imageTransport_; + std::shared_ptr infoManager_; std::shared_ptr camera_; + rclcpp::TimerBase::SharedPtr timer_; }; } // namespace spinnaker_camera_driver #endif // SPINNAKER_CAMERA_DRIVER__CAMERA_DRIVER_HPP_ diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/lifecycle_types.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/lifecycle_types.hpp new file mode 100644 index 00000000..1552eeae --- /dev/null +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/lifecycle_types.hpp @@ -0,0 +1,32 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2025 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__LIFECYCLE_TYPES_HPP_ +#define SPINNAKER_CAMERA_DRIVER__LIFECYCLE_TYPES_HPP_ + +#include + +#ifdef IMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE +#include +#include +using NodeType = rclcpp_lifecycle::LifecycleNode; +using LCState = rclcpp_lifecycle::State; +using CbReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +#else +using NodeType = rclcpp::Node; +#endif + +#endif // SPINNAKER_CAMERA_DRIVER__LIFECYCLE_TYPES_HPP_ diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp index 63776c15..d0c1a82e 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -28,6 +29,26 @@ class SpinnakerWrapperImpl; class SpinnakerWrapper { public: + struct Stats + { + Stats() = default; + explicit Stats(size_t nr, size_t ni, size_t ns, size_t nt) + : numberReceived(nr), numberIncomplete(ni), numberSkipped(ns), acquisitionTimeouts(nt) + { + } + void clear() + { + numberReceived = 0; + numberIncomplete = 0; + numberSkipped = 0; + acquisitionTimeouts = 0; + } + size_t numberReceived{0}; + size_t numberIncomplete{0}; + size_t numberSkipped{0}; + size_t acquisitionTimeouts{0}; + bool acquisitionError{false}; + }; struct Exception : public std::exception { explicit Exception(const std::string & what) : what_(what) {} @@ -38,7 +59,7 @@ class SpinnakerWrapper }; using Callback = std::function; - SpinnakerWrapper(); + explicit SpinnakerWrapper(rclcpp::Logger logger); std::string getLibraryVersion() const; void refreshCameraList(); @@ -52,11 +73,9 @@ class SpinnakerWrapper void setComputeBrightness(bool b); void setAcquisitionTimeout(double sec); void useIEEE1588(bool b); + void getAndClearStatistics(Stats * stats); std::string getIEEE1588Status() const; - std::string getPixelFormat() const; - double getReceiveFrameRate() const; - double getIncompleteRate(); std::string getNodeMapAsString(); std::string setEnum(const std::string & nodeName, const std::string & val, std::string * retVal); std::string setDouble(const std::string & nodeName, double val, double * retVal); diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/utils.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/utils.hpp new file mode 100644 index 00000000..7f152b77 --- /dev/null +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/utils.hpp @@ -0,0 +1,40 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2025 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__UTILS_HPP_ +#define SPINNAKER_CAMERA_DRIVER__UTILS_HPP_ + +#include +#include + +namespace spinnaker_camera_driver +{ +namespace utils +{ +#ifdef IMAGE_TRANSPORT_SUPPORTS_NODE_INTERFACES +std::shared_ptr makeCameraInfoManager( + const std::shared_ptr &, + const std::shared_ptr &, + const std::shared_ptr &, + const std::shared_ptr &, + const std::string & cameraName, const std::string & urlParameterName, uint32_t qSize); +#else +std::shared_ptr makeCameraInfoManager( + rclcpp::Node * node, const std::string & cameraName, const std::string & parameterName); +#endif + +} // namespace utils +} // namespace spinnaker_camera_driver +#endif // SPINNAKER_CAMERA_DRIVER__UTILS_HPP_ diff --git a/spinnaker_camera_driver/launch/driver_node.launch.py b/spinnaker_camera_driver/launch/driver_node.launch.py index ac3248d2..92ca96b6 100644 --- a/spinnaker_camera_driver/launch/driver_node.launch.py +++ b/spinnaker_camera_driver/launch/driver_node.launch.py @@ -168,6 +168,7 @@ def launch_setup(context, *args, **kwargs): package='spinnaker_camera_driver', executable='camera_driver_node', output='screen', + # prefix=['xterm -e gdb --args'], name=[LaunchConfig('camera_name')], parameters=[ example_parameters[camera_type], diff --git a/spinnaker_camera_driver/launch/gige_node.launch.py b/spinnaker_camera_driver/launch/gige_node.launch.py index 9db6c24b..e6267759 100644 --- a/spinnaker_camera_driver/launch/gige_node.launch.py +++ b/spinnaker_camera_driver/launch/gige_node.launch.py @@ -177,6 +177,7 @@ def launch_setup(context, *args, **kwargs): executable='camera_driver_node', output='screen', name=[LaunchConfig('camera_name')], + # prefix=["xterm -e gdb -e xrun --args"], parameters=[ example_parameters[camera_type], { diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp index 0447b4ae..9936533b 100644 --- a/spinnaker_camera_driver/src/camera.cpp +++ b/spinnaker_camera_driver/src/camera.cpp @@ -111,57 +111,216 @@ 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), + const std::shared_ptr & bi, + const std::shared_ptr & pi, + const std::shared_ptr & li, + const std::shared_ptr & ti, + const std::shared_ptr & ci, + const std::shared_ptr & oi, + const std::shared_ptr & si, + image_transport::ImageTransport * it, camera_info_manager::CameraInfoManager * im, + const std::string & logName, const std::string & prefix, bool useStatus) +: node_base_interface_(bi), + node_parameters_interface_(pi), + node_logging_interface_(li), + node_timers_interface_(ti), + node_clock_interface_(ci), + node_topics_interface_(oi), + node_services_interface_(si), + imageTransport_(it), + infoManager_(im), + logName_(logName), 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)) + "dropped frames", std::bind(&Camera::droppedDiagnostics, this, std::placeholders::_1)), + acquisitionErrorTask_( + "acquisition error", std::bind(&Camera::acquisitionDiagnostics, this, std::placeholders::_1)) { - imageTransport_ = it; prefix_ = prefix.empty() ? std::string("") : (prefix + "."); topicPrefix_ = prefix.empty() ? std::string("") : (prefix + "/"); - lastStatusTime_ = node_->now(); - if (useStatus) { - statusTimer_ = rclcpp::create_timer( - node_, node_->get_clock(), rclcpp::Duration(5, 0), std::bind(&Camera::printStatus, this)); - } + lastStatusTime_ = clock()->now(); + runStatusTimer_ = useStatus; imageTask_.addTask(&incompleteFrameTask_); imageTask_.addTask(&dropFrameTask_); } Camera::~Camera() { - stop(); - wrapper_.reset(); // invoke destructor + deactivate(); + deconfigure(); } -bool Camera::stop() +bool Camera::configure() { - stopDiagnostics(); - stopCamera(); - if (wrapper_) { - wrapper_->deInitCamera(); + readParameters(); + imageMsg_.header.frame_id = frameId_; + metaMsg_.header.frame_id = frameId_; + try { + if (!readParameterDefinitionFile()) { + return (false); + } + } catch (const YAML::Exception & e) { + LOG_ERROR("error reading parameter definitions: " << e.what()); + return (false); } - if (statusTimer_ && !statusTimer_->is_canceled()) { - statusTimer_->cancel(); + try { + makePublishers(); + cameraInfoMsg_ = infoManager_->getCameraInfo(); + cameraInfoMsg_.header.frame_id = frameId_; + imageMsg_.header.frame_id = frameId_; + metaMsg_.header.frame_id = frameId_; + openDevice(); + // Must create the camera parameters before acquisition is started. + // Some parameters (like blackfly s chunk control) cannot be set once + // the camera is running. + createCameraParameters(); + makeSubscribers(); + } catch (const std::exception & e) { + LOG_ERROR("configure() failed: " << e.what()); + deconfigure(); + return (false); } - keepRunning_ = false; - if (thread_) { - thread_->join(); - thread_ = 0; + return (true); +} + +bool Camera::activate() +{ + startTimers(); // must wait until publishers are created + keepRunning_ = true; + thread_ = std::make_shared(&Camera::run, this); + if (!streamOnlyWhileSubscribed_) { + if (!startStreaming()) { + return (false); + } } return (true); } -bool Camera::stopCamera() +bool Camera::startStreaming() +{ + if (!cameraStreaming_) { + cameraStreaming_ = + wrapper_->startCamera(std::bind(&Camera::processImage, this, std::placeholders::_1)); + if (!cameraStreaming_) { + LOG_ERROR("failed to start camera!"); + return (false); + } else { + startDiagnostics(); + printCameraInfo(); + } + } + return (true); +} + +void Camera::makePublishers() +{ + metaPub_ = rclcpp::create_publisher( + node_parameters_interface_, node_topics_interface_, "~/" + topicPrefix_ + "meta", + rclcpp::QoS(1)); + pub_ = imageTransport_->advertiseCamera("~/" + topicPrefix_ + "image_raw", qosDepth_); +} + +void Camera::makeSubscribers() +{ + if (enableExternalControl_) { + controlSub_ = rclcpp::create_subscription( + node_parameters_interface_, node_topics_interface_, "~/" + topicPrefix_ + "control", 10, + std::bind(&Camera::controlCallback, this, std::placeholders::_1)); + } +} + +void Camera::startTimers() +{ + stopTimers(); // cancel any timers started earlier + if (streamOnlyWhileSubscribed_) { + checkSubscriptionsTimer_ = rclcpp::create_timer( + node_base_interface_, node_timers_interface_, clock(), rclcpp::Duration(1, 0), + std::bind(&Camera::checkSubscriptions, this)); + } + + if (runStatusTimer_) { + if (statusTimer_) { + statusTimer_->cancel(); + } + statusTimer_ = rclcpp::create_timer( + node_base_interface_, node_timers_interface_, clock(), rclcpp::Duration(5, 0), + std::bind(&Camera::updateStatus, this)); + } +} + +static void cancelTimer(rclcpp::TimerBase::SharedPtr * timer) +{ + if (*timer) { + (*timer)->cancel(); + } + timer->reset(); +} + +void Camera::stopTimers() +{ + cancelTimer(&checkSubscriptionsTimer_); + cancelTimer(&statusTimer_); +} + +void Camera::openDevice() +{ + wrapper_ = std::make_shared(get_logger()); + wrapper_->setDebug(debug_); + wrapper_->setComputeBrightness(computeBrightness_); + wrapper_->setAcquisitionTimeout(acquisitionTimeout_); + wrapper_->useIEEE1588(useIEEE1588_); + + LOG_INFO("using spinnaker lib version: " + wrapper_->getLibraryVersion()); + bool foundCamera = false; + for (int retry = 1; retry < 6; retry++) { + wrapper_->refreshCameraList(); + const auto camList = wrapper_->getSerialNumbers(); + if (std::find(camList.begin(), camList.end(), serial_) == camList.end()) { + LOG_WARN("no camera found with serial: " << serial_ << " on try # " << retry); + for (const auto & cam : camList) { + LOG_WARN(" instead found camera: " << cam); + } + std::this_thread::sleep_for(chrono::seconds(1)); + } else { + LOG_INFO("found camera with serial number: " << serial_); + foundCamera = true; + break; + } + } + if (!foundCamera) { + LOG_ERROR("giving up, camera " << serial_ << " not found!"); + throw std::runtime_error("camera " + serial_ + " not found!"); + } + if (!wrapper_->initCamera(serial_)) { + LOG_ERROR("camera " << serial_ << " init failed!"); + throw std::runtime_error("camera " + serial_ + "init failed!"); + } + if (dumpNodeMap_) { + LOG_INFO("dumping node map!"); + std::string nm = wrapper_->getNodeMapAsString(); + std::cout << nm; + } +} + +void Camera::closeDevice() +{ + if (wrapper_) { + wrapper_->deInitCamera(); + } + wrapper_.reset(); +} + +bool Camera::stopStreaming() { - if (cameraRunning_ && wrapper_) { - cameraRunning_ = false; - return wrapper_->stopCamera(); + if (cameraStreaming_ && wrapper_) { + if (wrapper_->stopCamera()) { + cameraStreaming_ = false; + stopDiagnostics(); + return (true); + } } return false; } @@ -181,28 +340,36 @@ std::ostream & operator<<(std::ostream & o, const ffmt & f) return (o); } -void Camera::printStatus() +void Camera::updateStatus() { if (wrapper_) { - const double dropRate = - (queuedCount_ > 0) ? (static_cast(droppedCount_) / static_cast(queuedCount_)) - : 0; - const rclcpp::Time t = node_->now(); + const rclcpp::Time t = clock()->now(); const rclcpp::Duration dt = t - lastStatusTime_; const double dtns = std::max(dt.nanoseconds(), (int64_t)1); const double outRate = publishedCount_ * 1e9 / dtns; - const double incompleteRate = wrapper_->getIncompleteRate(); + SpinnakerWrapper::Stats stats; + wrapper_->getAndClearStatistics(&stats); + acquisitionTimeouts_ += stats.acquisitionTimeouts; + acquisitionError_ = stats.acquisitionError; + const double inRate = stats.numberReceived * 1e9 / dtns; + const double dropRate = + (queuedCount_ > 0) ? (static_cast(droppedCount_) / static_cast(queuedCount_)) + : 0; std::stringstream ss; - ss << "rate [Hz] in " << ffmt(6, 2) << wrapper_->getReceiveFrameRate() << " out " << ffmt(6, 2) - << outRate << " drop " << ffmt(3, 0) << dropRate * 100; + ss << "IN: " << ffmt(6, 2) << inRate << " Hz "; + ss << " OUT: " << ffmt(6, 2) << outRate; + ss << " Hz drop " << ffmt(3, 0) << dropRate * 100 << "%"; if (useIEEE1588_) { ss << " " << wrapper_->getIEEE1588Status() << " off[s]: " << ffmt(6, 4) << ptpOffset_; } - if (incompleteRate != 0) { - ss << " INCOMPLETE: " << ffmt(3, 0) << incompleteRate * 100; + if (stats.numberIncomplete != 0) { + ss << " INCOMPLETE: " << ffmt(3, 0) << stats.numberIncomplete; + } + if (stats.numberSkipped != 0) { + ss << " SKIPPED: " << ffmt(3, 0) << stats.numberSkipped; } if ( - incompleteRate != 0 || + stats.numberIncomplete != 0 || stats.numberSkipped != 0 || (useIEEE1588_ && (ptpOffset_ > maxIEEE1588Offset_ || ptpOffset_ < minIEEE1588Offset_))) { LOG_WARN(ss.str()); } else { @@ -220,14 +387,14 @@ void Camera::printStatus() void Camera::checkSubscriptions() { - if (connectWhileSubscribed_) { + if (streamOnlyWhileSubscribed_) { if (pub_.getNumSubscribers() > 0 || metaPub_->get_subscription_count() != 0) { - if (!cameraRunning_) { - startCamera(); + if (!cameraStreaming_) { + startStreaming(); } } else { - if (cameraRunning_) { - stopCamera(); + if (cameraStreaming_) { + stopStreaming(); } } } @@ -259,18 +426,18 @@ void Camera::readParameters() } minIEEE1588Offset_ = safe_declare(prefix_ + "min_ieee_1588_offset", 0); maxIEEE1588Offset_ = safe_declare(prefix_ + "max_ieee_1588_offset", 0.1); - cameraInfoURL_ = safe_declare(prefix_ + "camerainfo_url", ""); - frameId_ = safe_declare(prefix_ + "frame_id", node_->get_name()); + frameId_ = safe_declare(prefix_ + "frame_id", name()); dumpNodeMap_ = safe_declare(prefix_ + "dump_node_map", false); qosDepth_ = safe_declare(prefix_ + "image_queue_size", 4); maxBufferQueueSize_ = static_cast(safe_declare(prefix_ + "buffer_queue_size", 4)); computeBrightness_ = safe_declare(prefix_ + "compute_brightness", false); acquisitionTimeout_ = safe_declare(prefix_ + "acquisition_timeout", 3.0); parameterFile_ = safe_declare(prefix_ + "parameter_file", "parameters.yaml"); - connectWhileSubscribed_ = safe_declare(prefix_ + "connect_while_subscribed", false); + streamOnlyWhileSubscribed_ = safe_declare(prefix_ + "connect_while_subscribed", false); enableExternalControl_ = safe_declare(prefix_ + "enable_external_control", false); - callbackHandle_ = node_->add_on_set_parameters_callback( + callbackHandle_ = node_parameters_interface_->add_on_set_parameters_callback( std::bind(&Camera::parameterChanged, this, std::placeholders::_1)); + cameraInfoURL_ = safe_declare(prefix_ + "camerainfo_url", ""); } bool Camera::readParameterDefinitionFile() @@ -677,9 +844,6 @@ 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_++; @@ -702,111 +866,48 @@ void Camera::doPublish(const ImageConstPtr & im) numDrops_ += (im->frameId_ > lastFrameId_) ? (im->frameId_ - lastFrameId_ - 1) : 0; } lastFrameId_ = im->frameId_; + if (topicDiagnostic_) { + topicDiagnostic_->tick(t); + } } void Camera::printCameraInfo() { - if (cameraRunning_) { + if (wrapper_ && cameraStreaming_) { LOG_INFO("camera has pixel format: " << wrapper_->getPixelFormat()); } } -void Camera::startCamera() +bool Camera::deactivate() { - if (!cameraRunning_) { - spinnaker_camera_driver::SpinnakerWrapper::Callback cb = - std::bind(&Camera::processImage, this, std::placeholders::_1); - cameraRunning_ = wrapper_->startCamera(cb); - if (!cameraRunning_) { - LOG_ERROR("failed to start camera!"); - } else { - printCameraInfo(); - } + stopTimers(); + stopStreaming(); + { + std::unique_lock lock(mutex_); + keepRunning_ = false; + cv_.notify_all(); + } + if (thread_) { + thread_->join(); + thread_.reset(); } + return (true); } -bool Camera::start() +bool Camera::deconfigure() { - readParameters(); - try { - if (!readParameterDefinitionFile()) { - return (false); - } - } catch (const YAML::Exception & e) { - LOG_ERROR("error reading parameter definitions: " << e.what()); - return (false); - } - - infoManager_ = std::make_shared( - node_, name_.empty() ? node_->get_name() : name_, cameraInfoURL_); - if (enableExternalControl_) { - controlSub_ = node_->create_subscription( - "~/" + topicPrefix_ + "control", 10, - std::bind(&Camera::controlCallback, this, std::placeholders::_1)); - } - metaPub_ = - node_->create_publisher("~/" + topicPrefix_ + "meta", 1); - - cameraInfoMsg_ = infoManager_->getCameraInfo(); - imageMsg_.header.frame_id = frameId_; - cameraInfoMsg_.header.frame_id = frameId_; - metaMsg_.header.frame_id = frameId_; - - pub_ = imageTransport_->advertiseCamera("~/" + topicPrefix_ + "image_raw", qosDepth_); - - wrapper_ = std::make_shared(); - wrapper_->setDebug(debug_); - wrapper_->setComputeBrightness(computeBrightness_); - wrapper_->setAcquisitionTimeout(acquisitionTimeout_); - wrapper_->useIEEE1588(useIEEE1588_); - - LOG_INFO("using spinnaker lib version: " + wrapper_->getLibraryVersion()); - bool foundCamera = false; - for (int retry = 1; retry < 6; retry++) { - wrapper_->refreshCameraList(); - const auto camList = wrapper_->getSerialNumbers(); - if (std::find(camList.begin(), camList.end(), serial_) == camList.end()) { - LOG_WARN("no camera found with serial: " << serial_ << " on try # " << retry); - for (const auto & cam : camList) { - LOG_WARN(" found cameras: " << cam); - } - std::this_thread::sleep_for(chrono::seconds(1)); - } else { - LOG_INFO("found camera with serial number: " << serial_); - foundCamera = true; - break; - } - } - startDiagnostics(); // not clear exactly when diagnostics should be started + destroySubscribers(); + closeDevice(); + destroyPublishers(); + return (true); +} - if (!foundCamera) { - LOG_ERROR("giving up, camera " << serial_ << " not found!"); - return (false); - } - keepRunning_ = true; - thread_ = std::make_shared(&Camera::run, this); +void Camera::destroySubscribers() { controlSub_.reset(); } - if (wrapper_->initCamera(serial_)) { - if (dumpNodeMap_) { - LOG_INFO("dumping node map!"); - std::string nm = wrapper_->getNodeMapAsString(); - std::cout << nm; - } - // Must first create the camera parameters before acquisition is started. - // Some parameters (like blackfly s chunk control) cannot be set once - // the camera is running. - createCameraParameters(); - if (!connectWhileSubscribed_) { - startCamera(); - } else { - checkSubscriptionsTimer_ = rclcpp::create_timer( - node_, node_->get_clock(), rclcpp::Duration(1, 0), - std::bind(&Camera::checkSubscriptions, this)); - } - } else { - LOG_ERROR("init camera failed for cam: " << serial_); - } - return (true); +void Camera::destroyPublishers() +{ + metaPub_.reset(); + pub_.shutdown(); } void Camera::startDiagnostics() @@ -815,7 +916,9 @@ void Camera::startDiagnostics() if (period <= 0) { return; } - updater_ = std::make_shared(node_, period); + updater_ = std::make_shared( + node_base_interface_, node_clock_interface_, node_logging_interface_, + node_parameters_interface_, node_timers_interface_, node_topics_interface_, period); updater_->setHardwareID(serial_); minFreqDiag_ = safe_declare("diagnostic_min_freq", -1.0); maxFreqDiag_ = safe_declare("diagnostic_max_freq", -1.0); @@ -834,6 +937,7 @@ void Camera::startDiagnostics() "image_arrival"); updater_->add(*imageArrivalDiagnostic_); updater_->add(imageTask_); + updater_->add(acquisitionErrorTask_); } void Camera::updateDiagnosticsStatus( @@ -862,6 +966,16 @@ void Camera::droppedDiagnostics(DiagnosticStatusWrapper & status) numDrops_ = 0; } +void Camera::acquisitionDiagnostics(DiagnosticStatusWrapper & status) +{ + if (acquisitionTimeouts_ != 0 || acquisitionError_) { + status.summary(Status::ERROR, "acquisition error!"); + } else { + status.summary(Status::OK, "acquisition ok!"); + } + acquisitionTimeouts_ = 0; // clear number of timeouts +} + void Camera::stopDiagnostics() { updater_.reset(); diff --git a/spinnaker_camera_driver/src/camera_driver.cpp b/spinnaker_camera_driver/src/camera_driver.cpp index 389776d8..fc08b999 100644 --- a/spinnaker_camera_driver/src/camera_driver.cpp +++ b/spinnaker_camera_driver/src/camera_driver.cpp @@ -18,20 +18,121 @@ #include #include #include +#include namespace spinnaker_camera_driver { -CameraDriver::CameraDriver(const rclcpp::NodeOptions & options) : Node("camera_driver", options) +CameraDriver::CameraDriver(const rclcpp::NodeOptions & options) : NodeType("camera_driver", options) { - imageTransport_ = std::make_shared( - std::shared_ptr(this, [](auto *) {})); - camera_ = std::make_shared(this, imageTransport_.get(), ""); - if (!camera_->start()) { - LOG_ERROR("startup failed!"); + const std::string cameraName = get_node_base_interface()->get_name(); +#ifdef IMAGE_TRANSPORT_SUPPORTS_NODE_INTERFACES + imageTransport_ = std::make_shared(image_transport::RequiredInterfaces(*this)); + infoManager_ = utils::makeCameraInfoManager( + get_node_base_interface(), get_node_parameters_interface(), get_node_logging_interface(), + get_node_services_interface(), cameraName, "camerainfo_url", 10); +#else + imageTransport_ = + std::make_shared(std::shared_ptr(this, [](auto *) {})); + infoManager_ = utils::makeCameraInfoManager(this, cameraName, "camerainfo_url"); +#endif + camera_ = std::make_shared( + get_node_base_interface(), get_node_parameters_interface(), get_node_logging_interface(), + get_node_timers_interface(), get_node_clock_interface(), get_node_topics_interface(), + get_node_services_interface(), imageTransport_.get(), infoManager_.get(), cameraName, ""); +#ifdef IMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE + get_node_base_interface()->get_context()->add_pre_shutdown_callback( + std::bind(&CameraDriver::preShutdown, this)); + + if (declare_parameter("auto_start", true)) { + // defer because one cannot call some of the required methods inside the constructor + timer_ = create_wall_timer(std::chrono::microseconds(0), [this]() -> void { + timer_->cancel(); + rclcpp_lifecycle::LifecycleNode::configure(); + rclcpp_lifecycle::LifecycleNode::activate(); + }); + } +#else + camera_->configure(); + camera_->activate(); +#endif +} + +CameraDriver::~CameraDriver() +{ + if (timer_) { + timer_->cancel(); + timer_->reset(); + } + shutdown(); // no harm, should be idempotent +} + +void CameraDriver::shutdown() +{ + if (camera_) { + camera_->deactivate(); + camera_->deconfigure(); + try { + camera_.reset(); + imageTransport_.reset(); + infoManager_.reset(); + } catch (const std::exception & e) { + LOG_ERROR("error during shutdown: " << e.what()); + } + LOG_INFO("shutdown complete."); } } -CameraDriver::~CameraDriver() {} +#ifdef IMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE + +void CameraDriver::preShutdown() { rclcpp_lifecycle::LifecycleNode::shutdown(); } + +CbReturn CameraDriver::on_configure(const LCState & s) +{ + return (changeState( + [s](CameraDriver & ob) -> CbReturn { return (ob.NodeType::on_configure(s)); }, + &Camera::configure, "configure")); +} + +CbReturn CameraDriver::on_activate(const LCState & s) +{ + return (changeState( + [s](CameraDriver & ob) -> CbReturn { return (ob.NodeType::on_activate(s)); }, &Camera::activate, + "activate")); +} + +CbReturn CameraDriver::on_deactivate(const LCState & s) +{ + return (changeState( + [s](CameraDriver & ob) -> CbReturn { return (ob.NodeType::on_deactivate(s)); }, + &Camera::deactivate, "deactivate")); +} + +CbReturn CameraDriver::on_cleanup(const LCState & s) +{ + auto ret = changeState( + [s](CameraDriver & ob) -> CbReturn { return (ob.NodeType::on_cleanup(s)); }, + &Camera::deconfigure, "cleanup"); + return (ret); +} + +CbReturn CameraDriver::on_shutdown(const LCState & state) +{ + auto ret = NodeType::on_shutdown(state); + if (ret != CbReturn::SUCCESS) { + return (ret); + } + shutdown(); + return CbReturn::SUCCESS; +} + +CbReturn CameraDriver::on_error(const LCState & state) +{ + LOG_ERROR("got error state: " << state.label()); + return (CbReturn::FAILURE); +} + +#endif + } // namespace spinnaker_camera_driver RCLCPP_COMPONENTS_REGISTER_NODE(spinnaker_camera_driver::CameraDriver) diff --git a/spinnaker_camera_driver/src/camera_driver_node.cpp b/spinnaker_camera_driver/src/camera_driver_node.cpp index ef93e6a8..4c6445a5 100644 --- a/spinnaker_camera_driver/src/camera_driver_node.cpp +++ b/spinnaker_camera_driver/src/camera_driver_node.cpp @@ -22,9 +22,9 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); auto node = std::make_shared(rclcpp::NodeOptions()); - RCLCPP_INFO(node->get_logger(), "camera_driver_node started up!"); - - rclcpp::spin(node); // should not return + RCLCPP_INFO(node->get_logger(), "camera driver node started up!"); + rclcpp::spin(node->get_node_base_interface()); // should not return + node.reset(); rclcpp::shutdown(); return 0; } diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper.cpp b/spinnaker_camera_driver/src/spinnaker_wrapper.cpp index 83f951f5..db5cf86a 100644 --- a/spinnaker_camera_driver/src/spinnaker_wrapper.cpp +++ b/spinnaker_camera_driver/src/spinnaker_wrapper.cpp @@ -22,7 +22,10 @@ namespace spinnaker_camera_driver { -SpinnakerWrapper::SpinnakerWrapper() { wrapperImpl_.reset(new SpinnakerWrapperImpl()); } +SpinnakerWrapper::SpinnakerWrapper(rclcpp::Logger logger) +{ + wrapperImpl_.reset(new SpinnakerWrapperImpl(logger)); +} std::string SpinnakerWrapper::getLibraryVersion() const { @@ -48,11 +51,11 @@ bool SpinnakerWrapper::startCamera(const Callback & cb) { return wrapperImpl_->s bool SpinnakerWrapper::stopCamera() { return wrapperImpl_->stopCamera(); } std::string SpinnakerWrapper::getPixelFormat() const { return wrapperImpl_->getPixelFormat(); } -double SpinnakerWrapper::getReceiveFrameRate() const + +void SpinnakerWrapper::getAndClearStatistics(Stats * stats) { - return (wrapperImpl_->getReceiveFrameRate()); + wrapperImpl_->getAndClearStatistics(stats); } -double SpinnakerWrapper::getIncompleteRate() { return (wrapperImpl_->getIncompleteRate()); } std::string SpinnakerWrapper::getNodeMapAsString() { return (wrapperImpl_->getNodeMapAsString()); } diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp index f53936ef..143b2da6 100644 --- a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp +++ b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp @@ -93,12 +93,12 @@ static bool set_acquisition_mode_continuous(GenApi::INodeMap & nodeMap) return false; } -SpinnakerWrapperImpl::SpinnakerWrapperImpl() +SpinnakerWrapperImpl::SpinnakerWrapperImpl(rclcpp::Logger logger) : logger_(logger) { system_ = Spinnaker::System::GetInstance(); if (!system_) { - std::cerr << "cannot instantiate spinnaker driver!" << std::endl; - throw std::runtime_error("failed to get spinnaker driver!"); + LOG_ERROR("cannot instantiate spinnaker SDK!"); + throw std::runtime_error("failed to instantiate spinnaker SDK!"); } refreshCameraList(); } @@ -116,7 +116,7 @@ SpinnakerWrapperImpl::~SpinnakerWrapperImpl() keepRunning_ = false; stopCamera(); deInitCamera(); - camera_ = 0; // call destructor, may not be needed + camera_ = nullptr; // call destructor, may not be needed cameraList_.Clear(); if (system_) { system_->ReleaseInstance(); @@ -144,6 +144,7 @@ std::vector SpinnakerWrapperImpl::getSerialNumbers() const std::string SpinnakerWrapperImpl::setEnum( const std::string & nodeName, const std::string & val, std::string * retVal) { + Lock lock(cameraMutex_); *retVal = "UNKNOWN"; const auto np = genicam_utils::find_node(nodeName, camera_, debug_); if (!np) { @@ -168,12 +169,12 @@ std::string SpinnakerWrapperImpl::setEnum( } } if (debug_) { - std::cout << "node " << nodeName << " invalid enum: " << val << std::endl; - std::cout << "allowed enum values: " << std::endl; + LOG_WARN("node " << nodeName << " invalid enum: " << val); + LOG_WARN("allowed enum values: "); GenApi::StringList_t validValues; p->GetSymbolics(validValues); for (const auto & ve : validValues) { - std::cout << " " << ve << std::endl; + LOG_WARN(" " << ve); } } return ("node " + nodeName + " invalid enum: " + val); @@ -231,23 +232,27 @@ static std::string set_parameter( std::string SpinnakerWrapperImpl::setDouble(const std::string & nn, double val, double * retVal) { *retVal = std::nan(""); + Lock lock(cameraMutex_); return (set_parameter(nn, val, retVal, camera_, debug_)); } std::string SpinnakerWrapperImpl::setBool(const std::string & nn, bool val, bool * retVal) { + Lock lock(cameraMutex_); *retVal = !val; return (set_parameter(nn, val, retVal, camera_, debug_)); } std::string SpinnakerWrapperImpl::setInt(const std::string & nn, int val, int * retVal) { + Lock lock(cameraMutex_); *retVal = -1; return (set_parameter(nn, val, retVal, camera_, debug_)); } std::string SpinnakerWrapperImpl::execute(const std::string & nn) { + Lock lock(cameraMutex_); const auto np = genicam_utils::find_node(nn, camera_, debug_, true); if (!np) { return ("node " + nn + " not found!"); @@ -263,20 +268,6 @@ std::string SpinnakerWrapperImpl::execute(const std::string & nn) return ("OK"); } -double SpinnakerWrapperImpl::getReceiveFrameRate() const -{ - return (avgTimeInterval_ > 0 ? (1.0 / avgTimeInterval_) : 0); -} - -double SpinnakerWrapperImpl::getIncompleteRate() -{ - const double r = - numImagesTotal_ == 0 ? 0 : (numIncompleteImagesTotal_ / static_cast(numImagesTotal_)); - numImagesTotal_ = 0; - numIncompleteImagesTotal_ = 0; - return (r); -} - static int int_ceil(size_t x, int y) { // compute the integer ceil(x / y) @@ -319,28 +310,27 @@ std::string SpinnakerWrapperImpl::getIEEE1588Status() const return (it == clockStatusMap.end() ? "INV" : it->second); } +void SpinnakerWrapperImpl::getAndClearStatistics(SpinnakerWrapper::Stats * stats) +{ + // lock down the statistics + Lock lock(statusMonitorMutex_); + *stats = stats_; + stats_.clear(); +} + void SpinnakerWrapperImpl::OnImageEvent(Spinnaker::ImagePtr imgPtr) { + // make sure the status monitor is not modifying the camera + // or other shared state like lastTime_ + Lock lock(statusMonitorMutex_); // update frame rate auto now = chrono::high_resolution_clock::now(); const uint64_t t = chrono::duration_cast(now.time_since_epoch()).count(); - if (avgTimeInterval_ == 0) { - if (lastTime_ != 0) { - avgTimeInterval_ = (t - lastTime_) * 1e-9; - } - } else { - const double dt = (t - lastTime_) * 1e-9; - const double alpha = 0.01; - avgTimeInterval_ = avgTimeInterval_ * (1.0 - alpha) + dt * alpha; - } - { - std::unique_lock lock(mutex_); - lastTime_ = t; - } - numImagesTotal_++; + lastTime_ = t; + stats_.numberReceived++; if (imgPtr->IsIncomplete()) { + stats_.numberIncomplete++; numIncompleteImages_++; - numIncompleteImagesTotal_++; } else { float expTime = 0; float gain = 0; @@ -358,6 +348,7 @@ void SpinnakerWrapperImpl::OnImageEvent(Spinnaker::ImagePtr imgPtr) const uint32_t maxExpTime = static_cast(is_readable(exposureTimeNode_) ? exposureTimeNode_->GetMax() : 0); if (useIEEE1588_) { + Lock lock(cameraMutex_); ptpStatus_ = camera_->GevIEEE1588Status(); } #if 0 @@ -386,6 +377,11 @@ void SpinnakerWrapperImpl::OnImageEvent(Spinnaker::ImagePtr imgPtr) imgPtr->GetImageStatus(), imgPtr->GetData(), imgPtr->GetWidth(), imgPtr->GetHeight(), imgPtr->GetStride(), imgPtr->GetBitsPerPixel(), imgPtr->GetNumChannels(), imgPtr->GetFrameID(), pixelFormat_, numIncompleteImages_)); + const size_t fid = imgPtr->GetFrameID(); + if (lastFrameId_ != 0 && fid > lastFrameId_) { + stats_.numberSkipped += fid - lastFrameId_ - 1; + } + lastFrameId_ = fid; numIncompleteImages_ = 0; callback_(img); } @@ -393,6 +389,7 @@ void SpinnakerWrapperImpl::OnImageEvent(Spinnaker::ImagePtr imgPtr) bool SpinnakerWrapperImpl::initCamera(const std::string & serialNumber) { + Lock lock(cameraMutex_); if (camera_) { return false; } @@ -445,6 +442,7 @@ bool SpinnakerWrapperImpl::initCamera(const std::string & serialNumber) bool SpinnakerWrapperImpl::deInitCamera() { + Lock lock(cameraMutex_); if (!camera_) { return (false); } @@ -454,6 +452,7 @@ bool SpinnakerWrapperImpl::deInitCamera() bool SpinnakerWrapperImpl::startCamera(const SpinnakerWrapper::Callback & cb) { + Lock lock(cameraMutex_); if (!camera_ || cameraRunning_) { return false; } @@ -463,19 +462,23 @@ bool SpinnakerWrapperImpl::startCamera(const SpinnakerWrapper::Callback & cb) if (set_acquisition_mode_continuous(nodeMap)) { camera_->RegisterEventHandler(*this); camera_->BeginAcquisition(); - thread_ = std::make_shared(&SpinnakerWrapperImpl::monitorStatus, this); - cameraRunning_ = true; + { + Lock lock(statusMonitorMutex_); + keepRunning_ = true; + thread_ = std::make_shared(&SpinnakerWrapperImpl::monitorStatus, this); + cameraRunning_ = true; + } GenApi::CEnumerationPtr ptrPixelFormat = nodeMap.GetNode("PixelFormat"); if (GenApi::IsAvailable(ptrPixelFormat)) { setPixelFormat(ptrPixelFormat->GetCurrentEntry()->GetSymbolic().c_str()); } else { setPixelFormat("BayerRG8"); - std::cerr << "WARNING: driver could not read pixel format!" << std::endl; + LOG_WARN("driver could not read pixel format!"); } exposureTimeNode_ = nodeMap.GetNode("ExposureTime"); } else { - std::cerr << "failed to switch on continuous acquisition!" << std::endl; + LOG_ERROR("failed to switch on continuous acquisition!"); return (false); } callback_ = cb; @@ -484,18 +487,44 @@ bool SpinnakerWrapperImpl::startCamera(const SpinnakerWrapper::Callback & cb) bool SpinnakerWrapperImpl::stopCamera() { - if (camera_ && cameraRunning_) { - if (thread_) { - keepRunning_ = false; - thread_->join(); - thread_ = 0; + if (!cameraRunning_) { + return (true); + } else { + LOG_INFO("stopping camera..."); + } + Lock statusLock(statusMonitorMutex_); + Lock cameraLock(cameraMutex_); + int numRetries = 10; + for (int i = 0; i < numRetries; i++) { + if (camera_ && cameraRunning_) { + if (thread_) { + keepRunning_ = false; + statusMonitorCv_.notify_all(); + statusLock.unlock(); // so the status thread can grab it + thread_->join(); + thread_ = 0; + statusLock.lock(); // status thread is done + } + try { + // unregister the event handler first to avoid the case + // where the driver is in OnImageEvent() when EndAcquisition() is called. + try { + camera_->UnregisterEventHandler(*this); + } catch (const Spinnaker::Exception & e) { + // ignore complaints that no events are registered + } + camera_->EndAcquisition(); // before unregistering the event handler! + cameraRunning_ = false; + LOG_INFO("camera stopped successfully."); + return true; + } catch (const std::exception & e) { + LOG_ERROR( + "failed to stop camera: " << e.what() << " will retry " << numRetries - 1 - i + << " more times."); + } } - camera_->EndAcquisition(); // before unregistering the event handler! - camera_->UnregisterEventHandler(*this); - - cameraRunning_ = false; - return true; } + LOG_WARN("cannot stop camera"); return (false); } @@ -518,23 +547,42 @@ std::string SpinnakerWrapperImpl::getNodeMapAsString() void SpinnakerWrapperImpl::monitorStatus() { + Lock statusLock(statusMonitorMutex_); + const int monitor_period_sec = 1; while (keepRunning_) { - std::this_thread::sleep_for(chrono::seconds(1)); - uint64_t lastTime; - { - std::unique_lock lock(mutex_); - lastTime = lastTime_; - } - auto now = chrono::high_resolution_clock::now(); - uint64_t t = chrono::duration_cast(now.time_since_epoch()).count(); - if (t - lastTime > acquisitionTimeout_ && camera_) { - std::cout << "WARNING: acquisition timeout, restarting!" << std::endl; - // Mucking with the camera in this thread without proper - // locking does not feel good. Expect some rare crashes. - camera_->EndAcquisition(); - camera_->BeginAcquisition(); + statusMonitorCv_.wait_until( + statusLock, chrono::system_clock::now() + chrono::seconds(monitor_period_sec)); + const uint64_t t_now = + chrono::duration_cast(chrono::system_clock::now().time_since_epoch()) + .count(); + if (t_now - lastTime_ > acquisitionTimeout_) { + Lock lock(cameraMutex_); + if (camera_) { + LOG_WARN("Acquisition timeout, restarting streaming!"); + try { + try { + camera_->UnregisterEventHandler(*this); + } catch (const Spinnaker::Exception & e) { + // ignore complaints that no events are registered + } + try { + camera_->EndAcquisition(); + } catch (const Spinnaker::Exception & e) { + // ignore complaints that acquisition is not running + } + camera_->RegisterEventHandler(*this); + camera_->BeginAcquisition(); + } catch (const Spinnaker::Exception & e) { + LOG_WARN("restart attempt failed with error: " << e.what()); + } + } + stats_.acquisitionTimeouts++; + stats_.acquisitionError = true; + } else { + stats_.acquisitionError = false; } } + LOG_INFO("status monitoring thread exited."); } } // 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 27be8bff..d35bd439 100644 --- a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp +++ b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -33,7 +34,7 @@ namespace spinnaker_camera_driver class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler { public: - SpinnakerWrapperImpl(); + explicit SpinnakerWrapperImpl(rclcpp::Logger logger); ~SpinnakerWrapperImpl(); // ------- inherited methods // from ImageEventHandler @@ -51,10 +52,7 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler bool startCamera(const SpinnakerWrapper::Callback & cb); bool stopCamera(); - double getReceiveFrameRate() const; - double getIncompleteRate(); - size_t getNumIncompleteImages() const { return (numIncompleteImagesTotal_); } - size_t getNumDroppedImages() const { return (numDroppedImagesTotal_); } + void getAndClearStatistics(SpinnakerWrapper::Stats * stats); std::string getNodeMapAsString(); // methods for setting camera params @@ -70,18 +68,19 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler std::string getIEEE1588Status() const; private: + using Lock = std::unique_lock; void setPixelFormat(const std::string & pixFmt); bool setInINodeMap(double f, const std::string & field, double * fret); void monitorStatus(); - rclcpp::Logger get_logger() { return rclcpp::get_logger("Spinnaker Wrapper"); } + auto & get_logger() const { return (logger_); } // ----- variables -- + rclcpp::Logger logger_; Spinnaker::SystemPtr system_; Spinnaker::CameraList cameraList_; Spinnaker::CameraPtr camera_; SpinnakerWrapper::Callback callback_; - double avgTimeInterval_{0}; uint64_t lastTime_{0}; bool cameraRunning_{false}; bool debug_{false}; @@ -92,12 +91,13 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler Spinnaker::GenApi::CFloatPtr exposureTimeNode_; bool keepRunning_{true}; std::shared_ptr thread_; - std::mutex mutex_; + std::mutex statusMonitorMutex_; + std::condition_variable statusMonitorCv_; + std::mutex cameraMutex_; uint64_t acquisitionTimeout_{10000000000ULL}; + SpinnakerWrapper::Stats stats_; size_t numIncompleteImages_{0}; - size_t numImagesTotal_{0}; - size_t numIncompleteImagesTotal_{0}; - size_t numDroppedImagesTotal_{0}; + size_t lastFrameId_{0}; Spinnaker::GevIEEE1588StatusEnums ptpStatus_{Spinnaker::GevIEEE1588Status_Disabled}; }; } // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/utils.cpp b/spinnaker_camera_driver/src/utils.cpp new file mode 100644 index 00000000..33147497 --- /dev/null +++ b/spinnaker_camera_driver/src/utils.cpp @@ -0,0 +1,60 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2025 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. + +#include + +namespace spinnaker_camera_driver +{ +namespace utils +{ +template +T safeDeclare( + const std::shared_ptr & pi, + const std::string & name, const T & def) +{ + try { + return (pi->declare_parameter(name, rclcpp::ParameterValue(def)).get()); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) { + const rclcpp::Parameter p = pi->get_parameter(name); + return (p.get_parameter_value().get()); + } +} + +#ifdef IMAGE_TRANSPORT_SUPPORTS_NODE_INTERFACES +std::shared_ptr makeCameraInfoManager( + const std::shared_ptr & bi, + const std::shared_ptr & pi, + const std::shared_ptr & li, + const std::shared_ptr & si, + const std::string & cameraName, const std::string & urlParameterName, uint32_t qSize) +{ + const auto calib = safeDeclare(pi, urlParameterName, ""); + auto mgr = std::make_shared( + bi, si, li, cameraName, calib, rclcpp::QoS(qSize)); + return (mgr); +} +#else +std::shared_ptr makeCameraInfoManager( + rclcpp::Node * node, const std::string & cameraName, const std::string & parameterName) +{ + const auto calib = + safeDeclare(node->get_node_parameters_interface(), parameterName, ""); + auto mgr = std::make_shared(node, cameraName, calib); + return (mgr); +} +#endif + +} // namespace utils +} // namespace spinnaker_camera_driver diff --git a/spinnaker_synchronized_camera_driver/CMakeLists.txt b/spinnaker_synchronized_camera_driver/CMakeLists.txt index 122c3fbd..d9e799b2 100644 --- a/spinnaker_synchronized_camera_driver/CMakeLists.txt +++ b/spinnaker_synchronized_camera_driver/CMakeLists.txt @@ -42,6 +42,11 @@ find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +if(${image_transport_VERSION} VERSION_GREATER_EQUAL "6.4.0") + add_definitions(-DIMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE) + add_definitions(-DIMAGE_TRANSPORT_SUPPORTS_NODE_INTERFACES) +endif() + set(ROS_DEPENDENCIES rclcpp::rclcpp rclcpp_components::component) diff --git a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/exposure_controller_factory.hpp b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/exposure_controller_factory.hpp index bba1a427..1810888e 100644 --- a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/exposure_controller_factory.hpp +++ b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/exposure_controller_factory.hpp @@ -17,6 +17,8 @@ #define SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__EXPOSURE_CONTROLLER_FACTORY_HPP_ #include +#include +#include #include namespace spinnaker_camera_driver @@ -32,7 +34,8 @@ namespace spinnaker_synchronized_camera_driver namespace exposure_controller_factory { std::shared_ptr newInstance( - const std::string & type, const std::string & name, rclcpp::Node * node); + const std::string & type, const std::string & name, + const std::shared_ptr & pi); } // namespace exposure_controller_factory } // namespace spinnaker_synchronized_camera_driver #endif // SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__EXPOSURE_CONTROLLER_FACTORY_HPP_ diff --git a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/follower_exposure_controller.hpp b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/follower_exposure_controller.hpp index f05a4903..f58dba78 100644 --- a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/follower_exposure_controller.hpp +++ b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/follower_exposure_controller.hpp @@ -17,6 +17,7 @@ #define SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__FOLLOWER_EXPOSURE_CONTROLLER_HPP_ #include +#include #include #include @@ -26,7 +27,9 @@ class FollowerExposureController : public spinnaker_camera_driver::ExposureContr { public: using Camera = spinnaker_camera_driver::Camera; - explicit FollowerExposureController(const std::string & name, rclcpp::Node * n); + explicit FollowerExposureController( + const std::string & name, + const std::shared_ptr & pi); void update( Camera * cam, const std::shared_ptr & img) final; void addCamera(const std::shared_ptr & cam) final; @@ -40,13 +43,21 @@ class FollowerExposureController : public spinnaker_camera_driver::ExposureContr template T declare_param(const std::string & n, const T & def) { - return (node_->declare_parameter(name_ + "." + n, def)); + return ( + node_parameters_interface_->declare_parameter(name_ + "." + n, rclcpp::ParameterValue(def)) + .get()); + } + + void set_param(const rclcpp::Parameter & p) + { + const std::vector pv{p}; + node_parameters_interface_->set_parameters(pv); } // ----------------- variables -------------------- std::string name_; std::string cameraName_; - rclcpp::Node * node_{0}; + std::shared_ptr node_parameters_interface_; std::string exposureParameterName_; std::string gainParameterName_; std::string masterControllerName_; diff --git a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/master_exposure_controller.hpp b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/master_exposure_controller.hpp index dbe0d861..bfb08027 100644 --- a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/master_exposure_controller.hpp +++ b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/master_exposure_controller.hpp @@ -17,6 +17,7 @@ #define SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__MASTER_EXPOSURE_CONTROLLER_HPP_ #include +#include #include #include @@ -26,7 +27,10 @@ class MasterExposureController : public spinnaker_camera_driver::ExposureControl { public: using Camera = spinnaker_camera_driver::Camera; - explicit MasterExposureController(const std::string & name, rclcpp::Node * n); + explicit MasterExposureController( + const std::string & name, + const std::shared_ptr & pi); + void update( Camera * cam, const std::shared_ptr & img) final; void addCamera(const std::shared_ptr & cam) final; @@ -47,13 +51,21 @@ class MasterExposureController : public spinnaker_camera_driver::ExposureControl template T declare_param(const std::string & n, const T & def) { - return (node_->declare_parameter(name_ + "." + n, def)); + return ( + node_parameters_interface_->declare_parameter(name_ + "." + n, rclcpp::ParameterValue(def)) + .get()); + } + + void set_param(const rclcpp::Parameter & p) + { + const std::vector pv{p}; + node_parameters_interface_->set_parameters(pv); } // ----------------- variables -------------------- std::string name_; std::string cameraName_; - rclcpp::Node * node_{0}; + std::shared_ptr node_parameters_interface_; int32_t targetBrightness_{128}; std::string exposureParameterName_; std::string gainParameterName_; diff --git a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/synchronized_camera_driver.hpp b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/synchronized_camera_driver.hpp index 12206d26..2e1ac463 100644 --- a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/synchronized_camera_driver.hpp +++ b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/synchronized_camera_driver.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -34,20 +35,41 @@ class ExposureController; namespace spinnaker_synchronized_camera_driver { class TimeEstimator; -class SynchronizedCameraDriver : public rclcpp::Node +class SynchronizedCameraDriver : public NodeType { public: + using CameraInfoManager = camera_info_manager::CameraInfoManager; + using ImageTransport = image_transport::ImageTransport; explicit SynchronizedCameraDriver(const rclcpp::NodeOptions & options); ~SynchronizedCameraDriver(); bool update(size_t idx, uint64_t hostTime, double dt, uint64_t * frameTime); +#ifdef IMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE +protected: + void preShutdown(); + CbReturn on_configure(const LCState & state) override; + CbReturn on_activate(const LCState & state) override; + CbReturn on_deactivate(const LCState & state) override; + CbReturn on_cleanup(const LCState & state) override; + CbReturn on_shutdown(const LCState & state) override; + CbReturn on_error(const LCState & state) override; +#endif + private: - void createCameras(); - void createExposureControllers(); + bool createCameras(); + bool createExposureControllers(); + void destroyCameras(); + void destroyExposureControllers(); void printStatus(); + bool configure(); + bool activate(); + bool deactivate(); + bool deconfigure(); + void shutdown(); // ----- variables -- - std::shared_ptr imageTransport_; + std::shared_ptr imageTransport_; std::map> cameras_; + std::vector> infoManagers_; std::vector> timeKeepers_; rclcpp::TimerBase::SharedPtr statusTimer_; double avgFrameInterval_{-1}; @@ -57,6 +79,7 @@ class SynchronizedCameraDriver : public rclcpp::Node std::shared_ptr timeEstimator_; std::unordered_map> exposureControllers_; + rclcpp::TimerBase::SharedPtr timer_; }; } // namespace spinnaker_synchronized_camera_driver #endif // SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__SYNCHRONIZED_CAMERA_DRIVER_HPP_ diff --git a/spinnaker_synchronized_camera_driver/src/exposure_controller_factory.cpp b/spinnaker_synchronized_camera_driver/src/exposure_controller_factory.cpp index 40f13cf3..e95bc792 100644 --- a/spinnaker_synchronized_camera_driver/src/exposure_controller_factory.cpp +++ b/spinnaker_synchronized_camera_driver/src/exposure_controller_factory.cpp @@ -24,12 +24,13 @@ namespace exposure_controller_factory { static rclcpp::Logger get_logger() { return (rclcpp::get_logger("cam_sync")); } std::shared_ptr newInstance( - const std::string & type, const std::string & name, rclcpp::Node * node) + const std::string & type, const std::string & name, + const std::shared_ptr & pi) { if (type == "master") { - return (std::make_shared(name, node)); + return (std::make_shared(name, pi)); } else if (type == "follower") { - return (std::make_shared(name, node)); + return (std::make_shared(name, pi)); } BOMB_OUT("unknown exposure controller type: " << type); return (nullptr); diff --git a/spinnaker_synchronized_camera_driver/src/follower_exposure_controller.cpp b/spinnaker_synchronized_camera_driver/src/follower_exposure_controller.cpp index 88153449..a381895c 100644 --- a/spinnaker_synchronized_camera_driver/src/follower_exposure_controller.cpp +++ b/spinnaker_synchronized_camera_driver/src/follower_exposure_controller.cpp @@ -21,8 +21,9 @@ namespace spinnaker_synchronized_camera_driver { FollowerExposureController::FollowerExposureController( - const std::string & name, rclcpp::Node * node) -: name_(name), node_(node) + const std::string & name, + const std::shared_ptr & pi) +: name_(name), node_parameters_interface_(pi) { exposureParameterName_ = declare_param("exposure_parameter", "exposure_time"); gainParameterName_ = declare_param("gain_parameter", "gain"); @@ -73,12 +74,12 @@ void FollowerExposureController::update( bool parametersChanged{false}; if (masterExposureTime != currentExposureTime_) { const auto expName = cam->getPrefix() + exposureParameterName_; - node_->set_parameter(rclcpp::Parameter(expName, masterExposureTime)); + set_param(rclcpp::Parameter(expName, masterExposureTime)); parametersChanged = true; } if (masterGain != currentGain_) { const auto gainName = cam->getPrefix() + gainParameterName_; - node_->set_parameter(rclcpp::Parameter(gainName, masterGain)); + set_param(rclcpp::Parameter(gainName, masterGain)); parametersChanged = true; } if (parametersChanged) { diff --git a/spinnaker_synchronized_camera_driver/src/master_exposure_controller.cpp b/spinnaker_synchronized_camera_driver/src/master_exposure_controller.cpp index be772cd2..ce171253 100644 --- a/spinnaker_synchronized_camera_driver/src/master_exposure_controller.cpp +++ b/spinnaker_synchronized_camera_driver/src/master_exposure_controller.cpp @@ -22,8 +22,11 @@ namespace spinnaker_synchronized_camera_driver { -MasterExposureController::MasterExposureController(const std::string & name, rclcpp::Node * node) -: name_(name), node_(node) +MasterExposureController::MasterExposureController( + const std::string & name, + const std::shared_ptr & pi) + +: name_(name), node_parameters_interface_(pi) { exposureParameterName_ = declare_param("exposure_parameter", "exposure_time"); gainParameterName_ = declare_param("gain_parameter", "gain"); @@ -231,9 +234,9 @@ void MasterExposureController::update( << currentExposureTime_ << " " << currentGain_ << "]"); numFramesSkip_ = maxFramesSkip_; // restart frame skipping const auto expName = cam->getPrefix() + exposureParameterName_; - node_->set_parameter(rclcpp::Parameter(expName, currentExposureTime_)); + set_param(rclcpp::Parameter(expName, currentExposureTime_)); const auto gainName = cam->getPrefix() + gainParameterName_; - node_->set_parameter(rclcpp::Parameter(gainName, currentGain_)); + set_param(rclcpp::Parameter(gainName, currentGain_)); } } } diff --git a/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp b/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp index c088921f..357ab63d 100644 --- a/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp +++ b/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include #include #include #include @@ -25,27 +27,93 @@ namespace spinnaker_synchronized_camera_driver { SynchronizedCameraDriver::SynchronizedCameraDriver(const rclcpp::NodeOptions & options) -: Node("sync_cam_driver", options), timeEstimator_(new TimeEstimator()) +: NodeType("sync_cam_driver", options), timeEstimator_(new TimeEstimator()) { - imageTransport_ = std::make_shared( +#ifdef IMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE + imageTransport_ = std::make_shared(image_transport::RequiredInterfaces(*this)); + get_node_base_interface()->get_context()->add_pre_shutdown_callback( + std::bind(&SynchronizedCameraDriver::preShutdown, this)); + if (declare_parameter("auto_start", true)) { + // defer because one cannot call some of the required methods inside the constructor + timer_ = create_wall_timer(std::chrono::microseconds(0), [this]() -> void { + timer_->cancel(); + rclcpp_lifecycle::LifecycleNode::configure(); + rclcpp_lifecycle::LifecycleNode::activate(); + }); + } +#else + imageTransport_ = std::make_shared( std::shared_ptr(this, [](auto *) {})); - createExposureControllers(); // before cams so they can refer to it - createCameras(); - // start cameras only when all synchronizer state has been set up! + if (configure()) { + activate(); + } +#endif +} + +SynchronizedCameraDriver::~SynchronizedCameraDriver() { shutdown(); } + +bool SynchronizedCameraDriver::configure() +{ + if (!createExposureControllers()) { + return (false); + } + if (!createCameras()) { + return (false); + } for (auto & c : cameras_) { - c.second->start(); + if (!c.second->configure()) { + return (false); + } } + return (true); +} +bool SynchronizedCameraDriver::activate() +{ + for (auto & c : cameras_) { + if (!c.second->activate()) { + return (false); + } + } statusTimer_ = rclcpp::create_timer( this, this->get_clock(), rclcpp::Duration(5, 0), std::bind(&SynchronizedCameraDriver::printStatus, this)); + return (true); } -SynchronizedCameraDriver::~SynchronizedCameraDriver() +bool SynchronizedCameraDriver::deactivate() { - if (!statusTimer_->is_canceled()) { + for (auto & c : cameras_) { + if (!c.second->deactivate()) { + return (false); + } + } + if (statusTimer_ && !statusTimer_->is_canceled()) { statusTimer_->cancel(); } + return (true); +} + +bool SynchronizedCameraDriver::deconfigure() +{ + for (auto & c : cameras_) { + if (!c.second->deconfigure()) { + return (false); + } + } + destroyCameras(); + destroyExposureControllers(); + return (true); +} + +void SynchronizedCameraDriver::shutdown() +{ + if (timer_) { + timer_->cancel(); + timer_->reset(); + } + (void)deactivate(); + (void)deconfigure(); } void SynchronizedCameraDriver::printStatus() @@ -88,14 +156,16 @@ void SynchronizedCameraDriver::printStatus() } } -void SynchronizedCameraDriver::createExposureControllers() +bool SynchronizedCameraDriver::createExposureControllers() { using svec = std::vector; const svec controllers = this->declare_parameter("exposure_controllers", svec()); for (const auto & c : controllers) { const std::string type = this->declare_parameter(c + ".type", ""); if (!type.empty()) { - exposureControllers_.insert({c, exposure_controller_factory::newInstance(type, c, this)}); + exposureControllers_.insert( + {c, + exposure_controller_factory::newInstance(type, c, this->get_node_parameters_interface())}); LOG_INFO("created exposure controller: " << c); } else { BOMB_OUT("no controller type specified for controller " << c); @@ -105,9 +175,10 @@ void SynchronizedCameraDriver::createExposureControllers() for (auto & c : exposureControllers_) { c.second->link(exposureControllers_); } + return (true); } -void SynchronizedCameraDriver::createCameras() +bool SynchronizedCameraDriver::createCameras() { using svec = std::vector; const svec cameras = this->declare_parameter("cameras", svec()); @@ -116,8 +187,19 @@ void SynchronizedCameraDriver::createCameras() } for (size_t i = 0; i < cameras.size(); i++) { const auto & c = cameras[i]; - auto cam = - std::make_shared(this, imageTransport_.get(), c, false); +#ifdef IMAGE_TRANSPORT_SUPPORTS_NODE_INTERFACES + auto mgr = spinnaker_camera_driver::utils::makeCameraInfoManager( + get_node_base_interface(), get_node_parameters_interface(), get_node_logging_interface(), + get_node_services_interface(), c, c + ".camerainfo_url", 10); +#else + auto mgr = + spinnaker_camera_driver::utils::makeCameraInfoManager(this, c, c + ".camerainfo_url"); +#endif + infoManagers_.push_back(mgr); + auto cam = std::make_shared( + get_node_base_interface(), get_node_parameters_interface(), get_node_logging_interface(), + get_node_timers_interface(), get_node_clock_interface(), get_node_topics_interface(), + get_node_services_interface(), imageTransport_.get(), mgr.get(), c, c, false); cameras_.insert({c, cam}); timeKeepers_.push_back(std::make_shared(i, c, this)); cam->setSynchronizer(timeKeepers_.back()); @@ -133,6 +215,16 @@ void SynchronizedCameraDriver::createCameras() } } numUpdatesRequired_ = cameras.size() * 3; + return (true); +} + +void SynchronizedCameraDriver::destroyExposureControllers() { exposureControllers_.clear(); } + +void SynchronizedCameraDriver::destroyCameras() +{ + cameras_.clear(); + timeKeepers_.clear(); + infoManagers_.clear(); } bool SynchronizedCameraDriver::update( @@ -156,6 +248,72 @@ bool SynchronizedCameraDriver::update( return (gotTime); } +#ifdef IMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE + +void SynchronizedCameraDriver::preShutdown() +{ + LOG_INFO("running preShutdown()"); + rclcpp_lifecycle::LifecycleNode::shutdown(); +} + +CbReturn SynchronizedCameraDriver::on_configure(const LCState & s) +{ + LOG_INFO("requested configure()"); + const auto ret = NodeType::on_configure(s); + if (ret != CbReturn::SUCCESS) { + return (ret); + } + return (configure() ? CbReturn::SUCCESS : CbReturn::FAILURE); +} + +CbReturn SynchronizedCameraDriver::on_activate(const LCState & s) +{ + LOG_INFO("requested activate()"); + const auto ret = NodeType::on_activate(s); + if (ret != CbReturn::SUCCESS) { + return (ret); + } + return (activate() ? CbReturn::SUCCESS : CbReturn::FAILURE); +} + +CbReturn SynchronizedCameraDriver::on_deactivate(const LCState & s) +{ + LOG_INFO("requested deactivate()"); + const auto ret = NodeType::on_deactivate(s); + if (ret != CbReturn::SUCCESS) { + return (ret); + } + return (deactivate() ? CbReturn::SUCCESS : CbReturn::FAILURE); +} + +CbReturn SynchronizedCameraDriver::on_cleanup(const LCState & s) +{ + LOG_INFO("requested cleanup()"); + const auto ret = NodeType::on_cleanup(s); + if (ret != CbReturn::SUCCESS) { + return (ret); + } + return (deconfigure() ? CbReturn::SUCCESS : CbReturn::FAILURE); +} + +CbReturn SynchronizedCameraDriver::on_shutdown(const LCState & state) +{ + LOG_INFO("requested shutdown()"); + auto ret = NodeType::on_shutdown(state); + if (ret != CbReturn::SUCCESS) { + return (ret); + } + shutdown(); + return CbReturn::SUCCESS; +} + +CbReturn SynchronizedCameraDriver::on_error(const LCState & state) +{ + LOG_ERROR("got error state: " << state.label()); + return (CbReturn::FAILURE); +} +#endif + } // namespace spinnaker_synchronized_camera_driver RCLCPP_COMPONENTS_REGISTER_NODE(spinnaker_synchronized_camera_driver::SynchronizedCameraDriver) diff --git a/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver_node.cpp b/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver_node.cpp index a293fe73..afeb0d92 100644 --- a/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver_node.cpp +++ b/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver_node.cpp @@ -22,7 +22,8 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); auto node = std::make_shared( rclcpp::NodeOptions()); - rclcpp::spin(node); // should not return + rclcpp::spin(node->get_node_base_interface()); // should not return + node.reset(); rclcpp::shutdown(); return 0; }