fixed get camera params

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -143,6 +143,10 @@ class OBCameraNode {
 
   std::optional<OBCameraParam> findDefaultCameraParam();
 
+  std::optional<OBCameraParam> getDepthCameraParam();
+
+  std::optional<OBCameraParam> getColorCameraParam();
+
   void getExposureCallback(const std::shared_ptr<GetInt32::Request>& request,
                            std::shared_ptr<GetInt32::Response>& response,
                            const stream_index_pair& stream_index);

orbbec_camera/include/orbbec_camera/ob_camera_node_factory.h

@@ -52,6 +52,7 @@ class OBCameraNodeFactory : public rclcpp::Node {
   std::atomic_bool is_alive_{false};
   std::atomic_bool device_connected_{false};
   std::string serial_number_;
+  std::string device_unique_id_;
   std::shared_ptr<Parameters> parameters_ = nullptr;
   std::shared_ptr<std::thread> query_thread_ = nullptr;
   std::recursive_mutex device_lock_;

orbbec_camera/src/ob_camera_node.cpp

@@ -326,8 +326,10 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& f
       point_cloud_publisher_->get_subscription_count() == 0) {
     return;
   }
-  if (!camera_param_) {
+  if (!camera_param_ && depth_registration_) {
     camera_param_ = pipeline_->getCameraParam();
+  } else if (!camera_param_) {
+    camera_param_ = getDepthCameraParam();
   }
   point_cloud_filter_.setCameraParam(*camera_param_);
   point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
@@ -494,8 +496,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
   }
   image.data = (uchar*)video_frame->data();
   auto timestamp = frameTimeStampToROSTime(video_frame->systemTimeStamp());
-  if (!camera_param_) {
+  if (!camera_param_ && depth_registration_) {
     camera_param_ = pipeline_->getCameraParam();
+  } else if (!camera_param_ && stream_index == COLOR) {
+    camera_param_ = getColorCameraParam();
+  } else if (!camera_param_) {
+    camera_param_ = getDepthCameraParam();
   }
   auto& intrinsic =
       stream_index == COLOR ? camera_param_->rgbIntrinsic : camera_param_->depthIntrinsic;
@@ -531,6 +537,32 @@ std::optional<OBCameraParam> OBCameraNode::findDefaultCameraParam() {
   return {};
 }
 
+std::optional<OBCameraParam> OBCameraNode::getDepthCameraParam() {
+  auto camera_params = device_->getCalibrationCameraParamList();
+  for (size_t i = 0; i < camera_params->count(); i++) {
+    auto param = camera_params->getCameraParam(i);
+    int depth_w = param.depthIntrinsic.width;
+    int depth_h = param.depthIntrinsic.height;
+    if (depth_w * height_[DEPTH] == depth_h * width_[DEPTH]) {
+      return param;
+    }
+  }
+  return {};
+}
+
+std::optional<OBCameraParam> OBCameraNode::getColorCameraParam() {
+  auto camera_params = device_->getCalibrationCameraParamList();
+  for (size_t i = 0; i < camera_params->count(); i++) {
+    auto param = camera_params->getCameraParam(i);
+    int color_w = param.rgbIntrinsic.width;
+    int color_h = param.rgbIntrinsic.height;
+    if (color_w * height_[COLOR] == color_h * width_[COLOR]) {
+      return param;
+    }
+  }
+  return {};
+}
+
 void OBCameraNode::publishStaticTF(const rclcpp::Time& t, const std::vector<float>& trans,
                                    const tf2::Quaternion& q, const std::string& from,
                                    const std::string& to) {

orbbec_camera/src/ob_camera_node_factory.cpp

@@ -88,13 +88,14 @@ void OBCameraNodeFactory::onDeviceDisconnected(const std::shared_ptr<ob::DeviceL
   }
   RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected");
   for (size_t i = 0; i < device_list->deviceCount(); i++) {
-    std::string serial_number = device_list->serialNumber(i);
+    std::string uid = device_list->uid(i);
     std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
-    RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected: " << serial_number);
+    RCLCPP_INFO_STREAM(logger_, "device with" << uid << "disconnected");
-    if (device_info_ && device_info_->serialNumber() == serial_number) {
+    if (uid == device_unique_id_) {
       ob_camera_node_.reset();
       device_.reset();
       device_connected_ = false;
+      device_unique_id_.clear();
       break;
     }
   }
@@ -214,7 +215,7 @@ void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &lis
     }
 
     if (device_ == nullptr) {
-     // RCLCPP_WARN(logger_, "Device with serial number %s not found", serial_number_.c_str());
+      // RCLCPP_WARN(logger_, "Device with serial number %s not found", serial_number_.c_str());
       device_connected_ = false;
       return;
     } else {
@@ -251,6 +252,7 @@ void OBCameraNodeFactory::startDevice(const std::shared_ptr<ob::DeviceList> &lis
   device_connected_ = true;
   device_info_ = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info_.get());
+  device_unique_id_ = device_info_->uid();
   RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->name() << " connected");
   RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->serialNumber());
   RCLCPP_INFO_STREAM(logger_, "Firmware version: " << device_info_->firmwareVersion());