From 68662847c5d0912a54a18ff2d69606ca909bc208 Mon Sep 17 00:00:00 2001
From: Joe Dong <jian.dong@pobox.com>
Date: Wed, 28 Dec 2022 17:11:30 +0800
Subject: [PATCH] add check connection timer

---
 .../include/orbbec_camera/ob_camera_node_factory.h     |  3 +++
 orbbec_camera/src/ob_camera_node_factory.cpp           | 10 +++++++++-
 2 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h
index e6cc41cb..2f3aa046 100644
--- a/orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h
+++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h
@@ -39,6 +39,8 @@ class OBCameraNodeFactory : public rclcpp::Node {
 
   static OBLogSeverity obLogSeverityFromString(const std::string& log_level);
 
+  void checkConnectTimer();
+
   void queryDevice();
 
  private:
@@ -55,6 +57,7 @@ class OBCameraNodeFactory : public rclcpp::Node {
   std::shared_ptr<std::thread> query_thread_ = nullptr;
   std::recursive_mutex device_lock_;
   size_t device_num_ = 1;
+  rclcpp::TimerBase::SharedPtr check_connect_timer_ = nullptr;
 };
 }  // namespace orbbec_camera
 
diff --git a/orbbec_camera/src/ob_camera_node_factory.cpp b/orbbec_camera/src/ob_camera_node_factory.cpp
index 512f6315..ea08a191 100644
--- a/orbbec_camera/src/ob_camera_node_factory.cpp
+++ b/orbbec_camera/src/ob_camera_node_factory.cpp
@@ -52,6 +52,8 @@ void OBCameraNodeFactory::init() {
     deviceDisconnectCallback(removed_list);
     deviceConnectCallback(added_list);
   });
+  check_connect_timer_ =
+      this->create_wall_timer(std::chrono::milliseconds(1000), [this]() { checkConnectTimer(); });
   query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); });
 }
 
@@ -105,10 +107,16 @@ OBLogSeverity OBCameraNodeFactory::obLogSeverityFromString(const std::string &lo
   } else if (log_level == "fatal") {
     return OBLogSeverity::OB_LOG_SEVERITY_FATAL;
   } else {
-    return OBLogSeverity::OB_LOG_SEVERITY_INFO;
+    return OBLogSeverity::OB_LOG_SEVERITY_NONE;
   }
 }
 
+void OBCameraNodeFactory::checkConnectTimer() {
+  if (!device_connected_) {
+    RCLCPP_ERROR_STREAM(logger_, "checkConnectTimer: device not connected");
+    return;
+  }
+}
 void OBCameraNodeFactory::queryDevice() {
   while (is_alive_ && rclcpp::ok()) {
     std::lock_guard<decltype(device_lock_)> lock(device_lock_);