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 315117a2..f76dd924 100644
--- a/zed_components/src/zed_camera/include/zed_camera_component.hpp
+++ b/zed_components/src/zed_camera/include/zed_camera_component.hpp
@@ -672,7 +672,7 @@ class ZedCamera : public rclcpp::Node
   std::thread mVideoDepthThread;  // RGB/Depth data publish thread
   std::thread mPcThread;          // Point Cloud publish thread
   std::thread mSensThread;        // Sensors data publish thread
-  bool mThreadStop = false;
+  std::atomic<bool> mThreadStop = false;
   rclcpp::TimerBase::SharedPtr mInitTimer;
   rclcpp::TimerBase::SharedPtr mPathTimer;
   rclcpp::TimerBase::SharedPtr mFusedPcTimer;
diff --git a/zed_components/src/zed_camera/src/zed_camera_component.cpp b/zed_components/src/zed_camera/src/zed_camera_component.cpp
index 64da9c6a..dd3e585c 100644
--- a/zed_components/src/zed_camera/src/zed_camera_component.cpp
+++ b/zed_components/src/zed_camera/src/zed_camera_component.cpp
@@ -69,6 +69,7 @@ const int QOS_QUEUE_SIZE = 10;
 
 ZedCamera::ZedCamera(const rclcpp::NodeOptions & options)
 : Node("zed_node", options),
+  mThreadStop(false),
   mQos(QOS_QUEUE_SIZE),
   mAiInstanceID(0),
   mDiagUpdater(this),
@@ -3928,12 +3929,20 @@ bool ZedCamera::startCamera()
   mInitParams.async_grab_camera_recovery =
     true;    // Camera recovery is handled asynchronously to provide information
              // about this status
+
+  // NOTE: this is a temp fix to GMSL2 camera close issues
+  // TODO: check if this issue has been fixed in the SDK
+  if(sl_tools::isZEDX(mCamUserModel)) {
+    RCLCPP_INFO(get_logger(), "Disable async recovery for GMSL2 cameras");
+    mInitParams.async_grab_camera_recovery = false; 
+  }
   // <---- ZED configuration
 
   // ----> Try to connect to a camera, to a stream, or to load an SVO
   sl_tools::StopWatch connectTimer(get_clock());
 
   mThreadStop = false;
