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

feat: add support for publishing raw color MJPG images

Browse Source
This commit is contained in:
slz
2026-03-23 18:12:11 +08:00
parent 6344b2cfa0
commit a041062663
2 changed files with 51 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+7
View File
@@ -39,6 +39,7 @@
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <camera_info_manager/camera_info_manager.hpp>
#include <image_publisher/image_publisher.hpp>
@@ -482,6 +483,10 @@ class OBCameraNode {
bool setupFormatConvertType(OBFormat format);
bool setupFormatConvertType(OBFormat format, ob::FormatConvertFilter& filter);
bool hasCompressedImageSubscriber(const stream_index_pair& stream_index) const;
void publishCompressedColorImage(const std::shared_ptr<ob::Frame>& frame,
const stream_index_pair& stream_index,
const rclcpp::Time& timestamp, const std::string& frame_id);
orbbec_camera_msgs::msg::IMUInfo createIMUInfo(const stream_index_pair& stream_index);
@@ -570,6 +575,8 @@ class OBCameraNode {
std::map<stream_index_pair, int> rotation_stream_;
std::map<stream_index_pair, std::string> stream_name_;
std::map<stream_index_pair, std::shared_ptr<image_publisher>> image_publishers_;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr>
compressed_image_publishers_;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
camera_info_publishers_;
std::map<stream_index_pair, bool> frame_info_logged_;
orbbec_camera/src/ob_camera_node.cpp
+44 -4
View File
@@ -2566,13 +2566,24 @@ void OBCameraNode::setupPublishers() {
if (use_intra_process_) {
image_qos_profile = rmw_qos_profile_default;
}
if (use_intra_process_) {
const bool is_mjpg_color_stream =
(stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
format_[stream_index] == OB_FORMAT_MJPG;
if (use_intra_process_ || is_mjpg_color_stream) {
image_publishers_[stream_index] =
std::make_shared<image_rcl_publisher>(*node_, topic, image_qos_profile);
} else {
image_publishers_[stream_index] =
std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
}
if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
format_[stream_index] == OB_FORMAT_MJPG) {
compressed_image_publishers_[stream_index] =
node_->create_publisher<sensor_msgs::msg::CompressedImage>(
topic + "/compressed",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile),
image_qos_profile));
}
topic = name + "/camera_info";
auto camera_info_qos = camera_info_qos_[stream_index];
@@ -3506,8 +3517,8 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
}
bool has_subscriber = false;
if (image_publishers_.count(stream_index) && image_publishers_[stream_index]) {
has_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0;
if (image_publishers_.count(stream_index) && image_publishers_.at(stream_index)) {
has_subscriber = image_publishers_.at(stream_index)->get_subscription_count() > 0;
}
if (stream_index == COLOR && enable_color_undistortion_ && color_undistortion_publisher_) {
has_subscriber = true;
@@ -3623,9 +3634,11 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
CHECK_NOTNULL(image_publishers_[stream_index]);
const bool has_raw_image_subscriber =
image_publishers_[stream_index]->get_subscription_count() > 0;
const bool has_compressed_image_subscriber = hasCompressedImageSubscriber(stream_index);
const bool enable_undistortion_publish =
(stream_index == COLOR && enable_color_undistortion_ && color_undistortion_publisher_);
bool has_subscriber = has_raw_image_subscriber || enable_undistortion_publish;
bool has_subscriber =
has_raw_image_subscriber || has_compressed_image_subscriber || enable_undistortion_publish;
has_subscriber =
has_subscriber || camera_info_publishers_[stream_index]->get_subscription_count() > 0;
has_subscriber =
@@ -3728,6 +3741,10 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
camera_info.p.at(3) = -fx * ex.trans[0] / 1000.0 + 0.0;
camera_info.p.at(7) = -fy * ex.trans[1] / 1000.0 + 0.0;
}
if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
frame->getFormat() == OB_FORMAT_MJPG && has_compressed_image_subscriber) {
publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
}
CHECK_NOTNULL(image_publishers_[stream_index]);
if (!has_raw_image_subscriber && !enable_undistortion_publish) {
return;
@@ -3810,6 +3827,29 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
}
}
bool OBCameraNode::hasCompressedImageSubscriber(const stream_index_pair &stream_index) const {
auto it = compressed_image_publishers_.find(stream_index);
return it != compressed_image_publishers_.end() && it->second &&
it->second->get_subscription_count() > 0;
}
void OBCameraNode::publishCompressedColorImage(const std::shared_ptr<ob::Frame> &frame,
const stream_index_pair &stream_index,
const rclcpp::Time &timestamp,
const std::string &frame_id) {
auto it = compressed_image_publishers_.find(stream_index);
if (it == compressed_image_publishers_.end() || !it->second) {
return;
}
sensor_msgs::msg::CompressedImage msg;
msg.header.stamp = timestamp;
msg.header.frame_id = frame_id;
msg.format = "jpeg";
const auto *data = static_cast<const uint8_t *>(frame->getData());
msg.data.assign(data, data + frame->getDataSize());
it->second->publish(std::move(msg));
}
void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &frame,
const stream_index_pair &stream_index,
const std_msgs::msg::Header &header) {