Add reboot service

zed_components/src/tools/include/sl_types.hpp

@@ -157,6 +157,7 @@ typedef std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::PointStamped>>
 typedef std::shared_ptr<rclcpp::Subscription<sensor_msgs::msg::NavSatFix>> gnssFixSub;
 typedef std::shared_ptr<rclcpp::Subscription<rosgraph_msgs::msg::Clock>> clockSub;
 
+typedef rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr rebootSrvPtr;
 typedef rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr resetOdomSrvPtr;
 typedef rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr resetPosTrkSrvPtr;
 typedef rclcpp::Service<zed_msgs::srv::SetPose>::SharedPtr setPoseSrvPtr;

zed_components/src/zed_camera/include/zed_camera_component.hpp

@@ -41,6 +41,8 @@ class ZedCamera : public rclcpp::Node
   void initParameters();
   void initServices();
   void initThreads();
+  void guardDataThread();
+  void releaseDataThread();
 
   void close();
 
@@ -83,6 +85,7 @@ class ZedCamera : public rclcpp::Node
   bool startStreamingServer();
   void stopStreamingServer();
   void closeCamera();
+  bool restartCamera();
   // <---- Initialization functions
 
   // ----> Dynamic Parameters Handlers
@@ -116,6 +119,7 @@ class ZedCamera : public rclcpp::Node
   // <---- Dynamic Parameters Handlers
 
   // ----> Callbacks
+
   void callback_pubFusedPc();
   void callback_pubPaths();
   void callback_pubTemp();
@@ -126,6 +130,11 @@ class ZedCamera : public rclcpp::Node
   void callback_updateDiagnostic(
     diagnostic_updater::DiagnosticStatusWrapper & stat);
 
