diff --git a/orbbec_camera/config/tools/megah26xdecode/mega_h26x_decode_params.json b/orbbec_camera/config/tools/megah26xdecode/mega_h26x_decode_params.json new file mode 100644 index 00000000..369651fa --- /dev/null +++ b/orbbec_camera/config/tools/megah26xdecode/mega_h26x_decode_params.json @@ -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" + ] + } +} \ No newline at end of file diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index edaefb60..132d1895 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -1263,14 +1263,13 @@ void OBCameraNode::setupPublishers() { camera_info_publishers_[stream_index] = node_->create_publisher( topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile), camera_info_qos_profile)); - + topic = name + "/h26x_encoded_data"; auto image_h264_qos_profile = getRMWQosProfileFromString(image_qos); if (format_str_[stream_index] == "H264" || format_str_[stream_index] == "H265") { camera_h26x_publishers_[stream_index] = node_->create_publisher( - "/camera/color/h26x_encoded_data", - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_h264_qos_profile), - image_h264_qos_profile)); + topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_h264_qos_profile), + image_h264_qos_profile)); } if (isGemini335PID(pid)) { metadata_publishers_[stream_index] = diff --git a/orbbec_camera/tools/mega_h26x_decode_node.cpp b/orbbec_camera/tools/mega_h26x_decode_node.cpp index bab6801c..6bd28203 100644 --- a/orbbec_camera/tools/mega_h26x_decode_node.cpp +++ b/orbbec_camera/tools/mega_h26x_decode_node.cpp @@ -4,6 +4,7 @@ extern "C" { #include #include #include +#include } #include @@ -18,128 +19,181 @@ extern "C" { #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "libobsensor/hpp/Utils.hpp" + class H264DecoderNode : public rclcpp::Node { public: H264DecoderNode() : Node("h264_decoder_node") { + av_log_set_level(AV_LOG_QUIET); avformat_network_init(); + params_init(); rclcpp::QoS qos_settings(30); qos_settings.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); qos_settings.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); qos_settings.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST); - compressed_image_subscriber_ = this->create_subscription( - "/camera/color/h26x_encoded_data", qos_settings, - std::bind(&H264DecoderNode::compressedImageCallback, this, std::placeholders::_1)); + // compressed_image_subscriber_ = this->create_subscription( + // "/camera/color/h26x_encoded_data", qos_settings, + // std::bind(&H264DecoderNode::compressedImageCallback, this, std::placeholders::_1)); - rgb_image_publisher_ = this->create_publisher( - "/camera/color/h26x_decoder/image_raw", qos_settings); + // rgb_image_publisher_ = this->create_publisher( + // "/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( + encode_topics_[i], qos_settings, + [this, i](sensor_msgs::msg::CompressedImage::SharedPtr msg) { + this->compressedImageCallback(msg, i); + }); + + auto decode_pub = + this->create_publisher(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: - 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>(); + decode_topics_ = + json_data["h26x_decode_params"]["decode_topics"].get>(); + } + void decode_init(const sensor_msgs::msg::CompressedImage::SharedPtr msg, size_t index) { if (msg->format == "h264") { - codec_ = std::shared_ptr(avcodec_find_decoder(AV_CODEC_ID_H264), - [](const AVCodec*) {}); - codec_context_ = std::shared_ptr(avcodec_alloc_context3(codec_.get()), - [](AVCodecContext* ctx) { - if (ctx) { - avcodec_free_context(&ctx); - } - }); + codec_[index] = std::shared_ptr(avcodec_find_decoder(AV_CODEC_ID_H264), + [](const AVCodec*) {}); + codec_context_[index] = std::shared_ptr(avcodec_alloc_context3(codec_[index].get()), + [](AVCodecContext* ctx) { + if (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"); return; } - frame_ = std::shared_ptr(av_frame_alloc(), [](AVFrame* f) { + frame_[index] = std::shared_ptr(av_frame_alloc(), [](AVFrame* f) { if (f) av_frame_free(&f); }); - packet_ = std::shared_ptr(av_packet_alloc(), [](AVPacket* p) { + packet_[index] = std::shared_ptr(av_packet_alloc(), [](AVPacket* p) { if (p) av_packet_free(&p); }); - codec_init_ = 0; } else if (msg->format == "h265") { - codec_ = std::shared_ptr(avcodec_find_decoder(AV_CODEC_ID_HEVC), + codec_[index] = std::shared_ptr(avcodec_find_decoder(AV_CODEC_ID_HEVC), [](const AVCodec*) {}); - codec_context_ = std::shared_ptr(avcodec_alloc_context3(codec_.get()), + codec_context_[index] = std::shared_ptr(avcodec_alloc_context3(codec_[index].get()), [](AVCodecContext* ctx) { if (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"); return; } - frame_ = std::shared_ptr(av_frame_alloc(), [](AVFrame* f) { + frame_[index] = std::shared_ptr(av_frame_alloc(), [](AVFrame* f) { if (f) av_frame_free(&f); }); - packet_ = std::shared_ptr(av_packet_alloc(), [](AVPacket* p) { + packet_[index] = std::shared_ptr(av_packet_alloc(), [](AVPacket* p) { if (p) av_packet_free(&p); }); - codec_init_ = 0; } } - void decode_frame(const sensor_msgs::msg::CompressedImage::SharedPtr msg) { - av_packet_unref(packet_.get()); - packet_->data = const_cast(msg->data.data()); - packet_->size = msg->data.size(); + void decode_frame(const sensor_msgs::msg::CompressedImage::SharedPtr msg, size_t index) { + av_packet_unref(packet_[index].get()); + packet_[index]->data = const_cast(msg->data.data()); + packet_[index]->size = msg->data.size(); std::stringstream ss; const size_t bytes_to_print = std::min(msg->data.size(), 32); for (size_t i = 0; i < bytes_to_print; ++i) { ss << std::hex << std::setw(2) << std::setfill('0') << static_cast(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()); - if (send_ret_ >= 0) { - receive_ret_ = avcodec_receive_frame(codec_context_.get(), frame_.get()); - if (receive_ret_ >= 0) { - cv::Mat rgb_image(frame_->height, frame_->width, CV_8UC3); - SwsContext* sws_context = sws_getContext(frame_->width, frame_->height, - static_cast(frame_->format), - frame_->width, frame_->height, AV_PIX_FMT_BGR24, + send_ret_[index]= avcodec_send_packet(codec_context_[index].get(), packet_[index].get()); + if (send_ret_[index] >= 0) { + receive_ret_[index] = avcodec_receive_frame(codec_context_[index].get(), frame_[index].get()); + if (receive_ret_[index] >= 0) { + cv::Mat rgb_image(frame_[index]->height, frame_[index]->width, CV_8UC3); + SwsContext* sws_context = sws_getContext(frame_[index]->width, frame_[index]->height, + static_cast(frame_[index]->format), + frame_[index]->width, frame_[index]->height, AV_PIX_FMT_BGR24, SWS_BILINEAR, nullptr, nullptr, nullptr); uint8_t* dest[1] = {rgb_image.data}; int linesize[1] = {static_cast(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); auto rgb_image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb_image).toImageMsg(); 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) { - RCLCPP_INFO(this->get_logger(), "Format: %s", msg->format.c_str()); - if (codec_init_) { - decode_init(msg); + void compressedImageCallback(const sensor_msgs::msg::CompressedImage::SharedPtr msg, + size_t index) { + std::lock_guard lock(buffer_mutex_); + // 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::SharedPtr compressed_image_subscriber_; - rclcpp::Publisher::SharedPtr rgb_image_publisher_; - std::shared_ptr codec_; - std::shared_ptr codec_context_; - std::shared_ptr frame_; - std::shared_ptr packet_; - int codec_init_ = 1; - int send_ret_; - int receive_ret_; + std::vector encode_topics_; + std::vector decode_topics_; + std::vector::SharedPtr> + encode_subscribers_; + std::vector::SharedPtr> decode_publishers_; + + // rclcpp::Subscription::SharedPtr + // compressed_image_subscriber_; rclcpp::Publisher::SharedPtr + // rgb_image_publisher_; + std::vector> codec_; + std::vector> codec_context_; + std::vector> frame_; + std::vector> packet_; + std::vector codec_init_; + std::vector send_ret_; + std::vector receive_ret_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - rclcpp::spin(node); + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 1); + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; }