fixed HUB hot plug bug

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -72,6 +72,8 @@ class OBCameraNodeDriver : public rclcpp::Node {
 
   void syncTime();
 
+  void resetDevice();
+
  private:
   std::string config_path_;
   std::unique_ptr<ob::Context> ctx_ = nullptr;
@@ -92,5 +94,9 @@ class OBCameraNodeDriver : public rclcpp::Node {
   int num_devices_connected_ = 0;
   rclcpp::TimerBase::SharedPtr check_connect_timer_ = nullptr;
   std::shared_ptr<std::thread> sync_time_thread_ = nullptr;
+  std::shared_ptr<std::thread> reset_device_thread_ = nullptr;
+  std::mutex reset_device_mutex_;
+  std::condition_variable reset_device_cond_;
+  std::atomic_bool reset_device_flag_{false};
 };
 }  // namespace orbbec_camera

orbbec_camera/launch/multi_camera.launch.xml

@@ -6,21 +6,21 @@
     <!-- stereo_s_u3, astrapro, astra -->
     <arg name="camera1_prefix" default="01"/>
     <arg name="camera2_prefix" default="02"/>
-    <arg name="camera1_serila_number" default="AY3A131007R"/>
+    <arg name="camera1_usb_port" default="2-3.3"/>
-    <arg name="camera2_serila_number" default="AY3JB20003L"/>
+    <arg name="camera2_usb_port" default="1-4.4"/>
     <arg name="device_num" default="2"/>
     <node name="camera" pkg="orbbec_camera" exec="ob_cleanup_shm_node" output="screen"/>
     <group>
         <include file="$(find-pkg-share orbbec_camera)/launch/$(var 3d_sensor).launch.xml">
             <arg name="camera_name" value="camera_$(var camera1_prefix)"/>
-            <arg name="serial_number" value="$(var camera1_serila_number)"/>
+            <arg name="usb_port" value="$(var camera1_usb_port)"/>
             <arg name="device_num" value="$(var device_num)"/>
         </include>
     </group>
     <group>
         <include file="$(find-pkg-share orbbec_camera)/launch/$(var 3d_sensor).launch.xml">
             <arg name="camera_name" value="camera_$(var camera2_prefix)"/>
-            <arg name="serial_number" value="$(var camera2_serila_number)"/>
+            <arg name="usb_port" value="$(var camera2_usb_port)"/>
             <arg name="device_num" value="$(var device_num)"/>
         </include>
     </group>

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -49,6 +49,10 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
   if (query_thread_ && query_thread_->joinable()) {
     query_thread_->join();
   }
+  if (reset_device_thread_ && reset_device_thread_->joinable()) {
+    reset_device_cond_.notify_all();
+    reset_device_thread_->join();
+  }
 }
 
 void OBCameraNodeDriver::init() {
@@ -71,7 +75,7 @@ void OBCameraNodeDriver::init() {
   query_thread_ = std::make_shared<std::thread>([this]() { queryDevice(); });
   device_count_update_thread_ = std::make_shared<std::thread>([this]() { deviceCountUpdate(); });
   sync_time_thread_ = std::make_shared<std::thread>([this]() { syncTime(); });
-  CHECK_NOTNULL(device_count_update_thread_);
+  reset_device_thread_ = std::make_shared<std::thread>([this]() { resetDevice(); });
 }
 
 void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
@@ -105,11 +109,10 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
     std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
     RCLCPP_INFO_STREAM(logger_, "device with " << uid << " disconnected");
     if (uid == device_unique_id_) {
-      ob_camera_node_.reset();
+      std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
-      device_.reset();
+      reset_device_flag_ = true;
-      device_connected_ = false;
+      reset_device_cond_.notify_all();
       current_device_disconnected = true;
-      device_unique_id_.clear();
       break;
     }
   }
@@ -175,6 +178,21 @@ void OBCameraNodeDriver::syncTime() {
   }
 }
 
+void OBCameraNodeDriver::resetDevice() {
+  while (is_alive_ && rclcpp::ok()) {
+    std::unique_lock<decltype(reset_device_mutex_)> lock(reset_device_mutex_);
+    reset_device_cond_.wait(lock,
+                            [this]() { return !is_alive_ || !rclcpp::ok() || reset_device_flag_; });
+    if (!is_alive_ || !rclcpp::ok()) {
+      break;
+    }
+    ob_camera_node_.reset();
+    device_.reset();
+    device_connected_ = false;
+    device_unique_id_.clear();
+    reset_device_flag_ = false;
+  }
+}
 void OBCameraNodeDriver::releaseDeviceSemaphore(sem_t *device_sem, int &num_devices_connected) {
   RCLCPP_INFO_THROTTLE(logger_, *get_clock(), 1000, "Release device semaphore");
   sem_post(device_sem);
@@ -316,15 +334,17 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
         std::string uid = device_info->uid();
         auto port_id = parseUsbPort(uid);
         if (port_id == usb_port) {
-          RCLCPP_INFO_STREAM(logger_, "Device port id " << port_id << " matched");
+          RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000,
+                                       "Device port id " << port_id << " matched");
           return dev;
         }
       } else {
         std::string uid = list->uid(i);
         auto port_id = parseUsbPort(uid);
-        RCLCPP_INFO_STREAM(logger_, "Device usb port: " << uid);
+        RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000, "Device usb port: " << uid);
         if (port_id == usb_port) {
-          RCLCPP_INFO_STREAM(logger_, "Device usb port <<" << uid << " matched");
+          RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000,
+                                       "Device usb port " << uid << " matched");
           return list->getDevice(i);
         }
       }