+  mGrabStatus = sl::ERROR_CODE::LAST;
 
   if (!mSvoMode && !mSimMode && !mStreamMode) {
     if (mCamSerialNumber > 0) {
@@ -5198,7 +5207,13 @@ void ZedCamera::stopObjDetect()
     DEBUG_STREAM_OD(
       "Publishing EMPTY OBJ message "
         << mPubObjDet->get_topic_name());
-    mPubObjDet->publish(std::move(objMsg));
+    try {
+      mPubObjDet->publish(std::move(objMsg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what() );
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
     // <---- Send an empty message to indicate that no more objects are tracked
     // (e.g clean RVIZ2)
   }
@@ -5304,7 +5319,13 @@ void ZedCamera::stopBodyTracking()
     DEBUG_STREAM_OD(
       "Publishing EMPTY OBJ message "
         << mPubBodyTrk->get_topic_name());
-    mPubBodyTrk->publish(std::move(objMsg));
+    try {
+      mPubBodyTrk->publish(std::move(objMsg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
     // <---- Send an empty message to indicate that no more objects are tracked
     // (e.g clean RVIZ2)
   }
@@ -5717,7 +5738,13 @@ void ZedCamera::publishImuFrameAndTopic()
   cameraImuTransfMgs->transform.translation.y = sl_tr.y;
   cameraImuTransfMgs->transform.translation.z = sl_tr.z;
 
-  mPubCamImuTransf->publish(std::move(cameraImuTransfMgs));
+  try{
+    mPubCamImuTransf->publish(std::move(cameraImuTransfMgs));
+  } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
 
   // Publish IMU TF as static TF
   if (!mPublishImuTF) {
@@ -5839,342 +5866,356 @@ void ZedCamera::threadFunc_zedGrab()
 
   // Infinite grab thread
   while (1) {
-    if (mUseSimTime && !mClockAvailable) {
-      rclcpp::Clock steady_clock(RCL_STEADY_TIME);
-      RCLCPP_WARN_THROTTLE(
-        get_logger(), steady_clock, 5000.0,
-        "Waiting for a valid simulation time on the '/clock' topic...");
-      continue;
-    }
-
-    sl_tools::StopWatch grabElabTimer(get_clock());
+    try {
+      if (mUseSimTime && !mClockAvailable) {
+        rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+        RCLCPP_WARN_THROTTLE(
+          get_logger(), steady_clock, 5000.0,
+          "Waiting for a valid simulation time on the '/clock' topic...");
+        continue;
+      }
 
-    // ----> Interruption check
-    if (!rclcpp::ok()) {
-      DEBUG_STREAM_COMM("Ctrl+C received: stopping grab thread");
-      break;
-    }
+      sl_tools::StopWatch grabElabTimer(get_clock());
 
-    if (mThreadStop) {
-      DEBUG_STREAM_COMM("Grab thread stopped");
-      break;
-    }
-    // <---- Interruption check
+      // ----> Interruption check
+      if (!rclcpp::ok()) {
+        DEBUG_STREAM_COMM("Ctrl+C received: stopping grab thread");
+        break;
+      }
 
-    // ----> Apply depth settings
-    applyDepthSettings();
-    // <---- Apply depth settings
+      if (mThreadStop) {
+        DEBUG_STREAM_COMM("Grab thread stopped");
+        break;
+      }
+      // <---- Interruption check
 
-    // ----> Apply video dynamic parameters
-    if (!mSimMode && !mSvoMode) {
-      applyVideoSettings();
-    }
-    // <---- Apply video dynamic parameters
+      // ----> Apply depth settings
+      applyDepthSettings();
+      // <---- Apply depth settings
 
-    // ----> Check for Positional Tracking requirement
-    if (isPosTrackingRequired() && !mPosTrackingStarted) {
-      static int pt_err_count = 0;
-      if (!startPosTracking()) {
-        if (++pt_err_count >= 3) {
-          RCLCPP_FATAL(
-            get_logger(),
-            "It's not possible to enable the required Positional "
-            "Tracking module.");
-          exit(EXIT_FAILURE);
+      // ----> Apply video dynamic parameters
+      if (!mSimMode && !mSvoMode) {
+        applyVideoSettings();
+      }
+      // <---- Apply video dynamic parameters
+
+      // ----> Check for Positional Tracking requirement
+      if (isPosTrackingRequired() && !mPosTrackingStarted) {
+        static int pt_err_count = 0;
+        if (!startPosTracking()) {
+          if (++pt_err_count >= 3) {
+            RCLCPP_FATAL(
+              get_logger(),
+              "It's not possible to enable the required Positional "
+              "Tracking module.");
+            exit(EXIT_FAILURE);
+          }
+        } else {
+          pt_err_count = 0;
         }
-      } else {
-        pt_err_count = 0;
       }
-    }
 
-    if (mGnssFusionEnabled && !mGnssFixValid) {
-      rclcpp::Clock steady_clock(RCL_STEADY_TIME);
-      RCLCPP_WARN_THROTTLE(
-        get_logger(), steady_clock, 5000.0,
-        " * Waiting for the first valid GNSS fix...");
-    }
-    // ----> Check for Positional Tracking requirement
+      if (mGnssFusionEnabled && !mGnssFixValid) {
+        rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+        RCLCPP_WARN_THROTTLE(
+          get_logger(), steady_clock, 5000.0,
+          " * Waiting for the first valid GNSS fix...");
+      }
+      // ----> Check for Positional Tracking requirement
 
-    if (!mDepthDisabled) {
-      // ----> Check for Spatial Mapping requirement
+      if (!mDepthDisabled) {
+        // ----> Check for Spatial Mapping requirement
 
-      mMappingMutex.lock();
-      bool required = mMappingEnabled;
+        mMappingMutex.lock();
+        bool required = mMappingEnabled;
 
-      if (required && !mSpatialMappingRunning) {
-        start3dMapping();
-      }
-      mMappingMutex.unlock();
+        if (required && !mSpatialMappingRunning) {
+          start3dMapping();
+        }
+        mMappingMutex.unlock();
 
-      // <---- Check for Spatial Mapping requirement
+        // <---- Check for Spatial Mapping requirement
 
-      // ----> Check for Object Detection requirement
-      mObjDetMutex.lock();
-      if (mObjDetEnabled && !mObjDetRunning) {
-        startObjDetect();
-        if (!sl_tools::isObjDetAvailable(mCamRealModel)) {
-          mObjDetEnabled = false;
+        // ----> Check for Object Detection requirement
+        mObjDetMutex.lock();
+        if (mObjDetEnabled && !mObjDetRunning) {
+          startObjDetect();
+          if (!sl_tools::isObjDetAvailable(mCamRealModel)) {
+            mObjDetEnabled = false;
+          }
         }
-      }
-      mObjDetMutex.unlock();
+        mObjDetMutex.unlock();
 
-      // ----> Check for Object Detection requirement
+        // ----> Check for Object Detection requirement
 
-      // ----> Check for Body Tracking requirement
-      mBodyTrkMutex.lock();
-      if (mBodyTrkEnabled && !mBodyTrkRunning) {
-        startBodyTracking();
-        if (!sl_tools::isObjDetAvailable(mCamRealModel)) {
-          mBodyTrkEnabled = false;
+        // ----> Check for Body Tracking requirement
+        mBodyTrkMutex.lock();
+        if (mBodyTrkEnabled && !mBodyTrkRunning) {
+          startBodyTracking();
+          if (!sl_tools::isObjDetAvailable(mCamRealModel)) {
+            mBodyTrkEnabled = false;
+          }
         }
+        mBodyTrkMutex.unlock();
+        // ----> Check for Object Detection requirement
       }
-      mBodyTrkMutex.unlock();
-      // ----> Check for Object Detection requirement
-    }
-
-    // ----> Grab freq calculation
-    double elapsed_sec = mGrabFreqTimer.toc();
-    mGrabPeriodMean_sec->addValue(elapsed_sec);
-    mGrabFreqTimer.tic();
-
-    // RCLCPP_INFO_STREAM(get_logger(), "Grab period: "
-    // << mGrabPeriodMean_sec->getAvg() / 1e6
-    // << " Freq: " << 1e6 / mGrabPeriodMean_usec->getAvg());
-    // <---- Grab freq calculation
-
-    if (!mSvoPause) {
-      // Start processing timer for diagnostic
-      grabElabTimer.tic();
-
-      // ZED grab
-      mGrabStatus = mZed->grab(mRunParams);
-
-      // ----> Grab errors?
-      // Note: disconnection are automatically handled by the ZED SDK
-      if (mGrabStatus != sl::ERROR_CODE::SUCCESS) {
-        if (mSvoMode && mGrabStatus == sl::ERROR_CODE::END_OF_SVOFILE_REACHED) {
-          // ----> Check SVO status
-          if (mSvoLoop) {
-            mZed->setSVOPosition(0);
-            RCLCPP_WARN(
+
+      // ----> Grab freq calculation
+      double elapsed_sec = mGrabFreqTimer.toc();
+      mGrabPeriodMean_sec->addValue(elapsed_sec);
+      mGrabFreqTimer.tic();
+
+      // RCLCPP_INFO_STREAM(get_logger(), "Grab period: "
+      // << mGrabPeriodMean_sec->getAvg() / 1e6
+      // << " Freq: " << 1e6 / mGrabPeriodMean_usec->getAvg());
+      // <---- Grab freq calculation
+
+      if (!mSvoPause) {
+        // Start processing timer for diagnostic
+        grabElabTimer.tic();
+
+        // ZED grab
+        mGrabStatus = mZed->grab(mRunParams);
+
+        // ----> Grab errors?
+        // Note: disconnection are automatically handled by the ZED SDK
+        if (mGrabStatus != sl::ERROR_CODE::SUCCESS) {
+          if (mSvoMode && mGrabStatus == sl::ERROR_CODE::END_OF_SVOFILE_REACHED) {
+            // ----> Check SVO status
+            if (mSvoLoop) {
+              mZed->setSVOPosition(0);
+              RCLCPP_WARN(
+                get_logger(),
+                "SVO reached the end and it has been restarted.");
+              rclcpp::sleep_for(
+                std::chrono::microseconds(
+                  static_cast<int>(mGrabPeriodMean_sec->getAvg() * 1e6)));
+              continue;
+            } else {
+              RCLCPP_WARN(
+                get_logger(),
+                "SVO reached the end. The node has been stopped.");
+              break;
+            }
+            // <---- Check SVO status
+          } else if (mGrabStatus == sl::ERROR_CODE::CAMERA_REBOOTING) {
+            RCLCPP_ERROR_STREAM(
+              get_logger(),
+              "Connection issue detected: "
+                << sl::toString(mGrabStatus).c_str());
+            rclcpp::sleep_for(1000ms);
+            continue;
+          } else if (mGrabStatus == sl::ERROR_CODE::CAMERA_NOT_INITIALIZED || mGrabStatus == sl::ERROR_CODE::FAILURE) {
+            RCLCPP_ERROR_STREAM(
               get_logger(),
-              "SVO reached the end and it has been restarted.");
-            rclcpp::sleep_for(
-              std::chrono::microseconds(
-                static_cast<int>(mGrabPeriodMean_sec->getAvg() * 1e6)));
+              "Camera issue detected: "
+                << sl::toString(mGrabStatus).c_str() << ". Trying to recover the connection...");
+            rclcpp::sleep_for(1000ms);
             continue;
           } else {
-            RCLCPP_WARN(
+            RCLCPP_ERROR_STREAM(
               get_logger(),
-              "SVO reached the end. The node has been stopped.");
-            break;
+              "Critical camera error: " << sl::toString(mGrabStatus).c_str()
+                                        << ". NODE KILLED.");
+            mZed.reset();
+            exit(EXIT_FAILURE);
           }
-          // <---- Check SVO status
-        } else if (mGrabStatus == sl::ERROR_CODE::CAMERA_REBOOTING) {
-          RCLCPP_ERROR_STREAM(
-            get_logger(),
-            "Connection issue detected: "
-              << sl::toString(mGrabStatus).c_str());
-          rclcpp::sleep_for(1000ms);
-          continue;
-        } else {
-          RCLCPP_ERROR_STREAM(
-            get_logger(),
-            "Critical camera error: " << sl::toString(mGrabStatus).c_str()
-                                      << ". Node stopped.");
-          break;
         }
-      }
-      // <---- Grab errors?
+        // <---- Grab errors?
 
-      mFrameCount++;
+        mFrameCount++;
 
-      if (mGnssFusionEnabled) {
-        // Process Fusion data
-        mFusionStatus = mFusion.process();
-        // ----> Fusion errors?
-        if (mFusionStatus != sl::FUSION_ERROR_CODE::SUCCESS &&
-          mFusionStatus != sl::FUSION_ERROR_CODE::NO_NEW_DATA_AVAILABLE)
-        {
-          RCLCPP_ERROR_STREAM(
-            get_logger(),
-            "Fusion error: " << sl::toString(mFusionStatus).c_str());
+        if (mGnssFusionEnabled) {
+          // Process Fusion data
+          mFusionStatus = mFusion.process();
+          // ----> Fusion errors?
+          if (mFusionStatus != sl::FUSION_ERROR_CODE::SUCCESS &&
+            mFusionStatus != sl::FUSION_ERROR_CODE::NO_NEW_DATA_AVAILABLE)
+          {
+            RCLCPP_ERROR_STREAM(
+              get_logger(),
+              "Fusion error: " << sl::toString(mFusionStatus).c_str());
+          }
+          // <---- Fusion errors?
         }
-        // <---- Fusion errors?
-      }
 
-      // ----> Timestamp
-      if (mSvoMode) {
-        mFrameTimestamp = sl_tools::slTime2Ros(
-          mZed->getTimestamp(sl::TIME_REFERENCE::CURRENT));
-      } else if (mSimMode) {
-        if (mUseSimTime) {
-          mFrameTimestamp = get_clock()->now();
-        } else {
+        // ----> Timestamp
+        if (mSvoMode) {
           mFrameTimestamp = sl_tools::slTime2Ros(
-            mZed->getTimestamp(sl::TIME_REFERENCE::IMAGE));
+            mZed->getTimestamp(sl::TIME_REFERENCE::CURRENT));
+        } else if (mSimMode) {
+          if (mUseSimTime) {
+            mFrameTimestamp = get_clock()->now();
+          } else {
+            mFrameTimestamp = sl_tools::slTime2Ros(
+              mZed->getTimestamp(sl::TIME_REFERENCE::IMAGE));
+          }
+        } else {
+          mFrameTimestamp =
+            sl_tools::slTime2Ros(mZed->getTimestamp(sl::TIME_REFERENCE::IMAGE));
         }
-      } else {
-        mFrameTimestamp =
-          sl_tools::slTime2Ros(mZed->getTimestamp(sl::TIME_REFERENCE::IMAGE));
-      }
-      // <---- Timestamp
+        // <---- Timestamp
 
-      if (mStreamingServerRequired && !mStreamingServerRunning) {
-        DEBUG_STR("Streaming server required, but not running");
-        startStreamingServer();
-      }
+        if (mStreamingServerRequired && !mStreamingServerRunning) {
+          DEBUG_STR("Streaming server required, but not running");
+          startStreamingServer();
+        }
 
-      if (!mSimMode) {
-        if (mGnssFusionEnabled && mGnssFixNew) {
-          mGnssFixNew = false;
-
-          rclcpp::Time real_frame_ts = sl_tools::slTime2Ros(
-            mZed->getTimestamp(sl::TIME_REFERENCE::IMAGE));
-          DEBUG_STREAM_GNSS(
-            "GNSS synced frame ts: "
-              << real_frame_ts.nanoseconds() << " nsec");
-          float dT_sec = (static_cast<float>(real_frame_ts.nanoseconds()) -
-            static_cast<float>(mGnssTimestamp.nanoseconds())) /
-            1e9;
-          DEBUG_STREAM_GNSS(
-            "DeltaT: "
-              << dT_sec << " sec [" << std::fixed << std::setprecision(9)
-              << static_cast<float>(real_frame_ts.nanoseconds()) / 1e9 << "-"
-              << static_cast<float>(mGnssTimestamp.nanoseconds()) / 1e9 << "]");
-
-          if (dT_sec < 0.0) {
-            RCLCPP_WARN_STREAM(
-              get_logger(),
-              "GNSS sensor and ZED Timestamps are not good. dT = " << dT_sec
-                                                                   << " sec");
+        if (!mSimMode) {
+          if (mGnssFusionEnabled && mGnssFixNew) {
+            mGnssFixNew = false;
+
+            rclcpp::Time real_frame_ts = sl_tools::slTime2Ros(
+              mZed->getTimestamp(sl::TIME_REFERENCE::IMAGE));
+            DEBUG_STREAM_GNSS(
+              "GNSS synced frame ts: "
+                << real_frame_ts.nanoseconds() << " nsec");
+            float dT_sec = (static_cast<float>(real_frame_ts.nanoseconds()) -
+              static_cast<float>(mGnssTimestamp.nanoseconds())) /
+              1e9;
+            DEBUG_STREAM_GNSS(
+              "DeltaT: "
+                << dT_sec << " sec [" << std::fixed << std::setprecision(9)
+                << static_cast<float>(real_frame_ts.nanoseconds()) / 1e9 << "-"
+                << static_cast<float>(mGnssTimestamp.nanoseconds()) / 1e9 << "]");
+
+            if (dT_sec < 0.0) {
+              RCLCPP_WARN_STREAM(
+                get_logger(),
+                "GNSS sensor and ZED Timestamps are not good. dT = " << dT_sec
+                                                                    << " sec");
+            }
           }
         }
-      }
 
-      // ----> Check recording status
-      mRecMutex.lock();
-      if (mRecording) {
-        mRecStatus = mZed->getRecordingStatus();
+        // ----> Check recording status
+        mRecMutex.lock();
+        if (mRecording) {
+          mRecStatus = mZed->getRecordingStatus();
 
-        if (!mRecStatus.status) {
-          rclcpp::Clock steady_clock(RCL_STEADY_TIME);
-          RCLCPP_ERROR_THROTTLE(
-            get_logger(), steady_clock, 1000.0,
-            "Error saving frame to SVO");
+          if (!mRecStatus.status) {
+            rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+            RCLCPP_ERROR_THROTTLE(
+              get_logger(), steady_clock, 1000.0,
+              "Error saving frame to SVO");
+          }
         }
+        mRecMutex.unlock();
+        // <---- Check recording status
       }
-      mRecMutex.unlock();
-      // <---- Check recording status
-    }
 
-    // ----> Retrieve Image/Depth data if someone has subscribed to
-    // Retrieve data if there are subscriber to topics
-    if (areVideoDepthSubscribed()) {
-      DEBUG_STREAM_VD("Retrieving video/depth data");
-      retrieveVideoDepth();
+      // ----> Retrieve Image/Depth data if someone has subscribed to
+      // Retrieve data if there are subscriber to topics
+      if (areVideoDepthSubscribed()) {
+        DEBUG_STREAM_VD("Retrieving video/depth data");
+        retrieveVideoDepth();
 
-      rclcpp::Time pub_ts;
-      publishVideoDepth(pub_ts);
+        rclcpp::Time pub_ts;
+        publishVideoDepth(pub_ts);
 
-      if (!sl_tools::isZED(mCamRealModel) && mVdPublishing &&
-        pub_ts != TIMEZERO_ROS)
-      {
-        if (mSensCameraSync || mSvoMode || mSimMode) {
-          publishSensorsData(pub_ts);
+        if (!sl_tools::isZED(mCamRealModel) && mVdPublishing &&
+          pub_ts != TIMEZERO_ROS)
+        {
+          if (mSensCameraSync || mSvoMode || mSimMode) {
+            publishSensorsData(pub_ts);
+          }
         }
-      }
 
-      mVdPublishing = true;
-    } else {
-      mVdPublishing = false;
-    }
-    // <---- Retrieve Image/Depth data if someone has subscribed to
+        mVdPublishing = true;
+      } else {
+        mVdPublishing = false;
+      }
+      // <---- Retrieve Image/Depth data if someone has subscribed to
 
-    if (!mDepthDisabled) {
-      // ----> Retrieve the point cloud if someone has subscribed to
+      if (!mDepthDisabled) {
+        // ----> Retrieve the point cloud if someone has subscribed to
 
-      size_t cloudSubnumber = 0;
-      try {
-#ifndef FOUND_FOXY
-        cloudSubnumber = mPubCloud.getNumSubscribers();
-#else
-        cloudSubnumber = count_subscribers(mPubCloud->get_topic_name());
-#endif
-      } catch (...) {
-        rcutils_reset_error();
-        DEBUG_STREAM_PC(
-          "threadFunc_zedGrab: Exception while counting point cloud "
-          "subscribers");
-        continue;
-      }
+        size_t cloudSubnumber = 0;
+        try {
+  #ifndef FOUND_FOXY
+          cloudSubnumber = mPubCloud.getNumSubscribers();
+  #else
+          cloudSubnumber = count_subscribers(mPubCloud->get_topic_name());
+  #endif
+        } catch (...) {
+          rcutils_reset_error();
+          DEBUG_STREAM_PC(
+            "threadFunc_zedGrab: Exception while counting point cloud "
+            "subscribers");
+          continue;
+        }
 
-      if (cloudSubnumber > 0) {
-        // Run the point cloud conversion asynchronously to avoid slowing down
-        // all the program
-        // Retrieve raw pointCloud data if latest Pointcloud is ready
-        std::unique_lock<std::mutex> pc_lock(mPcMutex, std::defer_lock);
-
-        if (pc_lock.try_lock()) {
-          DEBUG_STREAM_PC("Retrieving point cloud");
-          mZed->retrieveMeasure(
-            mMatCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU,
-            mMatResol);
-
-          // Signal Pointcloud thread that a new pointcloud is ready
-          mPcDataReadyCondVar.notify_one();
-          mPcDataReady = true;
-          mPcPublishing = true;
+        if (cloudSubnumber > 0) {
+          // Run the point cloud conversion asynchronously to avoid slowing down
+          // all the program
+          // Retrieve raw pointCloud data if latest Pointcloud is ready
+          std::unique_lock<std::mutex> pc_lock(mPcMutex, std::defer_lock);
+
+          if (pc_lock.try_lock()) {
+            DEBUG_STREAM_PC("Retrieving point cloud");
+            mZed->retrieveMeasure(
+              mMatCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU,
+              mMatResol);
+
+            // Signal Pointcloud thread that a new pointcloud is ready
+            mPcDataReadyCondVar.notify_one();
+            mPcDataReady = true;
+            mPcPublishing = true;
+          }
+        } else {
+          mPcPublishing = false;
         }
-      } else {
-        mPcPublishing = false;
-      }
-      // <---- Retrieve the point cloud if someone has subscribed to
+        // <---- Retrieve the point cloud if someone has subscribed to
 
-      // ----> Localization processing
-      if (mPosTrackingStarted) {
-        if (!mSvoPause) {
-          DEBUG_PT("================================================================");
-          DEBUG_PT("***** processOdometry *****");
-          processOdometry();
-          DEBUG_PT("***** processPose *****");
-          processPose();
-          if (mGnssFusionEnabled) {
-            if (mSvoMode) {
-              DEBUG_PT("***** processSvoGnssData *****");
-              processSvoGnssData();
+        // ----> Localization processing
+        if (mPosTrackingStarted) {
+          if (!mSvoPause) {
+            DEBUG_PT("================================================================");
+            DEBUG_PT("***** processOdometry *****");
+            processOdometry();
+            DEBUG_PT("***** processPose *****");
+            processPose();
+            if (mGnssFusionEnabled) {
+              if (mSvoMode) {
+                DEBUG_PT("***** processSvoGnssData *****");
+                processSvoGnssData();
+              }
+              DEBUG_PT("***** processGeoPose *****");
+              processGeoPose();
             }
-            DEBUG_PT("***** processGeoPose *****");
-            processGeoPose();
           }
+
+          // Publish `odom` and `map` TFs at the grab frequency
+          // RCLCPP_INFO(get_logger(), "Publishing TF -> threadFunc_zedGrab");
+          DEBUG_PT("***** publishTFs *****");
+          publishTFs(mFrameTimestamp);
         }
+        // <---- Localization processing
 
-        // Publish `odom` and `map` TFs at the grab frequency
-        // RCLCPP_INFO(get_logger(), "Publishing TF -> threadFunc_zedGrab");
-        DEBUG_PT("***** publishTFs *****");
-        publishTFs(mFrameTimestamp);
-      }
-      // <---- Localization processing
+        mObjDetMutex.lock();
+        if (mObjDetRunning) {
+          processDetectedObjects(mFrameTimestamp);
+        }
+        mObjDetMutex.unlock();
 
-      mObjDetMutex.lock();
-      if (mObjDetRunning) {
-        processDetectedObjects(mFrameTimestamp);
-      }
-      mObjDetMutex.unlock();
+        mBodyTrkMutex.lock();
+        if (mBodyTrkRunning) {
+          processBodies(mFrameTimestamp);
+        }
+        mBodyTrkMutex.unlock();
 
-      mBodyTrkMutex.lock();
-      if (mBodyTrkRunning) {
-        processBodies(mFrameTimestamp);
+        // ----> Region of interest
+        processRtRoi(mFrameTimestamp);
+        // <---- Region of interest
       }
-      mBodyTrkMutex.unlock();
 
-      // ----> Region of interest
-      processRtRoi(mFrameTimestamp);
-      // <---- Region of interest
+      // Diagnostic statistics update
+      double mean_elab_sec = mElabPeriodMean_sec->addValue(grabElabTimer.toc());
+    } catch(...) {
+          rcutils_reset_error();
+          DEBUG_STREAM_COMM("threadFunc_zedGrab: Generic exception.");
+          continue;
     }
-
-    // Diagnostic statistics update
-    double mean_elab_sec = mElabPeriodMean_sec->addValue(grabElabTimer.toc());
   }
 
   DEBUG_STREAM_COMM("Grab thread finished");
@@ -6182,6 +6223,12 @@ void ZedCamera::threadFunc_zedGrab()
 
 rclcpp::Time ZedCamera::publishSensorsData(rclcpp::Time t)
 {
+  if(mGrabStatus!=sl::ERROR_CODE::SUCCESS) {
+    DEBUG_SENS("Camera not ready");
+    rclcpp::sleep_for(1s);
+    return TIMEZERO_ROS;
+  }
+
   // ----> Subscribers count
   DEBUG_STREAM_SENS("Sensors callback: counting subscribers");
 
@@ -6371,7 +6418,13 @@ rclcpp::Time ZedCamera::publishSensorsData(rclcpp::Time t)
       // <---- Covariances copy
 
       DEBUG_STREAM_SENS("Publishing IMU message");
-      mPubImu->publish(std::move(imuMsg));
+      try {
+        mPubImu->publish(std::move(imuMsg));
+      } catch (std::system_error & e) {
+        DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+      } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
     } else {
       mImuPublishing = false;
     }
@@ -6429,7 +6482,13 @@ rclcpp::Time ZedCamera::publishSensorsData(rclcpp::Time t)
       // <---- Covariances copy
 
       DEBUG_STREAM_SENS("Publishing IMU RAW message");
-      mPubImuRaw->publish(std::move(imuRawMsg));
+      try {
+        mPubImuRaw->publish(std::move(imuRawMsg));
+      } catch (std::system_error & e) {
+        DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+      } catch(...) {
+        DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
     }
   }
 
@@ -6448,7 +6507,13 @@ rclcpp::Time ZedCamera::publishSensorsData(rclcpp::Time t)
       pressMsg->variance = 1.0585e-2;
 
       DEBUG_STREAM_SENS("Publishing PRESS message");
-      mPubPressure->publish(std::move(pressMsg));
+      try {
+        mPubPressure->publish(std::move(pressMsg));
+      } catch (std::system_error & e) {
+        DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+      } catch(...) {
+        DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+      }
     } else {
       mBaroPublishing = false;
     }
@@ -6479,7 +6544,13 @@ rclcpp::Time ZedCamera::publishSensorsData(rclcpp::Time t)
       magMsg->magnetic_field_covariance[8] = 0.047e-6;
 
       DEBUG_STREAM_SENS("Publishing MAG message");
-      mPubImuMag->publish(std::move(magMsg));
+      try {
+        mPubImuMag->publish(std::move(magMsg));
+      } catch (std::system_error & e) {
+        DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+      } catch(...) {
+        DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+      }
     } else {
       mMagPublishing = false;
     }
@@ -6743,7 +6814,7 @@ void ZedCamera::threadFunc_pointcloudElab()
 
     if (elapsed_usec < pc_period_usec) {
       int wait_usec = static_cast<int>(pc_period_usec - elapsed_usec);
-      +rclcpp::sleep_for(std::chrono::microseconds(wait_usec));
+      rclcpp::sleep_for(std::chrono::microseconds(wait_usec));
       DEBUG_STREAM_PC("threadFunc_pointcloudElab: wait_usec " << wait_usec);
     }
 
@@ -6834,53 +6905,59 @@ void ZedCamera::threadFunc_pubSensorsData()
   // <---- Advanced thread settings
 
   while (1) {
-    if (!rclcpp::ok()) {
-      DEBUG_STREAM_SENS("Ctrl+C received: stopping sensors thread");
-      mThreadStop = true;
-      break;
-    }
-    if (mThreadStop) {
-      DEBUG_STREAM_SENS(
-        "threadFunc_pubSensorsData (2): Sensors thread stopped");
-      break;
-    }
+    try {
+      if (!rclcpp::ok()) {
+        DEBUG_STREAM_SENS("Ctrl+C received: stopping sensors thread");
+        mThreadStop = true;
+        break;
+      }
+      if (mThreadStop) {
+        DEBUG_STREAM_SENS(
+          "threadFunc_pubSensorsData (2): Sensors thread stopped");
+        break;
+      }
 
-    // std::lock_guard<std::mutex> lock(mCloseZedMutex);
-    if (!mZed->isOpened()) {
-      DEBUG_STREAM_SENS("threadFunc_pubSensorsData: the camera is not open");
-      continue;
-    }
+      // std::lock_guard<std::mutex> lock(mCloseZedMutex);
+      if (!mZed->isOpened()) {
+        DEBUG_STREAM_SENS("threadFunc_pubSensorsData: the camera is not open");
+        continue;
+      }
 
-    // RCLCPP_INFO_STREAM(get_logger(),
-    // "threadFunc_pubSensorsData: Publishing Camera-IMU transform ");
-    // publishImuFrameAndTopic();
-    rclcpp::Time sens_ts = publishSensorsData();
+      // RCLCPP_INFO_STREAM(get_logger(),
+      // "threadFunc_pubSensorsData: Publishing Camera-IMU transform ");
+      // publishImuFrameAndTopic();
+      rclcpp::Time sens_ts = publishSensorsData();
 
-    // RCLCPP_INFO_STREAM(get_logger(), "threadFunc_pubSensorsData - sens_ts
-    // type:"
-    // << sens_ts.get_clock_type());
+      // RCLCPP_INFO_STREAM(get_logger(), "threadFunc_pubSensorsData - sens_ts
+      // type:"
+      // << sens_ts.get_clock_type());
 
-    // Publish TF at the same frequency of IMU data, so they are always
-    // synchronized
-    /*if (sens_ts != TIMEZERO_ROS)
-    {
-      RCLCPP_INFO(get_logger(), "Publishing TF -> threadFunc_pubSensorsData");
-      publishTFs(sens_ts);
-    }*/
+      // Publish TF at the same frequency of IMU data, so they are always
+      // synchronized
+      /*if (sens_ts != TIMEZERO_ROS)
+      {
+        RCLCPP_INFO(get_logger(), "Publishing TF -> threadFunc_pubSensorsData");
+        publishTFs(sens_ts);
+      }*/
 
-    // ----> Check publishing frequency
-    double sens_period_usec = 1e6 / mSensPubRate;
+      // ----> Check publishing frequency
+      double sens_period_usec = 1e6 / mSensPubRate;
 
-    double elapsed_usec = mSensPubFreqTimer.toc() * 1e6;
+      double elapsed_usec = mSensPubFreqTimer.toc() * 1e6;
 
-    if (elapsed_usec < sens_period_usec) {
-      rclcpp::sleep_for(
-        std::chrono::microseconds(
-          static_cast<int>(sens_period_usec - elapsed_usec)));
-    }
+      if (elapsed_usec < sens_period_usec) {
+        rclcpp::sleep_for(
+          std::chrono::microseconds(
+            static_cast<int>(sens_period_usec - elapsed_usec)));
+      }
 
-    mSensPubFreqTimer.tic();
-    // <---- Check publishing frequency
+      mSensPubFreqTimer.tic();
+      // <---- Check publishing frequency
+    } catch (...) {
+        rcutils_reset_error();
+        DEBUG_STREAM_COMM("threadFunc_pubSensorsData: Generic exception.");
+        continue;
+    }
   }
 
   DEBUG_STREAM_SENS("Sensors thread finished");
@@ -7226,7 +7303,13 @@ void ZedCamera::publishVideoDepth(rclcpp::Time & out_pub_ts)
       mMatLeft, mMatRight,
       mCameraFrameId, out_pub_ts);
     DEBUG_STREAM_VD("Publishing SIDE-BY-SIDE message");
-    mPubStereo.publish(std::move(combined));
+    try {
+      mPubStereo.publish(std::move(combined));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
   // <---- Publish the side-by-side image if someone has subscribed to
 
@@ -7237,7 +7320,13 @@ void ZedCamera::publishVideoDepth(rclcpp::Time & out_pub_ts)
       mMatLeftRaw, mMatRightRaw,
       mCameraFrameId, out_pub_ts);
     DEBUG_STREAM_VD("Publishing SIDE-BY-SIDE RAW message");
-    mPubRawStereo.publish(std::move(combined));
+    try {
+      mPubRawStereo.publish(std::move(combined));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
   // <---- Publish the side-by-side image if someone has subscribed to
 
@@ -7250,8 +7339,14 @@ void ZedCamera::publishVideoDepth(rclcpp::Time & out_pub_ts)
   // ---->  Publish the confidence image and map if someone has subscribed to
   if (mConfMapSubnumber > 0) {
     DEBUG_STREAM_VD("Publishing CONF MAP message");
-    mPubConfMap->publish(
-      *sl_tools::imageToROSmsg(mMatConf, mDepthOptFrameId, out_pub_ts));
+    try {
+      mPubConfMap->publish(
+        *sl_tools::imageToROSmsg(mMatConf, mDepthOptFrameId, out_pub_ts));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
   // <----  Publish the confidence image and map if someone has subscribed to
 
@@ -7271,7 +7366,13 @@ void ZedCamera::publishVideoDepth(rclcpp::Time & out_pub_ts)
     depthInfoMsg->max_depth = mMaxDepth;
 
     DEBUG_STREAM_VD("Publishing DEPTH INFO message");
-    mPubDepthInfo->publish(std::move(depthInfoMsg));
+    try {
+      mPubDepthInfo->publish(std::move(depthInfoMsg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
   // <---- Publish the depth info if someone has subscribed to
 
@@ -7304,7 +7405,13 @@ void ZedCamera::publishImageWithInfo(
   auto image = sl_tools::imageToROSmsg(img, imgFrameId, t);
   camInfoMsg->header.stamp = t;
   DEBUG_STREAM_VD("Publishing IMAGE message: " << t.nanoseconds() << " nsec");
-  pubImg.publish(std::move(image), camInfoMsg);
+  try {
+    pubImg.publish(std::move(image), camInfoMsg);
+  } catch (std::system_error & e) {
+    DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+  } catch(...) {
+    DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+  }
 }
 
 void ZedCamera::processOdometry()
@@ -7479,7 +7586,13 @@ void ZedCamera::publishOdom(
 
     // Publish odometry message
     DEBUG_STREAM_PT("Publishing ODOM message");
-    mPubOdom->publish(std::move(odomMsg));
+    try {
+      mPubOdom->publish(std::move(odomMsg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 }
 
@@ -7609,7 +7722,13 @@ void ZedCamera::publishPoseStatus()
     msg->odometry_status = static_cast<uint8_t>(mPosTrackingStatus.odometry_status);
     msg->spatial_memory_status = static_cast<uint8_t>(mPosTrackingStatus.spatial_memory_status);
 
-    mPubPoseStatus->publish(std::move(msg));
+    try {
+      mPubPoseStatus->publish(std::move(msg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 }
 
@@ -7631,7 +7750,13 @@ void ZedCamera::publishGnssPoseStatus()
 
     msg->gnss_fusion_status = static_cast<uint8_t>(mFusedPosTrackingStatus.gnss_fusion_status);
 
-    mPubGnssPoseStatus->publish(std::move(msg));
+    try {
+      mPubGnssPoseStatus->publish(std::move(msg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 }
 
@@ -7655,7 +7780,13 @@ void ZedCamera::publishGeoPoseStatus()
     msg->gnss_fusion_status =
       static_cast<uint8_t>(mFusedPosTrackingStatus.gnss_fusion_status);
 
-    mPubGeoPoseStatus->publish(std::move(msg));
+    try {
+      mPubGeoPoseStatus->publish(std::move(msg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 }
 
@@ -7701,7 +7832,13 @@ void ZedCamera::publishPose()
 
     // Publish pose stamped message
     DEBUG_STREAM_PT("Publishing POSE NO COV message");
-    mPubPose->publish(std::move(poseNoCov));
+    try {
+      mPubPose->publish(std::move(poseNoCov));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 
   if (mPublishPoseCov) {
@@ -7730,7 +7867,13 @@ void ZedCamera::publishPose()
 
       // Publish pose with covariance stamped message
       DEBUG_STREAM_PT("Publishing POSE COV message");
-      mPubPoseCov->publish(std::move(poseCov));
+      try {
+        mPubPoseCov->publish(std::move(poseCov));
+      } catch (std::system_error & e) {
+        DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+      } catch(...) {
+        DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+      }
     }
   }
 }
@@ -7941,7 +8084,13 @@ void ZedCamera::publishGnssPose()
 
     // Publish gnss message
     // DEBUG_GNSS("Publishing GNSS pose message");
-    mPubGnssPose->publish(std::move(msg));
+    try {
+      mPubGnssPose->publish(std::move(msg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 
   if (geoPoseSub > 0) {
@@ -7964,7 +8113,13 @@ void ZedCamera::publishGnssPose()
 
     // Publish gnss message
     // DEBUG_GNSS("Publishing GeoPose message");
-    mPubGeoPose->publish(std::move(msg));
+    try {
+      mPubGeoPose->publish(std::move(msg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 
   if (fusedFixSub > 0) {
@@ -8000,8 +8155,14 @@ void ZedCamera::publishGnssPose()
     // <---- Covariance
 
     // Publish Fused Fix message
-    // DEBUG_GNSS("Publishing Fused Fix message");
-    mPubFusedFix->publish(std::move(msg));
+    // DEBUG_GNSS("Publishing Fused Fix message");ù
+    try {
+      mPubFusedFix->publish(std::move(msg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 
   if (originFixSub > 0) {
@@ -8026,7 +8187,13 @@ void ZedCamera::publishGnssPose()
 
     // Publish Fused Fix message
     // DEBUG_GNSS("Publishing Fused Fix message");
-    mPubOriginFix->publish(std::move(msg));
+    try {
+      mPubOriginFix->publish(std::move(msg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 }
 
@@ -8171,7 +8338,13 @@ void ZedCamera::processDetectedObjects(rclcpp::Time t)
   }
 
   // DEBUG_STREAM_OD("Publishing OBJ DET message");
-  mPubObjDet->publish(std::move(objMsg));
+  try {
+    mPubObjDet->publish(std::move(objMsg));
+  } catch (std::system_error & e) {
+    DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+  } catch(...) {
+    DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+  }
 
   // ----> Diagnostic information update
   mObjDetElabMean_sec->addValue(odElabTimer.toc());
@@ -8328,7 +8501,13 @@ void ZedCamera::processBodies(rclcpp::Time t)
   }
 
   DEBUG_STREAM_OD("Publishing BODY TRK message");
-  mPubBodyTrk->publish(std::move(bodyMsg));
+  try {
+    mPubBodyTrk->publish(std::move(bodyMsg));
+  } catch (std::system_error & e) {
+    DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+  } catch(...) {
+    DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+  }
 
   // ----> Diagnostic information update
   mBodyTrkElabMean_sec->addValue(btElabTimer.toc());
@@ -8788,7 +8967,13 @@ void ZedCamera::publishDepthMapWithInfo(sl::Mat & depth, rclcpp::Time t)
     DEBUG_STREAM_VD(
       "Publishing DEPTH message: " << t.nanoseconds()
                                    << " nsec");
-    mPubDepth.publish(std::move(depth_img), mDepthCamInfoMsg);
+    try {
+      mPubDepth.publish(std::move(depth_img), mDepthCamInfoMsg);
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
     return;
   }
 
@@ -8821,7 +9006,13 @@ void ZedCamera::publishDepthMapWithInfo(sl::Mat & depth, rclcpp::Time t)
   }
 
   DEBUG_STREAM_VD("Publishing OPENNI DEPTH message");
-  mPubDepth.publish(std::move(openniDepthMsg), mDepthCamInfoMsg);
+  try {
+    mPubDepth.publish(std::move(openniDepthMsg), mDepthCamInfoMsg);
+  } catch (std::system_error & e) {
+    DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+  } catch(...) {
+    DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+  }
 }
 
 void ZedCamera::publishDisparity(sl::Mat disparity, rclcpp::Time t)
@@ -8847,7 +9038,13 @@ void ZedCamera::publishDisparity(sl::Mat disparity, rclcpp::Time t)
     mZed->getInitParameters().depth_maximum_distance;
 
   DEBUG_STREAM_VD("Publishing DISPARITY message");
-  mPubDisparity->publish(std::move(disparityMsg));
+  try {
+    mPubDisparity->publish(std::move(disparityMsg));
+  } catch (std::system_error & e) {
+    DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+  } catch(...) {
+    DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+  }
 }
 
 void ZedCamera::publishPointCloud()
@@ -8918,9 +9115,21 @@ void ZedCamera::publishPointCloud()
   // Pointcloud publishing
   DEBUG_STREAM_PC("Publishing POINT CLOUD message");
 #ifndef FOUND_FOXY
-  mPubCloud.publish(std::move(pcMsg));
+  try {
+    mPubCloud.publish(std::move(pcMsg));
+  } catch (std::system_error & e) {
+    DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+  } catch(...) {
+    DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+  }
 #else
-  mPubCloud->publish(std::move(pcMsg));
+  try {
+    mPubCloud->publish(std::move(pcMsg));
+  } catch (std::system_error & e) {
+    DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+  } catch(...) {
+    DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+  }
 #endif
 
   // Publish freq calculation
@@ -8933,9 +9142,16 @@ void ZedCamera::publishPointCloud()
 }
 
 void ZedCamera::callback_pubTemp()
-{
+{  
   DEBUG_STREAM_ONCE_SENS("Temperatures callback called");
 
+  if(mGrabStatus!=sl::ERROR_CODE::SUCCESS) {
+    DEBUG_SENS("Camera not ready");
+    rclcpp::sleep_for(1s);
+    return;
+  }
+
+
   if (sl_tools::isZED(mCamRealModel) || sl_tools::isZEDM(mCamRealModel)) {
     DEBUG_SENS(
       "callback_pubTemp: the callback should never be called for the ZED "
@@ -9006,7 +9222,13 @@ void ZedCamera::callback_pubTemp()
     leftTempMsg->temperature = static_cast<double>(mTempLeft);
     leftTempMsg->variance = 0.0;
 
-    mPubTempL->publish(std::move(leftTempMsg));
+    try {
+      mPubTempL->publish(std::move(leftTempMsg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 
   if (tempRightSubNumber > 0) {
@@ -9020,7 +9242,13 @@ void ZedCamera::callback_pubTemp()
     rightTempMsg->variance = 0.0;
 
     DEBUG_STREAM_SENS("Publishing RIGHT TEMP message");
-    mPubTempR->publish(std::move(rightTempMsg));
+    try {
+      mPubTempR->publish(std::move(rightTempMsg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 
   if (tempImuSubNumber > 0) {
@@ -9033,7 +9261,13 @@ void ZedCamera::callback_pubTemp()
     imuTempMsg->variance = 0.0;
 
     DEBUG_SENS("Publishing IMU TEMP message");
-    mPubImuTemp->publish(std::move(imuTempMsg));
+    try {
+      mPubImuTemp->publish(std::move(imuTempMsg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 }
 
@@ -9146,9 +9380,21 @@ void ZedCamera::callback_pubFusedPc()
   // Pointcloud publishing
   DEBUG_STREAM_MAP("Publishing FUSED POINT CLOUD message");
 #ifndef FOUND_FOXY
-  mPubFusedCloud.publish(std::move(pointcloudFusedMsg));
+  try {
+    mPubFusedCloud.publish(std::move(pointcloudFusedMsg));
+  } catch (std::system_error & e) {
+    DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+  } catch(...) {
+    DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+  }
 #else
-  mPubFusedCloud->publish(std::move(pointcloudFusedMsg));
+  try {
+    mPubFusedCloud->publish(std::move(pointcloudFusedMsg));
+  } catch (std::system_error & e) {
+    DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+  } catch(...) {
+    DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+  }
 #endif
 }
 
@@ -9234,7 +9480,13 @@ void ZedCamera::callback_pubPaths()
     mapPathMsg->poses = mPosePath;
 
     DEBUG_STREAM_PT("Publishing MAP PATH message");
-    mPubPosePath->publish(std::move(mapPathMsg));
+    try {
+      mPubPosePath->publish(std::move(mapPathMsg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 
   if (odomPathSub > 0) {
@@ -9244,7 +9496,13 @@ void ZedCamera::callback_pubPaths()
     odomPathMsg->poses = mOdomPath;
 
     DEBUG_STREAM_PT("Publishing ODOM PATH message");
-    mPubOdomPath->publish(std::move(odomPathMsg));
+    try {
+      mPubOdomPath->publish(std::move(odomPathMsg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
   }
 }
 
@@ -9907,6 +10165,10 @@ void ZedCamera::callback_updateDiagnostic(
     } else {
       stat.add("TF IMU", "DISABLED");
     }
+  } else if(mGrabStatus==sl::ERROR_CODE::LAST){
+    stat.summary(
+      diagnostic_msgs::msg::DiagnosticStatus::OK,
+      "Camera initializing" );
   } else {
     stat.summaryf(
       diagnostic_msgs::msg::DiagnosticStatus::ERROR,
@@ -10433,7 +10695,13 @@ void ZedCamera::callback_clickedPoint(
 
     // Publish the marker
     DEBUG_STREAM_MAP("Publishing PT MARKER message");
-    mPubMarker->publish(std::move(pt_marker));
+    try {
+      mPubMarker->publish(std::move(pt_marker));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
     // ----> Publish a blue sphere in the clicked point
 
     // ----> Publish the plane as green mesh
@@ -10499,7 +10767,13 @@ void ZedCamera::callback_clickedPoint(
 
     // Publish the marker
     DEBUG_STREAM_MAP("Publishing PLANE MARKER message");
-    mPubMarker->publish(std::move(plane_marker));
+    try {
+      mPubMarker->publish(std::move(plane_marker));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
     // <---- Publish the plane as green mesh
   }
 
@@ -10578,7 +10852,13 @@ void ZedCamera::callback_clickedPoint(
     }
 
     DEBUG_STREAM_MAP("Publishing PLANE message");
-    mPubPlane->publish(std::move(planeMsg));
+    try {
+      mPubPlane->publish(std::move(planeMsg));
+    } catch (std::system_error & e) {
+      DEBUG_STREAM_COMM("Message publishing ecception: " << e.what());
+    } catch(...) {
+      DEBUG_STREAM_COMM("Message publishing generic ecception: ");
+    }
     // <---- Publish the plane as custom message
   }
 }
diff --git a/zed_wrapper/config/common.yaml b/zed_wrapper/config/common.yaml
index 0c257b1c..3cbb7e96 100644
--- a/zed_wrapper/config/common.yaml
+++ b/zed_wrapper/config/common.yaml
@@ -20,7 +20,7 @@
             camera_max_reconnect: 5
             camera_flip: false
             serial_number: 0 # usually overwritten by launch file
-            pub_resolution: "NATIVE" # The resolution used for output. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission
+            pub_resolution: "CUSTOM" # The resolution used for output. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission
             pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM'
             pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images
             gpu_id: -1
diff --git a/zed_wrapper/config/ffmpeg.yaml b/zed_wrapper/config/ffmpeg.yaml
index 37ece5e0..82bad86d 100644
--- a/zed_wrapper/config/ffmpeg.yaml
+++ b/zed_wrapper/config/ffmpeg.yaml
@@ -9,7 +9,7 @@
         ffmpeg_image_transport:
           bit_rate: 4194304 # Default: 8242880
           delay: '0' # default is 4 frames for parallel processing. 0 is lowest latency
-          encoding: 'hevc_nvenc' # Only ever tested: libx264, h264_nvenc, h264, hevc_nvenc, h264_vaapi. If you have an Nvidia card it most likely supports hevc_nvenc. This will dramatically reduce the CPU load compare to libx264 (the default). You can list all available codecs with ffmpeg -codecs. In the relevant row, look for what it says under (encoders).
+          encoding: 'libx264' # Only ever tested: libx264, h264_nvenc, h264, hevc_nvenc, h264_vaapi. If you have an Nvidia card it most likely supports hevc_nvenc. This will dramatically reduce the CPU load compare to libx264 (the default). You can list all available codecs with ffmpeg -codecs. In the relevant row, look for what it says under (encoders).
           gop_size: 10 # The number of frames inbetween keyframes. Default is 15. The larger this number the more latency you will have, but also the more efficient the transmission becomes.
           measure_performance: false # Enable performance analysis
           performance_interval: 100 # number of frames between perf printouts
diff --git a/zed_wrapper/launch/zed_camera.launch.py b/zed_wrapper/launch/zed_camera.launch.py
index 32d65b82..b6708496 100644
--- a/zed_wrapper/launch/zed_camera.launch.py
+++ b/zed_wrapper/launch/zed_camera.launch.py
@@ -195,8 +195,9 @@ def launch_setup(context, *args, **kwargs):
         executable='zed_wrapper',
         name=node_name,
         output='screen',
-        # prefix=['xterm -e valgrind --tools=callgrind'],
-        # prefix=['xterm -e gdb -ex run --args'],
+        #prefix=['valgrind'],
+        #prefix=['xterm -e valgrind --tools=callgrind'],
+        #prefix=['xterm -e gdb -ex run --args'],
         #prefix=['gdbserver localhost:3000'],
         parameters=node_parameters
     )