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

Refactor camera info setup and usage

Browse Source
This commit is contained in:
Joe Dong
2024-11-07 11:43:40 +08:00
parent 392f62f0ee
commit 5340249a93
3 changed files with 40 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
@@ -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);
orbbec_camera/launch/gemini_330_series.launch.py
+2
View File
@@ -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'),
orbbec_camera/src/ob_camera_node.cpp
+36 -5
View File
@@ -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);