Merge pull request #439 from stereolabs/fix_od_service_sigsegv

Fix Object Detection service crash

Author: Myzhar

zed_components/src/tools/src/sl_win_avg.cpp

@@ -36,7 +36,7 @@ double WinAvg::setNewSize(size_t win_size)
     mSumVals -= val;
   }
 
-  return mSumVals / mVals.size();
+  return mVals.empty() ? 0.0 : mSumVals / mVals.size();
 }
 
 double WinAvg::addValue(double val)
@@ -63,9 +63,11 @@ double WinAvg::getAvg()
 {
   std::lock_guard<std::mutex> guard(mQueueMux);
 
-  double avg = mSumVals / mVals.size();
+  if (mVals.empty()) {
+    return 0.0;
+  }
 
-  return avg;
+  return mSumVals / mVals.size();
 }
 
 }

zed_components/src/zed_camera/include/zed_camera_component.hpp

@@ -89,6 +89,7 @@ class ZedCamera : public rclcpp::Node
 
   bool startCamera();
   bool startPosTracking();
+  bool startPosTrackingLocked();  // caller must hold mPtMutex
   bool saveAreaMemoryFile(const std::string & filePath);
   bool start3dMapping();
   void stop3dMapping();
@@ -1030,6 +1031,7 @@ class ZedCamera : public rclcpp::Node
   // <---- Threads and Timers
 
   // ----> Thread Sync
+  std::mutex mGrabMutex;
   std::mutex mRecMutex;
   std::mutex mDynParMutex;
   std::mutex mMappingMutex;
@@ -1073,9 +1075,9 @@ class ZedCamera : public rclcpp::Node
 
   bool mAreaFileExists = false;
   bool mResetOdomFromSrv = false;
-  bool mSpatialMappingRunning = false;
+  std::atomic<bool> mSpatialMappingRunning{false};
   std::atomic<bool> mObjDetRunning{false};
-  bool mBodyTrkRunning = false;
+  std::atomic<bool> mBodyTrkRunning{false};
   bool mRgbSubscribed = false;
   bool mGnssMsgReceived = false;  // Indicates if a NavSatFix topic has been
                                   // received, also with invalid position fix

zed_components/src/zed_camera/src/zed_camera_component_bodytrk.cpp

@@ -200,16 +200,21 @@ bool ZedCamera::startBodyTracking()
   bt_p.max_range = mBodyTrkMaxRange;
   bt_p.prediction_timeout_s = mBodyTrkPredTimeout;
 
-  sl::ERROR_CODE btError = mZed->enableBodyTracking(bt_p);
+  // ----> Safe enableBodyTracking
+  {
+    std::lock_guard<std::mutex> grab_lock(mGrabMutex);
+    sl::ERROR_CODE btError = mZed->enableBodyTracking(bt_p);
 
-  if (btError != sl::ERROR_CODE::SUCCESS) {
-    RCLCPP_ERROR_STREAM(
-      get_logger(),
-      "Body Tracking error: " << sl::toString(btError));
+    if (btError != sl::ERROR_CODE::SUCCESS) {
+      RCLCPP_ERROR_STREAM(
+        get_logger(),
+        "Body Tracking error: " << sl::toString(btError));
 
-    mBodyTrkRunning = false;
-    return false;
+      mBodyTrkRunning = false;
+      return false;
+    }
   }
+  // <---- Safe enableBodyTracking
 
   DEBUG_BT("Body Tracking enabled");
 
@@ -233,7 +238,13 @@ void ZedCamera::stopBodyTracking()
     RCLCPP_INFO(get_logger(), "=== Stopping Body Tracking ===");
     mBodyTrkRunning = false;
     mBodyTrkEnabled = false;
-    mZed->disableBodyTracking();
+
+    // ----> Safe disableBodyTracking
+    {
+      std::lock_guard<std::mutex> grab_lock(mGrabMutex);
+      mZed->disableBodyTracking();
+    }
+    // <---- Safe disableBodyTracking
 
     // ----> Send an empty message to indicate that no more objects are tracked
     // (e.g clean RVIZ2)

zed_components/src/zed_camera/src/zed_camera_component_main.cpp

@@ -3843,8 +3843,12 @@ void ZedCamera::startFusedPcTimer(double fusedPcRate)
     mFusedPcTimer->cancel();
   }
 
