diff --git a/spinnaker_camera_driver/CMakeLists.txt b/spinnaker_camera_driver/CMakeLists.txt index 60a94d74..a1297de7 100644 --- a/spinnaker_camera_driver/CMakeLists.txt +++ b/spinnaker_camera_driver/CMakeLists.txt @@ -75,6 +75,7 @@ set(ROS_DEPENDENCIES "rclcpp_components" "sensor_msgs" "std_msgs" + "std_srvs" "camera_info_manager" "image_transport" "flir_camera_msgs") diff --git a/spinnaker_camera_driver/cmake/spinnaker_camera_driverConfig.cmake.in b/spinnaker_camera_driver/cmake/spinnaker_camera_driverConfig.cmake.in index 6a5600cf..f73bc645 100644 --- a/spinnaker_camera_driver/cmake/spinnaker_camera_driverConfig.cmake.in +++ b/spinnaker_camera_driver/cmake/spinnaker_camera_driverConfig.cmake.in @@ -8,6 +8,7 @@ find_dependency(image_transport) find_dependency(flir_camera_msgs) find_dependency(sensor_msgs) find_dependency(std_msgs) +find_dependency(std_srvs) # Add the targets file include("${CMAKE_CURRENT_LIST_DIR}/spinnaker_camera_driverTargets.cmake") \ No newline at end of file diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp index b4d4e34e..e003e22e 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 namespace spinnaker_camera_driver @@ -88,6 +89,11 @@ class Camera void printStatus(); void checkSubscriptions(); void doPublish(const ImageConstPtr & im); + void resetCallback( + const std::shared_ptr req, + const std::shared_ptr res); + void delayedStart(); + rclcpp::Logger get_logger() { return rclcpp::get_logger( @@ -128,6 +134,7 @@ class Camera image_transport::ImageTransport * imageTransport_; image_transport::CameraPublisher pub_; rclcpp::Publisher::SharedPtr metaPub_; + rclcpp::Service::SharedPtr resetService_; std::string serial_; std::string name_; std::string cameraInfoURL_; @@ -156,6 +163,8 @@ class Camera rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr callbackHandle_; // keep alive callbacks rclcpp::TimerBase::SharedPtr statusTimer_; rclcpp::TimerBase::SharedPtr checkSubscriptionsTimer_; + rclcpp::TimerBase::SharedPtr delayedStartTimer_; + int restartDelay_{10}; bool cameraRunning_{false}; std::mutex mutex_; std::condition_variable cv_; 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 175861ca..e0322612 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp @@ -48,6 +48,7 @@ class SpinnakerWrapper bool deInitCamera(); bool startCamera(const SpinnakerWrapper::Callback & cb); bool stopCamera(); + void resetCamera(); void setDebug(bool b); void setComputeBrightness(bool b); void setAcquisitionTimeout(double sec); diff --git a/spinnaker_camera_driver/package.xml b/spinnaker_camera_driver/package.xml index 1c479eed..02fe1c83 100644 --- a/spinnaker_camera_driver/package.xml +++ b/spinnaker_camera_driver/package.xml @@ -34,6 +34,7 @@ rclcpp_components sensor_msgs std_msgs + std_srvs ament_lint_auto ament_lint_common diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp index c7e6df2c..481d12ce 100644 --- a/spinnaker_camera_driver/src/camera.cpp +++ b/spinnaker_camera_driver/src/camera.cpp @@ -224,6 +224,7 @@ void Camera::readParameters() parameterFile_ = safe_declare(prefix_ + "parameter_file", "parameters.yaml"); connectWhileSubscribed_ = safe_declare(prefix_ + "connect_while_subscribed", false); enableExternalControl_ = safe_declare(prefix_ + "enable_external_control", false); + restartDelay_ = safe_declare(prefix_ + "restart_delay", 10); callbackHandle_ = node_->add_on_set_parameters_callback( std::bind(&Camera::parameterChanged, this, std::placeholders::_1)); } @@ -478,6 +479,34 @@ void Camera::controlCallback(const flir_camera_msgs::msg::CameraControl::UniqueP } } +void Camera::resetCallback( + const std::shared_ptr, + const std::shared_ptr res) +{ + if (wrapper_) { + const bool cameraRunning = cameraRunning_; + stopCamera(); // stops the camera + wrapper_->resetCamera(); + if (cameraRunning) { + LOG_INFO("restarting camera in " << restartDelay_ << " seconds"); + delayedStartTimer_ = rclcpp::create_timer( + node_, node_->get_clock(), rclcpp::Duration(restartDelay_, 0), + std::bind(&Camera::delayedStart, this)); + } + res->success = true; + res->message = "camera reset!"; + } else { + res->success = false; + res->message = "camera not initialized!"; + } +} + +void Camera::delayedStart() +{ + delayedStartTimer_->cancel(); + startCamera(); +} + void Camera::processImage(const ImageConstPtr & im) { { @@ -689,7 +718,9 @@ bool Camera::start() metaMsg_.header.frame_id = frameId_; pub_ = imageTransport_->advertiseCamera("~/" + topicPrefix_ + "image_raw", qosDepth_); - + resetService_ = node_->create_service( + "~/" + prefix_ + "reset", + std::bind(&Camera::resetCallback, this, std::placeholders::_1, std::placeholders::_2)); wrapper_ = std::make_shared(); wrapper_->setDebug(debug_); wrapper_->setComputeBrightness(computeBrightness_); diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper.cpp b/spinnaker_camera_driver/src/spinnaker_wrapper.cpp index 03272b43..ac5c8ad1 100644 --- a/spinnaker_camera_driver/src/spinnaker_wrapper.cpp +++ b/spinnaker_camera_driver/src/spinnaker_wrapper.cpp @@ -101,6 +101,7 @@ std::string SpinnakerWrapper::execute(const std::string & nodeName) throw SpinnakerWrapper::Exception(e.what()); } } +void SpinnakerWrapper::resetCamera() { wrapperImpl_->resetCamera(); } void SpinnakerWrapper::setComputeBrightness(bool b) { wrapperImpl_->setComputeBrightness(b); } diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp index 6cbafcc2..bd18d864 100644 --- a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp +++ b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp @@ -473,6 +473,17 @@ bool SpinnakerWrapperImpl::stopCamera() return (false); } +void SpinnakerWrapperImpl::resetCamera() +{ + if (camera_) { + try { + camera_->DeviceReset(); + } catch (const Spinnaker::Exception & e) { + std::cerr << "reset failed with spinnaker error: " << e.what() << std::endl; + } + } +} + void SpinnakerWrapperImpl::setPixelFormat(const std::string & pixFmt) { pixelFormat_ = pixel_format::from_nodemap_string(pixFmt); diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp index 57023ef0..b407aeac 100644 --- a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp +++ b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp @@ -50,6 +50,7 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler bool startCamera(const SpinnakerWrapper::Callback & cb); bool stopCamera(); + void resetCamera(); double getReceiveFrameRate() const; double getIncompleteRate();