remove debug msg

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -94,6 +94,8 @@ class OBCameraNode {
   OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device);
   ~OBCameraNode();
 
+  void clean();
+
  private:
   void setupDevices();
 
@@ -124,8 +126,8 @@ class OBCameraNode {
 
   std::optional<OBCameraParam> findStreamDefaultCameraParam(const stream_index_pair& stream);
 
-  std::optional<OBCameraParam> findStreamCameraParam(const stream_index_pair& stream, uint32_t width,
+  std::optional<OBCameraParam> findStreamCameraParam(const stream_index_pair& stream,
-                                               uint32_t height);
+                                                     uint32_t width, uint32_t height);
 
   std::optional<OBCameraParam> findCameraParam(uint32_t color_width, uint32_t color_height,
                                                uint32_t depth_width, uint32_t depth_height);

orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h

@@ -36,6 +36,8 @@ class OBCameraNodeFactory : public rclcpp::Node {
 
   void deviceDisconnectCallback(const std::shared_ptr<ob::DeviceList>& device_list);
 
+  void printDeviceInfo(const std::shared_ptr<ob::DeviceInfo>& device_info);
+
  private:
   std::unique_ptr<ob::Context> ctx_;
   rclcpp::Logger logger_;

orbbec_camera/src/ob_camera_node.cpp

@@ -38,10 +38,17 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
   startPipeline();
 }
 
-OBCameraNode::~OBCameraNode() {
+OBCameraNode::~OBCameraNode() { clean(); }
+
+void OBCameraNode::clean() {
+  RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
+  is_running_.store(false);
   if (tf_thread_->joinable()) {
     tf_thread_->join();
   }
+  RCLCPP_WARN_STREAM(logger_, "stop pipeline");
+  pipeline_->stop();
+  RCLCPP_WARN_STREAM(logger_, "Destroy ~OBCameraNode DONE");
 }
 
 void OBCameraNode::setupDevices() {
@@ -181,6 +188,7 @@ void OBCameraNode::setupPublishers() {
 
 void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
 #if 0
+  // NOTE: This block code only for debug, it will be crazy slowly
   static int cnt = 0;
   const std::string home_dir = std::getenv("HOME");
   const std::string pc_file_name = home_dir + "/pc/point_cloud.ply";
@@ -195,11 +203,6 @@ void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
       point_cloud_filter_.setCreatePointFormat(OB_FORMAT_RGB_POINT);
       auto frame = point_cloud_filter_.process(frame_set);
       saveRGBPointsToPly(frame, pc_file_name);
-    } else if (frame_set->depthFrame() != nullptr) {
-      RCLCPP_INFO_STREAM(logger_, "has depth pc");
-      point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
-      auto frame = point_cloud_filter_.process(frame_set);
-      savePointsToPly(frame, pc_file_name);
     }
   }
 #endif
@@ -207,7 +210,6 @@ void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
     auto start = rclcpp::Clock().now();
     publishColorPointCloud(frame_set);
     auto end = rclcpp::Clock().now();
-    RCLCPP_INFO_STREAM(logger_, "process point cloud cost " << (end - start).seconds());
   }
 }
 
@@ -307,32 +309,19 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
   point_cloud_publisher_->publish(point_cloud_msg_);
 }
 void OBCameraNode::frameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
-  auto start = rclcpp::Clock().now();
-  rclcpp::Time t = rclcpp::Clock().now();
   auto color_frame = frame_set->colorFrame();
   auto depth_frame = frame_set->depthFrame();
   auto ir_frame = frame_set->irFrame();
   if (color_frame && enable_[COLOR]) {
-    auto color_start = rclcpp::Clock().now();
     publishColorFrame(color_frame);
-    RCLCPP_INFO_STREAM(
-        logger_, "publishColorFrame cost " << (rclcpp::Clock().now() - color_start).seconds());
   }
   if (depth_frame && enable_[DEPTH]) {
-    auto depth_start = rclcpp::Clock().now();
     publishDepthFrame(depth_frame);
-    RCLCPP_INFO_STREAM(
-        logger_, "publishDepthFrame cost " << (rclcpp::Clock().now() - depth_start).seconds());
   }
   if (ir_frame && enable_[IR0]) {
-    auto ir_start = rclcpp::Clock().now();
     publishIRFrame(ir_frame);
-    RCLCPP_INFO_STREAM(logger_,
-                       "publishIRFrame cost " << (rclcpp::Clock().now() - ir_start).seconds());
   }
   publishPointCloud(frame_set);
