Refactor camera info setup and usage

orbbec · Nov 7, 2024 · 5340249 · 5340249
1 parent 392f62f
commit 5340249
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 5 deletions.
diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
@@ -193,6 +193,8 @@ class OBCameraNode {
 
   void setupPublishers();
 
+  void setupCameraInfo();
+
   void publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans, const tf2::Quaternion& q,
                        const std::string& from, const std::string& to);
 

diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py
@@ -80,6 +80,8 @@ def generate_launch_description():
         DeclareLaunchArgument('color_contrast', default_value='-1'),
         DeclareLaunchArgument('color_gamma', default_value='-1'),
         DeclareLaunchArgument('color_hue', default_value='-1'),
+        DeclareLaunchArgument('color_info_url', default_value=''),
+        DeclareLaunchArgument('ir_info_url', default_value=''),
         DeclareLaunchArgument('color_ae_roi_left', default_value='-1'),
         DeclareLaunchArgument('color_ae_roi_top', default_value='-1'),
         DeclareLaunchArgument('color_ae_roi_right', default_value='-1'),

diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp
@@ -1202,6 +1202,7 @@ void OBCameraNode::setupTopics() {
     setupDevices();
     setupDepthPostProcessFilter();
     setupProfiles();
+    setupCameraInfo();
     selectBaseStream();
     setupCameraCtrlServices();
     setupPublishers();
@@ -1285,6 +1286,19 @@ void OBCameraNode::setupPipelineConfig() {
   }
 }
 
+void OBCameraNode::setupCameraInfo() {
+  std::string color_camera_name = camera_name_ + "_color";
+  if (!color_info_url_.empty()) {
+    color_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(
+        node_, color_camera_name, color_info_url_);
+  }
+  std::string ir_camera_name = camera_name_ + "_ir";
+  if (!ir_info_url_.empty()) {
+    ir_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(
+        node_, ir_camera_name, ir_info_url_);
+  }
+}
+
 void OBCameraNode::setupPublishers() {
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
   using CameraInfo = sensor_msgs::msg::CameraInfo;
@@ -2022,11 +2036,28 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   if (stream_index == COLOR && enable_color_undistortion_) {
     memset(&distortion, 0, sizeof(distortion));
   }
-  auto camera_info = convertToCameraInfo(intrinsic, distortion, width);
-  camera_info.header.stamp = timestamp;
-  camera_info.header.frame_id = frame_id;
-  camera_info.width = width;
-  camera_info.height = height;
+  sensor_msgs::msg::CameraInfo camera_info{};
+
+  if (color_info_manager_ && color_info_manager_->isCalibrated() && stream_index == COLOR) {
+    camera_info = color_info_manager_->getCameraInfo();
+    camera_info.header.stamp = timestamp;
+    camera_info.header.frame_id = frame_id;
+    camera_info.width = width;
+    camera_info.height = height;
+  } else if (ir_info_manager_ && ir_info_manager_->isCalibrated() &&
+             (stream_index == INFRA1 || stream_index == INFRA2 || stream_index == DEPTH)) {
+    camera_info = ir_info_manager_->getCameraInfo();
+    camera_info.header.stamp = timestamp;
+    camera_info.header.frame_id = frame_id;
+    camera_info.width = width;
+    camera_info.height = height;
+  } else {
+    camera_info = convertToCameraInfo(intrinsic, distortion, width);
+    camera_info.header.stamp = timestamp;
+    camera_info.header.frame_id = frame_id;
+    camera_info.width = width;
+    camera_info.height = height;
+  }
   if (frame->type() == OB_FRAME_IR_RIGHT && enable_stream_[INFRA1]) {
     auto stream_profile = frame->getStreamProfile();
     CHECK_NOTNULL(stream_profile);