+  if (fusedPcRate <= 0.0) {
+    return;
+  }
+
   std::chrono::milliseconds pubPeriod_msec(
-    static_cast<int>(1000.0 / (fusedPcRate)));
+    static_cast<int>(1000.0 / fusedPcRate));
   mFusedPcTimer = create_wall_timer(
     std::chrono::duration_cast<std::chrono::milliseconds>(pubPeriod_msec),
     std::bind(&ZedCamera::callback_pubFusedPc, this));
@@ -3888,8 +3892,13 @@ void ZedCamera::startPathPubTimer(double pathTimerRate)
 
 bool ZedCamera::startPosTracking()
 {
-  // Lock on Positional Tracking mutex to avoid race conditions
   std::lock_guard<std::mutex> lock(mPtMutex);
+  return startPosTrackingLocked();
+}
+
+bool ZedCamera::startPosTrackingLocked()
+{
+  // Caller must hold mPtMutex.
 
 #if (ZED_SDK_MAJOR_VERSION * 10 + ZED_SDK_MINOR_VERSION) >= 52
   // With ZED SDK v5.2 we can use Positional Tracking `GEN_3` even if depth is
@@ -3905,18 +3914,30 @@ bool ZedCamera::startPosTracking()
     return false;
   }
 
+
   if (mZed && mZed->isPositionalTrackingEnabled()) {
     if (!mAreaMemoryFilePath.empty() && mSaveAreaMemoryOnClosing) {
-      mZed->disablePositionalTracking(mAreaMemoryFilePath.c_str());
+      // ----> Safe disablePositionalTracking
+      {
+        std::lock_guard<std::mutex> grab_lock(mGrabMutex);
+        mZed->disablePositionalTracking(mAreaMemoryFilePath.c_str());
+      }
+      // <---- Safe disablePositionalTracking
       RCLCPP_INFO(
         get_logger(),
         "Area memory updated before restarting the Positional "
         "Tracking module.");
     } else {
-      mZed->disablePositionalTracking();
+      // ----> Safe disablePositionalTracking
+      {
+        std::lock_guard<std::mutex> grab_lock(mGrabMutex);
+        mZed->disablePositionalTracking();
+      }
+      // <---- Safe disablePositionalTracking
     }
   }
 
+
   RCLCPP_INFO(get_logger(), "=== Starting Positional Tracking ===");
 
   RCLCPP_INFO(get_logger(), " * Waiting for valid static transformations...");
@@ -4002,15 +4023,21 @@ bool ZedCamera::startPosTracking()
     DEBUG_PT(json.c_str());
   }
 
