i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 03:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add setupCameraInfo method to initialize camera info managers

Browse Source
This commit is contained in:
xiexun
2025-09-02 17:06:59 +08:00
parent 4c932d47a8
commit 8e8fb156f9
2 changed files with 37 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+35 -5
View File
@@ -1966,6 +1966,7 @@ void OBCameraNode::setupTopics() {
setupRightIrPostProcessFilter();
setupLeftIrPostProcessFilter();
setupProfiles();
setupCameraInfo();
selectBaseStream();
setupCameraCtrlServices();
setupPublishers();
@@ -2129,6 +2130,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;
@@ -3010,11 +3024,27 @@ 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->getType() == OB_FRAME_IR_RIGHT && enable_stream_[INFRA1]) {
auto stream_profile = frame->getStreamProfile();
CHECK_NOTNULL(stream_profile);