i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-12 11:10:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: update publish raw image

Browse Source
This commit is contained in:
slz
2026-05-07 16:26:34 +08:00
parent fc7e255141
commit d2d894741d
2 changed files with 43 additions and 33 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
@@ -460,6 +460,8 @@ class OBCameraNode {
std::shared_ptr<ob::Frame> softwareDecodeColorFrame(const std::shared_ptr<ob::Frame>& frame,
const stream_index_pair& stream_index);
bool isColorFrameDecodeRequired(const std::shared_ptr<ob::Frame>& frame) const;
bool decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame>& frame, uint8_t* buffer);
std::shared_ptr<ob::Frame> decodeIRMJPGFrame(const std::shared_ptr<ob::Frame>& frame);
orbbec_camera/src/ob_camera_node.cpp
+41 -33
View File
@@ -4019,6 +4019,19 @@ std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
return color_frame;
}
bool OBCameraNode::isColorFrameDecodeRequired(const std::shared_ptr<ob::Frame> &frame) const {
if (frame == nullptr) {
return false;
}
const auto format = frame->getFormat();
if (format == OB_FORMAT_RGB || format == OB_FORMAT_BGR || format == OB_FORMAT_RGB888 ||
format == OB_FORMAT_RGBA || format == OB_FORMAT_BGRA || format == OB_FORMAT_Y16 ||
format == OB_FORMAT_Y8) {
return false;
}
return true;
}
bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &frame,
uint8_t *buffer) {
if (frame == nullptr) {
@@ -4028,6 +4041,10 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
return false;
}
if (!isColorFrameDecodeRequired(frame)) {
return true;
}
stream_index_pair stream_index = COLOR;
switch (frame->getType()) {
case OB_FRAME_COLOR:
@@ -4058,15 +4075,6 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
has_subscriber = true;
}
if (metadata_publishers_.count(stream_index) && metadata_publishers_[stream_index] &&
metadata_publishers_[stream_index]->get_subscription_count() > 0) {
has_subscriber = true;
}
if (camera_info_publishers_.count(stream_index) && camera_info_publishers_[stream_index] &&
camera_info_publishers_[stream_index]->get_subscription_count() > 0) {
has_subscriber = true;
}
if (!has_subscriber) {
return false;
}
@@ -4277,30 +4285,27 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (image.empty() || image.cols != width || image.rows != height) {
image.create(height, width, image_format_[stream_index]);
}
if (frame->getType() == OB_FRAME_COLOR && !is_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "color frame is not decoded");
return;
}
if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "left color frame is not decoded");
return;
}
if (frame->getType() == OB_FRAME_COLOR_RIGHT && !is_right_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "right color frame is not decoded");
return;
}
if (frame->getType() == OB_FRAME_COLOR && frame->format() != OB_FORMAT_Y8 &&
frame->format() != OB_FORMAT_Y16 && frame->format() != OB_FORMAT_BGRA &&
frame->format() != OB_FORMAT_RGBA && has_subscriber) {
memcpy(image.data, rgb_buffer_, video_frame->getWidth() * video_frame->getHeight() * 3);
} else if (frame->getType() == OB_FRAME_COLOR_LEFT && frame->format() != OB_FORMAT_Y8 &&
frame->format() != OB_FORMAT_Y16 && frame->format() != OB_FORMAT_BGRA &&
frame->format() != OB_FORMAT_RGBA && has_subscriber) {
memcpy(image.data, rgb_buffer_left_, video_frame->getWidth() * video_frame->getHeight() * 3);
} else if (frame->getType() == OB_FRAME_COLOR_RIGHT && frame->format() != OB_FORMAT_Y8 &&
frame->format() != OB_FORMAT_Y16 && frame->format() != OB_FORMAT_BGRA &&
frame->format() != OB_FORMAT_RGBA && has_subscriber) {
memcpy(image.data, rgb_buffer_right_, video_frame->getWidth() * video_frame->getHeight() * 3);
if (isColorFrameDecodeRequired(frame) &&
(has_raw_image_subscriber || enable_undistortion_publish)) {
if (frame->getType() == OB_FRAME_COLOR && !is_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "color frame is not decoded");
return;
}
if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "left color frame is not decoded");
return;
}
if (frame->getType() == OB_FRAME_COLOR_RIGHT && !is_right_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "right color frame is not decoded");
return;
}
if (frame->getType() == OB_FRAME_COLOR) {
memcpy(image.data, rgb_buffer_, video_frame->getWidth() * video_frame->getHeight() * 3);
} else if (frame->getType() == OB_FRAME_COLOR_LEFT) {
memcpy(image.data, rgb_buffer_left_, video_frame->getWidth() * video_frame->getHeight() * 3);
} else if (frame->getType() == OB_FRAME_COLOR_RIGHT) {
memcpy(image.data, rgb_buffer_right_, video_frame->getWidth() * video_frame->getHeight() * 3);
}
} else {
memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
}
@@ -4327,6 +4332,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
camera_info_publishers_[stream_index]->publish(camera_info);
publishMetadata(frame, stream_index, camera_info.header);
if (!has_raw_image_subscriber && !save_images_[stream_index]) {
return;
}
if (stream_index == DEPTH) {
auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
image = image * depth_scale;