+  void callback_reboot(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::Trigger_Request> req,
+    std::shared_ptr<std_srvs::srv::Trigger_Response> res);
+
   void callback_resetOdometry(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<std_srvs::srv::Trigger_Request> req,
@@ -208,6 +217,8 @@ class ZedCamera : public rclcpp::Node
   void handlePointCloudPublishing();
   // Sensors thread
   void threadFunc_pubSensorsData();
+  // Restart thread
+  void threadFunc_zedRestart();
   // <---- Thread functions
 
   // ----> Publishing functions
@@ -801,6 +812,7 @@ class ZedCamera : public rclcpp::Node
   std::thread mVdThread;          // Video and Depth data processing thread
   std::thread mPcThread;          // Point Cloud publish thread
   std::thread mSensThread;        // Sensors data publish thread
+  std::thread mRestartThread;     // Camera restart thread for reboot service
   std::atomic<bool> mThreadStop;
   rclcpp::TimerBase::SharedPtr mInitTimer;
   rclcpp::TimerBase::SharedPtr mPathTimer;
@@ -825,6 +837,10 @@ class ZedCamera : public rclcpp::Node
   std::mutex mVdMutex;
   std::condition_variable mVdDataReadyCondVar;
   std::atomic_bool mVdDataReady;
+  bool mCameraRebooting = false; // Indicates if the camera is being rebooted
+  int activeUsers = 0;
+  std::mutex mRebootMutex;  // Mutex to protect camera rebooting
+  std::condition_variable mRebootCondVar;  // Condition variable to wait for camera during reboot process
   // <---- Thread Sync
 
   // ----> Status Flags
@@ -962,6 +978,7 @@ class ZedCamera : public rclcpp::Node
   // <---- SVO Recording parameters
 
   // ----> Services
+  rebootSrvPtr mRebootSrv;
   resetOdomSrvPtr mResetOdomSrv;
   resetPosTrkSrvPtr mResetPosTrkSrv;
   setPoseSrvPtr mSetPoseSrv;
@@ -997,6 +1014,7 @@ class ZedCamera : public rclcpp::Node
   const std::string mSrvResetRoiName = "reset_roi";
   const std::string mSrvToLlName = "toLL";  // Convert from `map` to `Lat Long`
   const std::string mSrvFromLlName = "fromLL";  // Convert from `Lat Long` to `map`
+  const std::string mSrvReboot = "reboot";
   // <---- Services names
 
   // ----> SVO v2

zed_components/src/zed_camera/src/zed_camera_component_main.cpp

@@ -290,6 +290,11 @@ void ZedCamera::initServices()
 
   std::string srv_prefix = "~/";
 
+
+  srv_name = srv_prefix + mSrvReboot;
+  mRebootSrv = create_service<std_srvs::srv::Trigger>(
+    srv_name, std::bind(&ZedCamera::callback_reboot, this, _1, _2, _3));
+
   if (!mDepthDisabled) {
     // Reset Odometry
     srv_name = srv_prefix + mSrvResetOdomName;
@@ -3312,6 +3317,94 @@ void ZedCamera::initThreads()
   mGrabThread = std::thread(&ZedCamera::threadFunc_zedGrab, this);
 }
 
+bool ZedCamera::restartCamera() {
+  sl_tools::StopWatch connectTimer(get_clock());
+
+  RCLCPP_INFO(get_logger(), "=== RESTARTING CAMERA ===");
+
+  // Wait longer for GMSL camera connection
+  int camera_timeout_sec = sl_tools::isZEDX(mCamRealModel) ? 15: mCamTimeoutSec;
+
+
+  while (1) {
+    mConnStatus = mZed->open(mInitParams);
+
+    if (mConnStatus == sl::ERROR_CODE::SUCCESS) {
+      DEBUG_STREAM_COMM("Opening successfull");
+      mUptimer.tic(); // Sets the beginning of the camera connection time
+      return true;
+    }
+
+    if (mConnStatus == sl::ERROR_CODE::INVALID_CALIBRATION_FILE) {
+      if (mOpencvCalibFile.empty()) {
+        RCLCPP_ERROR_STREAM(
+          get_logger(), "Calibration file error: "
+            << sl::toVerbose(mConnStatus));
+      } else {
+        RCLCPP_ERROR_STREAM(
+          get_logger(),
+          "If you are using a custom OpenCV calibration file, please check "
+          "the correctness of the path of the calibration file "
+          "in the parameter 'general.optional_opencv_calibration_file': '"
+            << mOpencvCalibFile << "'.");
+        RCLCPP_ERROR(
+          get_logger(),
+          "If the file exists, it may contain invalid information.");
+      }
+      return false;
+    }
+
+    RCLCPP_WARN(
+      get_logger(), "Error opening camera: %s",
+      sl::toString(mConnStatus).c_str());
+    if (mConnStatus == sl::ERROR_CODE::CAMERA_DETECTION_ISSUE &&
+      sl_tools::isZEDM(mCamUserModel))
+    {
+      RCLCPP_INFO(
+        get_logger(),
+        "Try to flip the USB3 Type-C connector and verify the USB3 "
+        "connection");
+    } else {
+      RCLCPP_INFO(get_logger(), "Please verify the camera connection");
+    }
+
+    if (connectTimer.toc() > mMaxReconnectTemp * camera_timeout_sec) {
+      RCLCPP_ERROR(get_logger(), "Camera detection timeout");
+      return false;
+    }
+
+    mDiagUpdater.force_update();
+
+    rclcpp::sleep_for(std::chrono::seconds(camera_timeout_sec));
+  }
+  return false;
+}
+
+
+void ZedCamera::guardDataThread() {
+  // Lock reboot mutex to ensure that the camera is not being rebooted
+  std::unique_lock<std::mutex> lock(mRebootMutex);
+
+  // Wait until camera is available
+  mRebootCondVar.wait(lock, [&]() { return !mCameraRebooting; });
+
+  // Mark that this thread is using the camera
+  ++activeUsers;
+}
+
+void ZedCamera::releaseDataThread() {
+  std::lock_guard<std::mutex> lock(mRebootMutex);
+
+  if (activeUsers > 0) {
+    --activeUsers;
+  }
+
+  // If reboot thread is waiting and no threads are accessing ZED, notify it
+  if (mCameraRebooting && activeUsers == 0) {
+      mRebootCondVar.notify_all();
+  }
+}
+
 void ZedCamera::startHeartbeatTimer()
 {
   if (mHeartbeatTimer != nullptr) {
@@ -4135,6 +4228,36 @@ void ZedCamera::publishImuFrameAndTopic()
   mImuTfFreqTimer.tic();
 }
 
+void ZedCamera::threadFunc_zedRestart()
+{
+  // Restart camera loop
+  bool camera_restarted = restartCamera();
+  if (!camera_restarted) {
+    RCLCPP_ERROR(get_logger(), "Failed to restart camera");
+    // Notify all threads to stop
+    {
+      std::lock_guard<std::mutex> lock(mRebootMutex);
+      mCameraRebooting = false;
+      mThreadStop = true;
+    }
+    mRebootCondVar.notify_all();
+    return;
+  }
+
+  // Reset camera states
+  mPosTrackingStarted = false;
+  mSpatialMappingRunning = false;
+  mObjDetRunning = false;
+  mBodyTrkRunning = false;
+
+  // Mark camera as available again
+  {
+      std::lock_guard<std::mutex> lock(mRebootMutex);
+      mCameraRebooting = false;
+  }
+  mRebootCondVar.notify_all();
+}
+
 void ZedCamera::threadFunc_zedGrab()
 {
   DEBUG_STREAM_COMM("Grab thread started");
@@ -4222,6 +4345,21 @@ void ZedCamera::threadFunc_zedGrab()
   // Infinite grab thread
   while (1) {
     auto t0 = get_clock()->now().nanoseconds();
+    {
+      // Lock reboot mutex to ensure that the camera is not being rebooted
+      std::unique_lock<std::mutex> lock(mRebootMutex);
+
+      // Wait until camera has been closed by reboot service call
+      mRebootCondVar.wait(lock, [&]() { return !mCameraRebooting; });
+
+      if (mRestartThread.joinable()) {
+        mRestartThread.join();
+      }
+
+        // Mark that this thread is using the camera
+        ++activeUsers;
+    }
+
     try {
       // ----> Interruption check
       if (!rclcpp::ok()) {
@@ -4615,6 +4753,9 @@ void ZedCamera::threadFunc_zedGrab()
                            << mSvoExpectedPeriod << " sec - Elab time:"
                            << effective_grab_period << " sec");
     }
+
+    // Release camera access for reboot
+    releaseDataThread();
   }
 
   // Stop the heartbeat
@@ -5172,6 +5313,8 @@ void ZedCamera::threadFunc_pubSensorsData()
   // <---- Advanced thread settings
 
   while (1) {
+    // Wait here on camera reboot
+    guardDataThread();
     try {
       if (!rclcpp::ok()) {
         DEBUG_STREAM_SENS("Ctrl+C received: stopping sensors thread");
@@ -5208,6 +5351,9 @@ void ZedCamera::threadFunc_pubSensorsData()
             << sleep_usec << " µsec");
         rclcpp::sleep_for(
           std::chrono::microseconds(sleep_usec)); // Avoid busy-waiting
+
+        // Release camera access
+        releaseDataThread();
         continue;
       }
 
@@ -5233,8 +5379,14 @@ void ZedCamera::threadFunc_pubSensorsData()
     } catch (...) {
       rcutils_reset_error();
       DEBUG_STREAM_COMM("threadFunc_pubSensorsData: Generic exception.");
+
+      // Release camera access
+      releaseDataThread();
       continue;
     }
+
+    // Release camera access
+    releaseDataThread();
   }
 
   DEBUG_STREAM_SENS("Sensors thread finished");
@@ -6493,6 +6645,62 @@ void ZedCamera::callback_pubPaths()
   }
 }
 
