Merge pull request #413 from stereolabs/add_disable_depth_service

Add disable depth service

Author: Myzhar

CHANGELOG.rst

@@ -1,6 +1,12 @@
 LATEST CHANGES
 ==============
 
+2026-02-04
+----------
+- Add `enable_depth` service to disable depth processing at runtime
+- When using GEN_3 with ZED SDK v5.2 or newer, Positional Tracking continues to provide localization feedback even if depth is disabled at runtime or when the node starts by setting the `depth.depth_mode` parameter to `NONE`.
+- New diagnostic information regarding Positional Tracking status: "Mode", "Odometry Status", "Spatial Memory Status", "Tracking Fusion Status".
+
 2026-01-30
 ----------
 - Add the `zed_debug` package for debugging ZED Components by loading them in a single C++ process.

zed_components/src/tools/include/sl_types.hpp

@@ -66,8 +66,7 @@
 #include <zed_msgs/srv/set_pose.hpp>
 #include <zed_msgs/srv/set_roi.hpp>
 #include <zed_msgs/srv/start_svo_rec.hpp>
-// #include <zed_msgs/srv/save_area_memory.hpp> TODO(Walter): Uncomment when
-// available in `zed_msgs` package from APT
+#include <zed_msgs/srv/save_area_memory.hpp>
 
 #ifdef FOUND_ISAAC_ROS_NITROS
   #include "isaac_ros_managed_nitros/managed_nitros_publisher.hpp"
@@ -154,12 +153,12 @@ typedef rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr clicke
 typedef rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gnssFixSub;
 typedef rclcpp::Subscription<rosgraph_msgs::msg::Clock>::SharedPtr clockSub;
 
+typedef rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr enableDepthPtr;
+
 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;
-// typedef rclcpp::Service<zed_msgs::srv::SaveAreaMemory>::SharedPtr saveAreaMemorySrvPtr; TODO(Walter): Uncomment when available in `zed_msgs` package from APT
-typedef rclcpp::Service<zed_msgs::srv::SetROI>::SharedPtr saveAreaMemorySrvPtr;
-
+typedef rclcpp::Service<zed_msgs::srv::SaveAreaMemory>::SharedPtr saveAreaMemorySrvPtr;
 typedef rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr enableObjDetPtr;
 typedef rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr enableBodyTrkPtr;
 typedef rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr enableMappingPtr;

zed_components/src/zed_camera/include/zed_camera_component.hpp

@@ -133,6 +133,10 @@ class ZedCamera : public rclcpp::Node
   void callback_updateDiagnostic(
     diagnostic_updater::DiagnosticStatusWrapper & stat);
 
+  void callback_enableDepth(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::SetBool_Request> req,
+    std::shared_ptr<std_srvs::srv::SetBool_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,
@@ -145,14 +149,10 @@ class ZedCamera : public rclcpp::Node
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<zed_msgs::srv::SetPose_Request> req,
     std::shared_ptr<zed_msgs::srv::SetPose_Response> res);
-  /*void callback_saveAreaMemory(
-    const std::shared_ptr<rmw_request_id_t> request_header,
-    const std::shared_ptr<zed_msgs::srv::SaveAreaMemory_Request> req,
-    std::shared_ptr<zed_msgs::srv::SaveAreaMemory_Response> res);*/// TODO(Walter): Uncomment when available in `zed_msgs` package from APT
   void callback_saveAreaMemory(
     const std::shared_ptr<rmw_request_id_t> request_header,
-    const std::shared_ptr<zed_msgs::srv::SetROI_Request> req,
-    std::shared_ptr<zed_msgs::srv::SetROI_Response> res);
+    const std::shared_ptr<zed_msgs::srv::SaveAreaMemory_Request> req,
+    std::shared_ptr<zed_msgs::srv::SaveAreaMemory_Response> res);
   void callback_enableObjDet(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<std_srvs::srv::SetBool_Request> req,
@@ -527,7 +527,7 @@ class ZedCamera : public rclcpp::Node
   double mCamMaxDepth = 15.0;
   sl::DEPTH_MODE mDepthMode = sl::DEPTH_MODE::NEURAL;
   PcRes mPcResolution = PcRes::COMPACT;
-  bool mDepthDisabled = false;  // Indicates if depth calculation is not required (DEPTH_MODE::NONE)
+  std::atomic<bool> mDepthDisabled = false;  // Indicates if depth calculation is not required (DEPTH_MODE::NONE)
   int mDepthStabilization = 0;
 
   int mCamTimeoutSec = 5;
@@ -987,6 +987,8 @@ class ZedCamera : public rclcpp::Node
   int mSvoFrameId = 0;
   int mSvoFrameCount = 0;
   bool mPosTrackingStarted = false;
+  std::atomic_bool mPoseLocked = false;
+  std::atomic<uint64_t> mPoseLockCount{0};
   bool mVdPublishing = false;  // Indicates if video and depth data are
                                // subscribed and then published
   bool mPcPublishing =
@@ -1026,6 +1028,7 @@ class ZedCamera : public rclcpp::Node
 
   // ----> Positional Tracking
   sl::Pose mLastZedPose;
+  sl::Pose mLastZedDeltaOdom;
   sl::Transform mInitialPoseSl;
   std::vector<geometry_msgs::msg::PoseStamped> mOdomPath;
   std::vector<geometry_msgs::msg::PoseStamped> mPosePath;
@@ -1117,6 +1120,7 @@ class ZedCamera : public rclcpp::Node
   // <---- SVO Recording parameters
 
   // ----> Services
+  enableDepthPtr mEnableDepthSrv;
   resetOdomSrvPtr mResetOdomSrv;
   resetPosTrkSrvPtr mResetPosTrkSrv;
   setPoseSrvPtr mSetPoseSrv;
@@ -1138,6 +1142,7 @@ class ZedCamera : public rclcpp::Node
   // <---- Services
 
   // ----> Services names
+  const std::string mSrvEnableDepthName = "enable_depth";
   const std::string mSrvResetOdomName = "reset_odometry";
   const std::string mSrvResetPoseName = "reset_pos_tracking";
   const std::string mSrvSetPoseName = "set_pose";

zed_components/src/zed_camera/src/zed_camera_component_bodytrk.cpp

@@ -259,8 +259,7 @@ bool ZedCamera::startBodyTracking()
   if (mDepthDisabled) {
     RCLCPP_WARN(
       get_logger(),
-      "Cannot start Body Tracking if "
-      "`depth.depth_mode` is set to `0` [NONE]");
+      "Cannot start Body Tracking if Depth processing is disabled");
     return false;
   }