added camera reset service

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")

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")

spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp

@@ -31,6 +31,7 @@
 #include <spinnaker_camera_driver/spinnaker_wrapper.hpp>
 #include <spinnaker_camera_driver/synchronizer.hpp>
 #include <std_msgs/msg/float64.hpp>
+#include <std_srvs/srv/trigger.hpp>
 #include <thread>
 
 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<std_srvs::srv::Trigger::Request> req,
+    const std::shared_ptr<std_srvs::srv::Trigger::Response> 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<flir_camera_msgs::msg::ImageMetaData>::SharedPtr metaPub_;
+  rclcpp::Service<std_srvs::srv::Trigger>::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_;

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);

spinnaker_camera_driver/package.xml

@@ -34,6 +34,7 @@
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

spinnaker_camera_driver/src/camera.cpp

@@ -224,6 +224,7 @@ void Camera::readParameters()
   parameterFile_ = safe_declare<std::string>(prefix_ + "parameter_file", "parameters.yaml");
   connectWhileSubscribed_ = safe_declare<bool>(prefix_ + "connect_while_subscribed", false);
   enableExternalControl_ = safe_declare<bool>(prefix_ + "enable_external_control", false);
+  restartDelay_ = safe_declare<int>(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<std_srvs::srv::Trigger::Request>,
+  const std::shared_ptr<std_srvs::srv::Trigger::Response> 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<std_srvs::srv::Trigger>(
+    "~/" + prefix_ + "reset",
+    std::bind(&Camera::resetCallback, this, std::placeholders::_1, std::placeholders::_2));
   wrapper_ = std::make_shared<spinnaker_camera_driver::SpinnakerWrapper>();
   wrapper_->setDebug(debug_);
   wrapper_->setComputeBrightness(computeBrightness_);

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); }
 

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);

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();