-  sl::ERROR_CODE err = mZed->enablePositionalTracking(ptParams);
+  // ----> Safe enablePositionalTracking
+  sl::ERROR_CODE err;
+  {
+    std::lock_guard<std::mutex> grab_lock(mGrabMutex);
+    err = mZed->enablePositionalTracking(ptParams);
 
-  if (err != sl::ERROR_CODE::SUCCESS) {
-    mPosTrackingStarted = false;
-    RCLCPP_WARN(
-      get_logger(), "Pos. Tracking not started: %s",
-      sl::toString(err).c_str());
-    return false;
+    if (err != sl::ERROR_CODE::SUCCESS) {
+      mPosTrackingStarted = false;
+      RCLCPP_WARN(
+        get_logger(), "Pos. Tracking not started: %s",
+        sl::toString(err).c_str());
+      return false;
+    }
   }
+  // <---- Safe enablePositionalTracking
 
   DEBUG_PT("Positional Tracking started");
 
@@ -4038,18 +4065,23 @@ bool ZedCamera::startPosTracking()
 
     fusion_params.gnss_calibration_parameters = gnss_par;
 
-    sl::FUSION_ERROR_CODE fus_err =
-      mFusion.enablePositionalTracking(fusion_params);
+    // ----> Safe enablePositionalTracking/disablePositionalTracking
+    {
+      std::lock_guard<std::mutex> grab_lock(mGrabMutex);
+      sl::FUSION_ERROR_CODE fus_err =
+        mFusion.enablePositionalTracking(fusion_params);
 
-    if (fus_err != sl::FUSION_ERROR_CODE::SUCCESS) {
-      mPosTrackingStarted = false;
-      RCLCPP_WARN(
-        get_logger(), "Fusion Pos. Tracking not started: %s",
-        sl::toString(fus_err).c_str());
-      mZed->disablePositionalTracking();
-      return false;
+      if (fus_err != sl::FUSION_ERROR_CODE::SUCCESS) {
+        mPosTrackingStarted = false;
+        RCLCPP_WARN(
+          get_logger(), "Fusion Pos. Tracking not started: %s",
+          sl::toString(fus_err).c_str());
+        mZed->disablePositionalTracking();
+        return false;
+      }
+      DEBUG_GNSS("Fusion Positional Tracking started");
     }
-    DEBUG_GNSS("Fusion Positional Tracking started");
+    // <---- Safe enablePositionalTracking/disablePositionalTracking
   }
   // <---- Enable Fusion Positional Tracking if required
 
@@ -4869,7 +4901,7 @@ void ZedCamera::threadFunc_zedGrab()
 
           // Dummy grab
           mZed->grab();
-  #endif
+#endif
           continue;
         } else {
           mGrabOnce = false; // Reset the flag and grab once
@@ -5007,14 +5039,19 @@ void ZedCamera::threadFunc_zedGrab()
       }
       // <---- Params Debug info
 