+void ZedCamera::callback_reboot(
+  const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<std_srvs::srv::Trigger_Request> req,
+  std::shared_ptr<std_srvs::srv::Trigger_Response> res)
+{
+  (void)request_header;
+  (void)req;
+
+  RCLCPP_INFO(get_logger(), "** Reboot service called **");
+
+  if (mZed == nullptr || !mZed->isOpened()) {
+    RCLCPP_ERROR(get_logger(), "ZED Camera not initialized");
+    res->message = "ZED Camera not initialized";
+    res->success = false;
+    return;
+  }
+
+  // Set camera as unavailable
+  {
+      std::lock_guard<std::mutex> lock(mRebootMutex);
+      mCameraRebooting = true;
+  }
+
+  // Wait for all threads to stop using camera
+  {
+      std::unique_lock<std::mutex> lock(mRebootMutex);
+      mRebootCondVar.wait(lock, [&]() { return activeUsers == 0; });
+  }
+
+  auto cam_info = mZed->getCameraInformation();
+
+  mZed->close();
+
+  RCLCPP_INFO_STREAM(get_logger(), "Rebooting camera with serial number: " << cam_info.serial_number);
+
+  sl::ERROR_CODE reboot_error = sl::ERROR_CODE::SUCCESS;
+  if (cam_info.input_type == sl::INPUT_TYPE::GMSL) {
+    reboot_error = sl::Camera::reboot(sl::INPUT_TYPE::GMSL);
+  } else {
+    reboot_error = sl::Camera::reboot(cam_info.serial_number);
+  }
+
+  if (reboot_error != sl::ERROR_CODE::SUCCESS) {
+    RCLCPP_ERROR(
+      get_logger(),
+      "Reboot failed: %s", sl::toString(reboot_error).c_str());
+    res->message = "Reboot failed";
+    res->success = false;
+    return;
+  }
+  mRestartThread = std::thread(&ZedCamera::threadFunc_zedRestart, this);
+
+  res->message = "Rebooting ZED Camera";
+  res->success = true;
+}
+
 void ZedCamera::callback_resetOdometry(
   const std::shared_ptr<rmw_request_id_t> request_header,
   const std::shared_ptr<std_srvs::srv::Trigger_Request> req,