i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-10 22:49:51 +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-05-07 11:44:05 +08:00
parent 9b69bbfa57
commit fc7e255141
2 changed files with 51 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+44 -4
View File
@@ -3060,13 +3060,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];
@@ -4034,8 +4045,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;
@@ -4151,9 +4162,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 =
@@ -4256,6 +4269,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 (image.empty() || image.cols != width || image.rows != height) {
image.create(height, width, image_format_[stream_index]);
@@ -4340,6 +4357,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) {