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

mega_h26x_decode tool node adds multi-camera decoding

Browse Source
This commit is contained in:
jj
2024-11-11 09:34:21 +08:00
parent 405b815079
commit 6a364f84d7
3 changed files with 123 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/config/tools/megah26xdecode/mega_h26x_decode_params.json
+12
View File
@@ -0,0 +1,12 @@
{
"h26x_decode_params": {
"encode_topics": [
"/camera_01/color/h26x_encoded_data",
"/camera_02/color/h26x_encoded_data"
],
"decode_topics": [
"/camera_01/color/h26x/image_raw",
"/camera_02/color/h26x/image_raw"
]
}
}
orbbec_camera/src/ob_camera_node.cpp
+3 -4
View File
@@ -1263,14 +1263,13 @@ void OBCameraNode::setupPublishers() {
camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>( camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>(
topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile)); camera_info_qos_profile));
topic = name + "/h26x_encoded_data";
auto image_h264_qos_profile = getRMWQosProfileFromString(image_qos); auto image_h264_qos_profile = getRMWQosProfileFromString(image_qos);
if (format_str_[stream_index] == "H264" || format_str_[stream_index] == "H265") { if (format_str_[stream_index] == "H264" || format_str_[stream_index] == "H265") {
camera_h26x_publishers_[stream_index] = camera_h26x_publishers_[stream_index] =
node_->create_publisher<sensor_msgs::msg::CompressedImage>( node_->create_publisher<sensor_msgs::msg::CompressedImage>(
"/camera/color/h26x_encoded_data", topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_h264_qos_profile),
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_h264_qos_profile), image_h264_qos_profile));
image_h264_qos_profile));
} }
if (isGemini335PID(pid)) { if (isGemini335PID(pid)) {
metadata_publishers_[stream_index] = metadata_publishers_[stream_index] =
orbbec_camera/tools/mega_h26x_decode_node.cpp
+108 -54
View File
@@ -4,6 +4,7 @@ extern "C" {
#include <libswscale/swscale.h> #include <libswscale/swscale.h>
#include <libavutil/imgutils.h> #include <libavutil/imgutils.h>
#include <libavutil/time.h> #include <libavutil/time.h>
#include <libavutil/log.h>
} }
#include <memory> #include <memory>
@@ -18,128 +19,181 @@ extern "C" {
#include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp"
#include "libobsensor/hpp/Utils.hpp" #include "libobsensor/hpp/Utils.hpp"
class H264DecoderNode : public rclcpp::Node { class H264DecoderNode : public rclcpp::Node {
public: public:
H264DecoderNode() : Node("h264_decoder_node") { H264DecoderNode() : Node("h264_decoder_node") {
av_log_set_level(AV_LOG_QUIET);
avformat_network_init(); avformat_network_init();
params_init();
rclcpp::QoS qos_settings(30); rclcpp::QoS qos_settings(30);
qos_settings.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); qos_settings.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
qos_settings.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); qos_settings.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
qos_settings.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST); qos_settings.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST);
compressed_image_subscriber_ = this->create_subscription<sensor_msgs::msg::CompressedImage>( // compressed_image_subscriber_ = this->create_subscription<sensor_msgs::msg::CompressedImage>(
"/camera/color/h26x_encoded_data", qos_settings, // "/camera/color/h26x_encoded_data", qos_settings,
std::bind(&H264DecoderNode::compressedImageCallback, this, std::placeholders::_1)); // std::bind(&H264DecoderNode::compressedImageCallback, this, std::placeholders::_1));
rgb_image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>( // rgb_image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>(
"/camera/color/h26x_decoder/image_raw", qos_settings); // "/camera/color/h26x_decoder/image_raw", qos_settings);
for (size_t i = 0; i < encode_topics_.size(); ++i) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("mega_h26x_decode"),
"encode_topics_: " << encode_topics_[i]);
RCLCPP_INFO_STREAM(rclcpp::get_logger("mega_h26x_decode"),
"decode_topics_: " << decode_topics_[i]);
auto encode_sub = this->create_subscription<sensor_msgs::msg::CompressedImage>(
encode_topics_[i], qos_settings,
[this, i](sensor_msgs::msg::CompressedImage::SharedPtr msg) {
this->compressedImageCallback(msg, i);
});
auto decode_pub =
this->create_publisher<sensor_msgs::msg::Image>(decode_topics_[i], qos_settings);
encode_subscribers_.push_back(encode_sub);
decode_publishers_.push_back(decode_pub);
}
codec_.resize(encode_topics_.size());
codec_context_.resize(encode_topics_.size());
frame_.resize(encode_topics_.size());
packet_.resize(encode_topics_.size());
codec_init_.resize(encode_topics_.size());
send_ret_.resize(encode_topics_.size());
receive_ret_.resize(encode_topics_.size());
} }
private: private:
void decode_init(const sensor_msgs::msg::CompressedImage::SharedPtr msg) { std::mutex buffer_mutex_;
void params_init() {
std::ifstream file(
"install/orbbec_camera/share/orbbec_camera/config/tools/megah26xdecode/"
"mega_h26x_decode_params.json");
if (!file.is_open()) {
RCLCPP_ERROR(this->get_logger(), "Failed to open JSON file.");
return;
}
nlohmann::json json_data;
file >> json_data;
encode_topics_ =
json_data["h26x_decode_params"]["encode_topics"].get<std::vector<std::string>>();
decode_topics_ =
json_data["h26x_decode_params"]["decode_topics"].get<std::vector<std::string>>();
}
void decode_init(const sensor_msgs::msg::CompressedImage::SharedPtr msg, size_t index) {
if (msg->format == "h264") { if (msg->format == "h264") {
codec_ = std::shared_ptr<const AVCodec>(avcodec_find_decoder(AV_CODEC_ID_H264), codec_[index] = std::shared_ptr<const AVCodec>(avcodec_find_decoder(AV_CODEC_ID_H264),
[](const AVCodec*) {}); [](const AVCodec*) {});
codec_context_ = std::shared_ptr<AVCodecContext>(avcodec_alloc_context3(codec_.get()), codec_context_[index] = std::shared_ptr<AVCodecContext>(avcodec_alloc_context3(codec_[index].get()),
[](AVCodecContext* ctx) { [](AVCodecContext* ctx) {
if (ctx) { if (ctx) {
avcodec_free_context(&ctx); avcodec_free_context(&ctx);
} }
}); });
if (avcodec_open2(codec_context_.get(), codec_.get(), nullptr) < 0) { if (avcodec_open2(codec_context_[index].get(), codec_[index].get(), nullptr) < 0) {
RCLCPP_ERROR(this->get_logger(), "Failed to open codec"); RCLCPP_ERROR(this->get_logger(), "Failed to open codec");
return; return;
} }
frame_ = std::shared_ptr<AVFrame>(av_frame_alloc(), [](AVFrame* f) { frame_[index] = std::shared_ptr<AVFrame>(av_frame_alloc(), [](AVFrame* f) {
if (f) av_frame_free(&f); if (f) av_frame_free(&f);
}); });
packet_ = std::shared_ptr<AVPacket>(av_packet_alloc(), [](AVPacket* p) { packet_[index] = std::shared_ptr<AVPacket>(av_packet_alloc(), [](AVPacket* p) {
if (p) av_packet_free(&p); if (p) av_packet_free(&p);
}); });
codec_init_ = 0;
} else if (msg->format == "h265") { } else if (msg->format == "h265") {
codec_ = std::shared_ptr<const AVCodec>(avcodec_find_decoder(AV_CODEC_ID_HEVC), codec_[index] = std::shared_ptr<const AVCodec>(avcodec_find_decoder(AV_CODEC_ID_HEVC),
[](const AVCodec*) {}); [](const AVCodec*) {});
codec_context_ = std::shared_ptr<AVCodecContext>(avcodec_alloc_context3(codec_.get()), codec_context_[index] = std::shared_ptr<AVCodecContext>(avcodec_alloc_context3(codec_[index].get()),
[](AVCodecContext* ctx) { [](AVCodecContext* ctx) {
if (ctx) { if (ctx) {
avcodec_free_context(&ctx); avcodec_free_context(&ctx);
} }
}); });
if (avcodec_open2(codec_context_.get(), codec_.get(), nullptr) < 0) { if (avcodec_open2(codec_context_[index].get(), codec_[index].get(), nullptr) < 0) {
RCLCPP_ERROR(this->get_logger(), "Failed to open codec"); RCLCPP_ERROR(this->get_logger(), "Failed to open codec");
return; return;
} }
frame_ = std::shared_ptr<AVFrame>(av_frame_alloc(), [](AVFrame* f) { frame_[index] = std::shared_ptr<AVFrame>(av_frame_alloc(), [](AVFrame* f) {
if (f) av_frame_free(&f); if (f) av_frame_free(&f);
}); });
packet_ = std::shared_ptr<AVPacket>(av_packet_alloc(), [](AVPacket* p) { packet_[index] = std::shared_ptr<AVPacket>(av_packet_alloc(), [](AVPacket* p) {
if (p) av_packet_free(&p); if (p) av_packet_free(&p);
}); });
codec_init_ = 0;
} }
} }
void decode_frame(const sensor_msgs::msg::CompressedImage::SharedPtr msg) { void decode_frame(const sensor_msgs::msg::CompressedImage::SharedPtr msg, size_t index) {
av_packet_unref(packet_.get()); av_packet_unref(packet_[index].get());
packet_->data = const_cast<uint8_t*>(msg->data.data()); packet_[index]->data = const_cast<uint8_t*>(msg->data.data());
packet_->size = msg->data.size(); packet_[index]->size = msg->data.size();
std::stringstream ss; std::stringstream ss;
const size_t bytes_to_print = std::min<size_t>(msg->data.size(), 32); const size_t bytes_to_print = std::min<size_t>(msg->data.size(), 32);
for (size_t i = 0; i < bytes_to_print; ++i) { for (size_t i = 0; i < bytes_to_print; ++i) {
ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(msg->data[i]) << " "; ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(msg->data[i]) << " ";
} }
RCLCPP_INFO(this->get_logger(), "Data (hex): %s", ss.str().c_str()); // RCLCPP_INFO(this->get_logger(), "Data (hex): %s", ss.str().c_str());
send_ret_ = avcodec_send_packet(codec_context_.get(), packet_.get()); send_ret_[index]= avcodec_send_packet(codec_context_[index].get(), packet_[index].get());
if (send_ret_ >= 0) { if (send_ret_[index] >= 0) {
receive_ret_ = avcodec_receive_frame(codec_context_.get(), frame_.get()); receive_ret_[index] = avcodec_receive_frame(codec_context_[index].get(), frame_[index].get());
if (receive_ret_ >= 0) { if (receive_ret_[index] >= 0) {
cv::Mat rgb_image(frame_->height, frame_->width, CV_8UC3); cv::Mat rgb_image(frame_[index]->height, frame_[index]->width, CV_8UC3);
SwsContext* sws_context = sws_getContext(frame_->width, frame_->height, SwsContext* sws_context = sws_getContext(frame_[index]->width, frame_[index]->height,
static_cast<AVPixelFormat>(frame_->format), static_cast<AVPixelFormat>(frame_[index]->format),
frame_->width, frame_->height, AV_PIX_FMT_BGR24, frame_[index]->width, frame_[index]->height, AV_PIX_FMT_BGR24,
SWS_BILINEAR, nullptr, nullptr, nullptr); SWS_BILINEAR, nullptr, nullptr, nullptr);
uint8_t* dest[1] = {rgb_image.data}; uint8_t* dest[1] = {rgb_image.data};
int linesize[1] = {static_cast<int>(rgb_image.step1())}; int linesize[1] = {static_cast<int>(rgb_image.step1())};
sws_scale(sws_context, frame_->data, frame_->linesize, 0, frame_->height, dest, linesize); sws_scale(sws_context, frame_[index]->data, frame_[index]->linesize, 0, frame_[index]->height, dest, linesize);
sws_freeContext(sws_context); sws_freeContext(sws_context);
auto rgb_image_msg = auto rgb_image_msg =
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb_image).toImageMsg(); cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb_image).toImageMsg();
rgb_image_msg->header.stamp = this->now(); rgb_image_msg->header.stamp = this->now();
rgb_image_publisher_->publish(*rgb_image_msg); decode_publishers_[index]->publish(*rgb_image_msg);
} }
} }
} }
void compressedImageCallback(const sensor_msgs::msg::CompressedImage::SharedPtr msg) { void compressedImageCallback(const sensor_msgs::msg::CompressedImage::SharedPtr msg,
RCLCPP_INFO(this->get_logger(), "Format: %s", msg->format.c_str()); size_t index) {
if (codec_init_) { std::lock_guard<std::mutex> lock(buffer_mutex_);
decode_init(msg); // RCLCPP_INFO(this->get_logger(), "Format: %s", msg->format.c_str());
if (codec_init_[index] == 0) {
decode_init(msg, index);
RCLCPP_INFO_STREAM(rclcpp::get_logger("mega_h26x_decode"), encode_topics_[index] << ":is decoding" );
codec_init_[index] = 1;
} }
decode_frame(msg); decode_frame(msg, index);
} }
rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_image_subscriber_; std::vector<std::string> encode_topics_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb_image_publisher_; std::vector<std::string> decode_topics_;
std::shared_ptr<const AVCodec> codec_; std::vector<rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr>
std::shared_ptr<AVCodecContext> codec_context_; encode_subscribers_;
std::shared_ptr<AVFrame> frame_; std::vector<rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr> decode_publishers_;
std::shared_ptr<AVPacket> packet_;
int codec_init_ = 1; // rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr
int send_ret_; // compressed_image_subscriber_; rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr
int receive_ret_; // rgb_image_publisher_;
std::vector<std::shared_ptr<const AVCodec>> codec_;
std::vector<std::shared_ptr<AVCodecContext>> codec_context_;
std::vector<std::shared_ptr<AVFrame>> frame_;
std::vector<std::shared_ptr<AVPacket>> packet_;
std::vector<int> codec_init_;
std::vector<int> send_ret_;
std::vector<int> receive_ret_;
}; };
int main(int argc, char** argv) { int main(int argc, char** argv) {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
auto node = std::make_shared<H264DecoderNode>(); auto node = std::make_shared<H264DecoderNode>();
rclcpp::spin(node); rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 1);
executor.add_node(node);
executor.spin();
rclcpp::shutdown(); rclcpp::shutdown();
return 0; return 0;
} }