fix: add device type check for status timer initialization and improve reset logic

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -279,9 +279,11 @@ void OBCameraNodeDriver::init() {
   check_connect_timer_ =
       this->create_wall_timer(std::chrono::milliseconds(1000), [this]() { checkConnectTimer(); });
   CHECK_NOTNULL(check_connect_timer_);
+  if (device_type_ == "camera") {
     device_status_timer_ =
         this->create_wall_timer(std::chrono::milliseconds(1000 / device_status_interval_hz),
                                 [this]() { deviceStatusTimer(); });
+  }
   auto qos = rclcpp::QoS(1).transient_local();
   if (node_options_.use_intra_process_comms()) {
     qos = rclcpp::QoS(1);
@@ -447,7 +449,6 @@ void OBCameraNodeDriver::queryDevice() {
 void OBCameraNodeDriver::resetDevice() {
   while (is_alive_ && rclcpp::ok()) {
     {
-      if (ob_camera_node_) {
       std::unique_lock<decltype(reset_device_mutex_)> lock(reset_device_mutex_);
       // Use a timeout to make the wait interruptible
       auto timeout = std::chrono::milliseconds(1000);
@@ -483,13 +484,9 @@ void OBCameraNodeDriver::resetDevice() {
 
         // Reset objects in order, with additional safety checks
         if (ob_camera_node_) {
-            try {
-              RCLCPP_INFO_STREAM(logger_, "Resetting ob_camera_node_");
           ob_camera_node_.reset();
-              RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ reset completed");
+        } else if (ob_lidar_node_) {
-            } catch (...) {
+          ob_lidar_node_.reset();
-              RCLCPP_WARN_STREAM(logger_, "Exception during ob_camera_node reset");
-            }
         }
 
         // Allow more time for internal SDK cleanup
@@ -529,13 +526,6 @@ void OBCameraNodeDriver::resetDevice() {
 
         device_unique_id_.clear();
       }
-      } else if (ob_lidar_node_) {
-        ob_lidar_node_.reset();
-        device_.reset();
-        device_info_.reset();
-        device_connected_ = false;
-        device_unique_id_.clear();
-      }
       reset_device_flag_ = false;
       last_reset_device_completion_time_ = std::chrono::steady_clock::now();
     }
@@ -707,7 +697,6 @@ void OBCameraNodeDriver::deviceStatusTimer() {
   }
   // RCLCPP_INFO_STREAM(logger_, "deviceStatusTimer() ");
 }
-
 void OBCameraNodeDriver::rebootDeviceCallback(
     const std::shared_ptr<std_srvs::srv::Empty::Request> request,
     std::shared_ptr<std_srvs::srv::Empty::Response> response) {
@@ -727,7 +716,6 @@ void OBCameraNodeDriver::rebootDeviceCallback(
     return;
   }
 
-  if (ob_camera_node_) {
   std::shared_ptr<int> process_lock_guard(
       nullptr, [this](int *) { pthread_mutex_unlock(orb_device_lock_); });
 
@@ -737,15 +725,19 @@ void OBCameraNodeDriver::rebootDeviceCallback(
     {
       std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
 
-        if (!device_connected_ || !ob_camera_node_) {
+      if (!device_connected_ || (!ob_camera_node_ && !ob_lidar_node_)) {
         RCLCPP_INFO(logger_, "Device not connected");
         reset_device_flag_ = false;
       } else {
         std::string current_device_uid = device_unique_id_;
         RCLCPP_INFO_STREAM(logger_, "Rebooting device with UID: " << current_device_uid);
+        if (ob_lidar_node_) {
+          ob_lidar_node_->rebootDevice();
+        } else if (ob_camera_node_) {
           ob_camera_node_->rebootDevice();
         }
       }
+    }
     if (reset_device_flag_) {
       RCLCPP_INFO(logger_, "Device reboot initiated, waiting for reconnection");
     }
@@ -761,13 +753,98 @@ void OBCameraNodeDriver::rebootDeviceCallback(
   }
   malloc_trim(0);
   return;
-    RCLCPP_INFO(logger_, "Reboot device");
-  } else if (ob_lidar_node_) {
-    ob_lidar_node_->rebootDevice();
-    device_connected_ = false;
-    device_ = nullptr;
-  }
 }
+// void OBCameraNodeDriver::rebootDeviceCallback(
+//     const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+//     std::shared_ptr<std_srvs::srv::Empty::Response> response) {
+//   (void)request;
+//   (void)response;
+
+//   malloc_trim(0);
+//   RCLCPP_INFO(logger_, "Reboot device service called");
+
+//   struct timespec timeout;
+//   clock_gettime(CLOCK_REALTIME, &timeout);
+//   timeout.tv_sec += 15;
+
+//   int lock_result = pthread_mutex_timedlock(orb_device_lock_, &timeout);
+//   if (lock_result != 0) {
+//     RCLCPP_WARN(logger_, "Failed to acquire process lock for reboot: %s", strerror(lock_result));
+//     return;
+//   }
+
+//   if (ob_camera_node_) {
+//     std::shared_ptr<int> process_lock_guard(
+//         nullptr, [this](int *) { pthread_mutex_unlock(orb_device_lock_); });
+
+//     try {
+//       std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
+//       reset_device_flag_ = true;
+//       {
+//         std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
+
+//         if (!device_connected_ || !ob_camera_node_) {
+//           RCLCPP_INFO(logger_, "Device not connected");
+//           reset_device_flag_ = false;
+//         } else {
+//           std::string current_device_uid = device_unique_id_;
+//           RCLCPP_INFO_STREAM(logger_, "Rebooting device with UID: " << current_device_uid);
+//           ob_camera_node_->rebootDevice();
+//         }
+//       }
+//       if (reset_device_flag_) {
+//         RCLCPP_INFO(logger_, "Device reboot initiated, waiting for reconnection");
+//       }
+
+//     } catch (std::exception &e) {
+//       RCLCPP_ERROR_STREAM(logger_, "Failed to reboot device: " << e.what());
+//     } catch (...) {
+//       RCLCPP_ERROR_STREAM(logger_, "Failed to reboot device: unknown error");
+//     }
+//     process_lock_guard.reset();
+//     if (reset_device_flag_) {
+//       reset_device_cond_.notify_all();
+//     }
+//     malloc_trim(0);
+//     return;
+//   } else if (ob_lidar_node_) {
+//     std::shared_ptr<int> process_lock_guard(
+//         nullptr, [this](int *) { pthread_mutex_unlock(orb_device_lock_); });
+
+//     try {
+//       std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
+//       reset_device_flag_ = true;
+//       {
+//         std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
+
+//         if (!device_connected_ || !ob_lidar_node_) {
+//           RCLCPP_INFO(logger_, "Device not connected");
+//           reset_device_flag_ = false;
+//         } else {
+//           std::string current_device_uid = device_unique_id_;
+//           RCLCPP_INFO_STREAM(logger_, "Rebooting lidar device with UID: " << current_device_uid);
+//           ob_lidar_node_->rebootDevice();
+//         }
+//       }
+//       if (reset_device_flag_) {
+//         RCLCPP_INFO(logger_, "Lidar device reboot initiated, waiting for reconnection");
+//       }
+
+//     } catch (std::exception &e) {
+//       RCLCPP_ERROR_STREAM(logger_, "Failed to reboot lidar device: " << e.what());
+//     } catch (...) {
+//       RCLCPP_ERROR_STREAM(logger_, "Failed to reboot lidar device: unknown error");
+//     }
+//     process_lock_guard.reset();
+//     if (reset_device_flag_) {
+//       reset_device_cond_.notify_all();
+//     }
+//     malloc_trim(0);
+//   } else {
+//     // No device node exists, just unlock
+//     pthread_mutex_unlock(orb_device_lock_);
+//   }
+// }
 
 std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
     const std::shared_ptr<ob::DeviceList> &list) {