From 0ac38844b0b26020a1dbd73115ffb70425c100bf Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Mon, 14 Sep 2026 12:34:26 +0800 Subject: [PATCH] fix: port decoder multiplexing optimization to v2/develop --- orbbec_camera/CMakeLists.txt | 36 ++++- .../include/orbbec_camera/jetson_nv_decoder.h | 14 +- orbbec_camera/src/jetson_nv_decoder.cpp | 137 ++++++++++++++++-- 3 files changed, 166 insertions(+), 21 deletions(-) diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index 79c53699..2d5ad886 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/orbbec_camera/CMakeLists.txt @@ -142,8 +142,40 @@ if(USE_NV_HW_DECODER) set(TEGRA_ARMABI /usr/lib/aarch64-linux-gnu/) add_definitions(-DUSE_NV_HW_DECODER) add_compile_options(-Wno-missing-field-initializers -Wno-unused-parameter) - set(NV_LIBRARIES -lnvjpeg -lnvbufsurface -lnvbufsurftransform -lyuv -lv4l2) - list(APPEND NV_LIBRARIES -L${TEGRA_ARMABI} -L${TEGRA_ARMABI}/tegra) + # Search Jetson Multimedia API libraries, excluding CUDA library directories. + find_library(JETSON_JPEG_LIBRARY + NAMES nvmm_jpeg nvjpeg + PATHS + "${TEGRA_ARMABI}/nvidia" + "${TEGRA_ARMABI}/tegra" + NO_DEFAULT_PATH + ) + if(NOT JETSON_JPEG_LIBRARY) + message(FATAL_ERROR + "Jetson Multimedia API JPEG library not found. " + "Install the Multimedia API matching the system L4T version." + ) + endif() + message(STATUS "Jetson JPEG library: ${JETSON_JPEG_LIBRARY}") + + set(NV_LIBRARIES + "${JETSON_JPEG_LIBRARY}" + -lnvbufsurface -lnvbufsurftransform -lyuv -lv4l2 + ) + list(APPEND NV_LIBRARIES + "-L${TEGRA_ARMABI}" + "-L${TEGRA_ARMABI}/nvidia" + "-L${TEGRA_ARMABI}/tegra" + ) + + # The nvmm_jpeg Multimedia API classes use the CUDA driver API. + if(JETSON_JPEG_LIBRARY MATCHES "/libnvmm_jpeg\\.so") + if(CMAKE_VERSION VERSION_LESS "3.17") + message(FATAL_ERROR "nvmm_jpeg support requires CMake 3.17 or newer to find CUDAToolkit.") + endif() + find_package(CUDAToolkit REQUIRED) + list(APPEND NV_LIBRARIES CUDA::cuda_driver) + endif() endif() set(COMMON_INCLUDE_DIRS diff --git a/orbbec_camera/include/orbbec_camera/jetson_nv_decoder.h b/orbbec_camera/include/orbbec_camera/jetson_nv_decoder.h index 1b7acc25..41c72d5b 100644 --- a/orbbec_camera/include/orbbec_camera/jetson_nv_decoder.h +++ b/orbbec_camera/include/orbbec_camera/jetson_nv_decoder.h @@ -14,15 +14,10 @@ * limitations under the License. *******************************************************************************/ #pragma once -#include "utils.h" -#include + +#include #include "jpeg_decoder.h" -#include -#include -#include -#include -#include namespace orbbec_camera { class JetsonNvJPEGDecoder : public JPEGDecoder { @@ -33,6 +28,7 @@ class JetsonNvJPEGDecoder : public JPEGDecoder { bool decode(const std::shared_ptr& frame, uint8_t* dest) override; private: - NvJPEGDecoder* decoder_; + class Impl; + std::unique_ptr decoder_; }; -} // namespace orbbec_camera \ No newline at end of file +} // namespace orbbec_camera diff --git a/orbbec_camera/src/jetson_nv_decoder.cpp b/orbbec_camera/src/jetson_nv_decoder.cpp index 3ad1dfc4..68310bc7 100644 --- a/orbbec_camera/src/jetson_nv_decoder.cpp +++ b/orbbec_camera/src/jetson_nv_decoder.cpp @@ -14,22 +14,125 @@ * limitations under the License. *******************************************************************************/ #include "orbbec_camera/jetson_nv_decoder.h" -#include -#include -#include + +#include +#include +#include + +#include +#include +#include #include #include -#include -#include #include #include +#include + +#include "jpegint.h" #include "orbbec_camera/utils.h" namespace orbbec_camera { +namespace { -JetsonNvJPEGDecoder::JetsonNvJPEGDecoder(int width, int height) : JPEGDecoder(width, height) {} +struct JpegErrorManager { + jpeg_error_mgr base; + jmp_buf jump_buffer; + char message[JMSG_LENGTH_MAX]; +}; -JetsonNvJPEGDecoder::~JetsonNvJPEGDecoder() { delete decoder_; } +void jpegErrorExit(j_common_ptr cinfo) { + auto *error = reinterpret_cast(cinfo->err); + (*cinfo->err->format_message)(cinfo, error->message); + longjmp(error->jump_buffer, 1); +} + +} // namespace + +class JetsonNvJPEGDecoder::Impl { + public: + Impl() { + std::memset(&cinfo_, 0, sizeof(cinfo_)); + std::memset(&error_, 0, sizeof(error_)); + cinfo_.err = jpeg_std_error(&error_.base); + error_.base.error_exit = jpegErrorExit; + + if (setjmp(error_.jump_buffer) != 0) { + return; + } + + jpeg_create_decompress(&cinfo_); + initialized_ = true; + cinfo_.mjpeg_decode = TRUE; + } + + ~Impl() { + if (initialized_) { + jpeg_destroy_decompress(&cinfo_); + } + } + + Impl(const Impl &) = delete; + Impl &operator=(const Impl &) = delete; + + bool isInitialized() const { return initialized_; } + + const char *lastError() const { return error_.message; } + + int decodeToFd(int &fd, unsigned char *input, unsigned long input_size, uint32_t &pixfmt, + uint32_t &width, uint32_t &height) { + if (!initialized_ || input == nullptr || input_size == 0) { + return -1; + } + + error_.message[0] = '\0'; + if (setjmp(error_.jump_buffer) != 0) { + return -1; + } + + NvBufSurface surface; + cinfo_.out_color_space = JCS_YCbCr; + jpeg_mem_src(&cinfo_, input, input_size); + + (void)jpeg_read_header(&cinfo_, TRUE); + + cinfo_.out_color_space = JCS_YCbCr; + cinfo_.IsVendorbuf = TRUE; + cinfo_.pVendor_buf = reinterpret_cast(&surface); + + uint32_t pixel_format = 0; + if (cinfo_.comp_info[0].h_samp_factor == 2) { + pixel_format = + cinfo_.comp_info[0].v_samp_factor == 2 ? V4L2_PIX_FMT_YUV420M : V4L2_PIX_FMT_YUV422M; + } else { + pixel_format = + cinfo_.comp_info[0].v_samp_factor == 1 ? V4L2_PIX_FMT_YUV444M : V4L2_PIX_FMT_YUV422RM; + } + + jpeg_start_decompress(&cinfo_); + if (cinfo_.global_state != DSTATE_READY) { + return -1; + } + + jpeg_read_raw_data(&cinfo_, nullptr, cinfo_.comp_info[0].v_samp_factor * DCTSIZE); + jpeg_finish_decompress(&cinfo_); + + width = cinfo_.image_width % 2 == 1 ? cinfo_.image_width + 1 : cinfo_.image_width; + height = cinfo_.image_height % 2 == 1 ? cinfo_.image_height + 1 : cinfo_.image_height; + pixfmt = pixel_format; + fd = cinfo_.fd; + return 0; + } + + private: + jpeg_decompress_struct cinfo_{}; + JpegErrorManager error_{}; + bool initialized_ = false; +}; + +JetsonNvJPEGDecoder::JetsonNvJPEGDecoder(int width, int height) + : JPEGDecoder(width, height), decoder_(std::make_unique()) {} + +JetsonNvJPEGDecoder::~JetsonNvJPEGDecoder() = default; bool JetsonNvJPEGDecoder::decode(const std::shared_ptr &frame, uint8_t *dest) { if (!isValidJPEG(frame)) { @@ -44,10 +147,24 @@ bool JetsonNvJPEGDecoder::decode(const std::shared_ptr &frame, u while (data_size > 4 && data[data_size - 1] == 0x00) { data_size--; } + + if (!decoder_ || !decoder_->isInitialized()) { + decoder_ = std::make_unique(); + } + if (!decoder_->isInitialized()) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), + "Failed to initialize NVIDIA JPEG decoder"); + decoder_.reset(); + return false; + } + int fd = -1; - decoder_ = NvJPEGDecoder::createJPEGDecoder("jpegdec"); - std::shared_ptr decoder_deleter(nullptr, [&](int *) { delete decoder_; }); - decoder_->decodeToFd(fd, data, data_size, pixfmt, width, height); + if (decoder_->decodeToFd(fd, data, data_size, pixfmt, width, height) != 0) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Failed to decode JPEG frame"); + decoder_.reset(); + return false; + } + if (pixfmt != V4L2_PIX_FMT_YUV422M) { RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Unexpected pixfmt: " << pixfmt); if (fd != -1) {