i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-12 23:39:50 +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/include/orbbec_camera/ob_camera_node.h
+2
View File
@@ -238,6 +238,8 @@ class OBCameraNode {
void setupPublishers(); void setupPublishers();
void setupCameraInfo();
void publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans, const tf2::Quaternion& q, void publishStaticTF(const rclcpp::Time& t, const tf2::Vector3& trans, const tf2::Quaternion& q,
const std::string& from, const std::string& to); const std::string& from, const std::string& to);
orbbec_camera/src/ob_camera_node.cpp
+35 -5
View File
@@ -1966,6 +1966,7 @@ void OBCameraNode::setupTopics() {
setupRightIrPostProcessFilter(); setupRightIrPostProcessFilter();
setupLeftIrPostProcessFilter(); setupLeftIrPostProcessFilter();
setupProfiles(); setupProfiles();
setupCameraInfo();
selectBaseStream(); selectBaseStream();
setupCameraCtrlServices(); setupCameraCtrlServices();
setupPublishers(); 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() { void OBCameraNode::setupPublishers() {
using PointCloud2 = sensor_msgs::msg::PointCloud2; using PointCloud2 = sensor_msgs::msg::PointCloud2;
using CameraInfo = sensor_msgs::msg::CameraInfo; 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_) { if (stream_index == COLOR && enable_color_undistortion_) {
memset(&distortion, 0, sizeof(distortion)); memset(&distortion, 0, sizeof(distortion));
} }
auto camera_info = convertToCameraInfo(intrinsic, distortion, width); sensor_msgs::msg::CameraInfo camera_info{};
camera_info.header.stamp = timestamp; if (color_info_manager_ && color_info_manager_->isCalibrated() && stream_index == COLOR) {
camera_info.header.frame_id = frame_id; camera_info = color_info_manager_->getCameraInfo();
camera_info.width = width; camera_info.header.stamp = timestamp;
camera_info.height = height; 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]) { if (frame->getType() == OB_FRAME_IR_RIGHT && enable_stream_[INFRA1]) {
auto stream_profile = frame->getStreamProfile(); auto stream_profile = frame->getStreamProfile();
CHECK_NOTNULL(stream_profile); CHECK_NOTNULL(stream_profile);