diff --git a/zed_components/src/tools/include/sl_types.hpp b/zed_components/src/tools/include/sl_types.hpp index 1a23d75e..42f3160d 100644 --- a/zed_components/src/tools/include/sl_types.hpp +++ b/zed_components/src/tools/include/sl_types.hpp @@ -157,6 +157,7 @@ typedef std::shared_ptr> typedef std::shared_ptr> gnssFixSub; typedef std::shared_ptr> clockSub; +typedef rclcpp::Service::SharedPtr rebootSrvPtr; typedef rclcpp::Service::SharedPtr resetOdomSrvPtr; typedef rclcpp::Service::SharedPtr resetPosTrkSrvPtr; typedef rclcpp::Service::SharedPtr setPoseSrvPtr; diff --git a/zed_components/src/zed_camera/include/zed_camera_component.hpp b/zed_components/src/zed_camera/include/zed_camera_component.hpp index 439fd9c7..5a5aa9fd 100644 --- a/zed_components/src/zed_camera/include/zed_camera_component.hpp +++ b/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 request_header, + const std::shared_ptr req, + std::shared_ptr res); + void callback_resetOdometry( const std::shared_ptr request_header, const std::shared_ptr 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 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 diff --git a/zed_components/src/zed_camera/src/zed_camera_component_main.cpp b/zed_components/src/zed_camera/src/zed_camera_component_main.cpp index 3dc73ed3..39e5a429 100644 --- a/zed_components/src/zed_camera/src/zed_camera_component_main.cpp +++ b/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( + 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 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 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 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 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 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 request_header, + const std::shared_ptr req, + std::shared_ptr 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 lock(mRebootMutex); + mCameraRebooting = true; + } + + // Wait for all threads to stop using camera + { + std::unique_lock 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 request_header, const std::shared_ptr req,