-      if (isDepthRequired() || isPosTrackingRequired()) {
-        DEBUG_STREAM_GRAB("Grab thread: grabbing...");
-        mGrabStatus = mZed->grab(mRunParams);  // Process the full pipeline with depth
+      // ----> Safe grab
+      {
+        std::lock_guard<std::mutex> grab_lock(mGrabMutex);
+        if (isDepthRequired() || isPosTrackingRequired()) {
+          DEBUG_STREAM_GRAB("Grab thread: grabbing...");
+          mGrabStatus = mZed->grab(mRunParams);  // Process the full pipeline with depth
 
-      } else {
-        DEBUG_GRAB("Grab thread: reading...");
-        mGrabStatus = mZed->read();  // Image and sensor data reading with no depth processing
+        } else {
+          DEBUG_GRAB("Grab thread: reading...");
+          mGrabStatus = mZed->read();  // Image and sensor data reading with no depth processing
+        }
       }
+      // <---- Safe grab
 
       DEBUG_GRAB("Grab thread: frame grabbed");
 
@@ -5041,8 +5078,8 @@ void ZedCamera::threadFunc_zedGrab()
               mOdomPath.clear();
               mPosePath.clear();
 
-              // Restart tracking
-              startPosTracking();
+              // Restart tracking — mPtMutex is already held by the grab loop
+              startPosTrackingLocked();
             }
             continue;
           } else {
@@ -5192,22 +5229,23 @@ void ZedCamera::threadFunc_zedGrab()
 
       // ----> Check recording status
       DEBUG_STREAM_GRAB("Grab thread: checking recording status");
-      mRecMutex.lock();
-      if (mRecording) {
-        mRecStatus = mZed->getRecordingStatus();
-        static int svo_rec_err_count = 0;
-        if (mRecStatus.is_recording && !mRecStatus.status) {
-          if (++svo_rec_err_count > 3) {
-            rclcpp::Clock steady_clock(RCL_STEADY_TIME);
-            RCLCPP_WARN_THROTTLE(
-              get_logger(), steady_clock, 1000.0,
-              "Error saving frame to SVO");
+      {
+        std::lock_guard<std::mutex> rec_lock(mRecMutex);
+        if (mRecording) {
+          mRecStatus = mZed->getRecordingStatus();
+          static int svo_rec_err_count = 0;
+          if (mRecStatus.is_recording && !mRecStatus.status) {
+            if (++svo_rec_err_count > 3) {
+              rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+              RCLCPP_WARN_THROTTLE(
+                get_logger(), steady_clock, 1000.0,
+                "Error saving frame to SVO");
+            }
+          } else {
+            svo_rec_err_count = 0;
           }
-        } else {
-          svo_rec_err_count = 0;
         }
       }
-      mRecMutex.unlock();
       // <---- Check recording status
 
       // ----> Retrieve Image/Depth data if someone has subscribed to

zed_components/src/zed_camera/src/zed_camera_component_objdet.cpp

@@ -976,14 +976,19 @@ bool ZedCamera::startObjDetect()
     od_p.custom_onnx_file = mYoloOnnxPath;
   }
 
-  sl::ERROR_CODE objDetError = mZed->enableObjectDetection(od_p);
-  if (objDetError != sl::ERROR_CODE::SUCCESS) {
-    RCLCPP_ERROR_STREAM(
-      get_logger(), "Object detection error: " << sl::toString(objDetError));
+  // ----> Safe enableObjectDetection
+  {
+    std::lock_guard<std::mutex> grab_lock(mGrabMutex);
+    sl::ERROR_CODE objDetError = mZed->enableObjectDetection(od_p);
+    if (objDetError != sl::ERROR_CODE::SUCCESS) {
+      RCLCPP_ERROR_STREAM(
+        get_logger(), "Object detection error: " << sl::toString(objDetError));
 
-    mObjDetRunning = false;
-    return false;
+      mObjDetRunning = false;
+      return false;
+    }
   }
+  // <---- Safe enableObjectDetection
 
   if (!mPubObjDet) {
     mPubObjDet = create_publisher<zed_msgs::msg::ObjectsStamped>(
@@ -1003,7 +1008,13 @@ void ZedCamera::stopObjDetect()
     RCLCPP_INFO(get_logger(), "=== Stopping Object Detection ===");
     mObjDetRunning = false;
     mObjDetEnabled = false;
-    mZed->disableObjectDetection();
+
+    // ----> Safe disableObjectDetection
+    {
+      std::lock_guard<std::mutex> grab_lock(mGrabMutex);
+      mZed->disableObjectDetection();
+    }
+    // <---- Safe disableObjectDetection
 
     // ----> Send an empty message to indicate that no more objects are tracked
     // (e.g clean RVIZ2)

zed_components/src/zed_camera/src/zed_camera_component_video_depth.cpp

@@ -1591,10 +1591,10 @@ void ZedCamera::processVideoDepth()
 #endif
       retrieveVideoDepth(gpu);
 
-      // Signal Video/Depth thread that a new pointcloud is ready
-      mVdDataReadyCondVar.notify_one();
+      // Signal Video/Depth thread that new data is ready
       mVdDataReady = true;
       mVdPublishing = true;
+      mVdDataReadyCondVar.notify_one();
     } else {
       DEBUG_VD(" * [processVideoDepth] vd_lock not locked");
     }
@@ -2706,10 +2706,10 @@ void ZedCamera::processPointCloud()
         " * [processPointCloud] Retrieved point cloud size: " << mMatCloud.getWidth() << "x" <<
           mMatCloud.getHeight());
 
-      // Signal Pointcloud thread that a new pointcloud is ready
-      mPcDataReadyCondVar.notify_one();
+      // Signal Pointcloud thread that new data is ready
       mPcDataReady = true;
       mPcPublishing = true;
+      mPcDataReadyCondVar.notify_one();
 
       DEBUG_STREAM_PC(
         " * [processPointCloud] Extracted point cloud: " << mMatCloud.getInfos().c_str() );