-  auto end = rclcpp::Clock().now();
-  RCLCPP_INFO_STREAM(logger_, "frameSetCallback cost " << (end - start).seconds());
 }
 std::optional<OBCameraParam> OBCameraNode::findDefaultCameraParam() {
   auto camera_params = device_->getCalibrationCameraParamList();

orbbec_camera/src/ob_camera_node_factory.cpp

@@ -21,8 +21,9 @@ OBCameraNodeFactory::~OBCameraNodeFactory() {
   }
 }
 void OBCameraNodeFactory::init() {
+  ctx_->setLoggerSeverity(OB_LOG_SEVERITY_NONE);
   is_alive_.store(true);
-  serial_number_ = declare_parameter<std::string>("serial_number", "BX4NC10000S");
+  serial_number_ = declare_parameter<std::string>("serial_number", "");
   query_thread_ = std::thread([=]() {
     std::chrono::milliseconds timespan(static_cast<int>(reconnect_timeout_ * 1e3));
     rclcpp::Time first_try_time = this->now();
@@ -61,6 +62,10 @@ void OBCameraNodeFactory::init() {
 
 void OBCameraNodeFactory::deviceConnectCallback(
     const std::shared_ptr<ob::DeviceList> &device_list) {
+  if (device_list->deviceCount() == 0) {
+    return;
+  }
+  RCLCPP_ERROR_STREAM(logger_, "deviceConnectCallback");
   CHECK_NOTNULL(device_list);
   if (!device_) {
     getDevice(device_list);
@@ -73,13 +78,38 @@ void OBCameraNodeFactory::deviceConnectCallback(
 
 void OBCameraNodeFactory::deviceDisconnectCallback(
     const std::shared_ptr<ob::DeviceList> &device_list) {
-  CHECK_NOTNULL(device_list);
+  if (device_list->deviceCount() == 0) {
-  auto dev = device_list->getDeviceBySN(serial_number_.c_str());
+    return;
-  if (dev) {
-    RCLCPP_ERROR_STREAM(logger_, "The device has been disconnected!");
-    ob_camera_node_.reset(nullptr);
-    device_.reset();
   }
+  RCLCPP_ERROR_STREAM(logger_, "deviceDisconnectCallback");
+  CHECK_NOTNULL(device_list);
+  ob_camera_node_.reset(nullptr);
+  device_.reset();
+//  try {
+//    for (size_t i = 0; i < device_list->deviceCount(); i++) {
+//      auto dev = device_list->getDevice(i);
+//      std::string sn1 = dev->getDeviceInfo()->serialNumber();
+//      std::string sn2 = device_->getDeviceInfo()->serialNumber();
+//      if (sn1 == sn2) {
+//        RCLCPP_ERROR(logger_, "The device with SN %s was disconnected!", sn1.c_str());
+//        ob_camera_node_.reset(nullptr);
+//        device_.reset();
+//      }
+//    }
+//  } catch (const ob::Error &e) {
+//    RCLCPP_ERROR_STREAM(logger_, e.getMessage());
+//  }
+}
+
+void OBCameraNodeFactory::printDeviceInfo(const std::shared_ptr<ob::DeviceInfo> &device_info) {
+  RCLCPP_INFO_STREAM(logger_, "name " << device_info->name());
+  RCLCPP_INFO_STREAM(logger_, "pid " << device_info->pid());
+  RCLCPP_INFO_STREAM(logger_, "vid " << device_info->vid());
+  RCLCPP_INFO_STREAM(logger_, "serial_name " << device_info->serialNumber());
+  RCLCPP_INFO_STREAM(logger_, "firmware version " << device_info->firmwareVersion());
+  RCLCPP_INFO_STREAM(logger_,
+                     "supported min sdk version " << device_info->supportedMinSdkVersion());
+  RCLCPP_INFO_STREAM(logger_, "hardware version " << device_info->hardwareVersion());
 }
 
 void OBCameraNodeFactory::getDevice(const std::shared_ptr<ob::DeviceList> &list) {
@@ -90,19 +120,20 @@ void OBCameraNodeFactory::getDevice(const std::shared_ptr<ob::DeviceList> &list)
     RCLCPP_WARN_STREAM(logger_, "No orbbec devices were found!");
     return;
   }
-  for (size_t i = 0; i < list->deviceCount(); i++) {
+  if (serial_number_.empty()) {
-    auto dev = list->getDevice(i);
+    for (size_t i = 0; i < list->deviceCount(); i++) {
-    if (dev != nullptr) {
+      auto dev = list->getDevice(i);
-      device_ = dev;
+      if (dev != nullptr) {
-      RCLCPP_INFO_STREAM(logger_, "device name " << dev->getDeviceInfo()->name());
+        device_ = dev;
-      RCLCPP_INFO_STREAM(logger_, "device pid " << dev->getDeviceInfo()->pid());
+        printDeviceInfo(device_->getDeviceInfo());
-      RCLCPP_INFO_STREAM(logger_, "device vid " << dev->getDeviceInfo()->vid());
+      }
-      RCLCPP_INFO_STREAM(logger_, "device serial_name " << dev->getDeviceInfo()->serialNumber());
+    }
-      RCLCPP_INFO_STREAM(logger_,
+  } else {
-                         "device firmware version " << dev->getDeviceInfo()->firmwareVersion());
+    device_ = list->getDeviceBySN(serial_number_.c_str());
-      RCLCPP_INFO_STREAM(logger_, "device supported min sdk version "
+    if (device_ == nullptr) {
-                                      << dev->getDeviceInfo()->supportedMinSdkVersion());
+      RCLCPP_ERROR_STREAM(logger_, "can not found device with SN " << serial_number_);
-      RCLCPP_INFO_STREAM(logger_, "device hardware version " << dev->getDeviceInfo()->hardwareVersion());
+    } else {
+      printDeviceInfo(device_->getDeviceInfo());
     }
   }
 }