add jetson hardware decoder

orbbec_camera/CMakeLists.txt

@@ -9,6 +9,8 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -O3")
 set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fPIC -g")
 set(CMAKE_BUILD_TYPE "Release")
 option(USE_RK_HW_DECODER "Use Rockchip hardware decoder" OFF)
+option(USE_NV_HW_DECODER "Use Nvidia hardware decoder" OFF)
+
 
 if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Werror -Wpedantic)
@@ -81,6 +83,18 @@ set(ORBBEC_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/SDK/include/)
 
 set(CMAKE_BUILD_RPATH "${CMAKE_BUILD_RPATH}:${ORBBEC_LIBS_DIR}")
 set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_RPATH}:${ORBBEC_LIBS_DIR}")
+if (USE_NV_HW_DECODER)
+  set(JETSON_MULTI_MEDIA_API_DIR /usr/src/jetson_multimedia_api)
+  set(JETSON_MULTI_MEDIA_API_CLASS_DIR ${JETSON_MULTI_MEDIA_API_DIR}/samples/common/classes)
+  set(JETSON_MULTI_MEDIA_API_INCLUDE_DIR ${JETSON_MULTI_MEDIA_API_DIR}/include/)
+  set(LIBJPEG8B_INCLUDE_DIR ${JETSON_MULTI_MEDIA_API_INCLUDE_DIR}/libjpeg-8b)
+  set(TEGRA_ARMABI /usr/lib/aarch64-linux-gnu/)
+  set(NV_LIBRARIES
+    -lnvjpeg -lnvbufsurface -lnvbufsurftransform -lyuv -lv4l2
+  )
+  list(APPEND NV_LIBRARIES
+    -L${TEGRA_ARMABI} -L${TEGRA_ARMABI}/tegra)
+endif ()
 
 set(COMMON_INCLUDE_DIRS
   $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
@@ -98,8 +112,18 @@ set(COMMON_LIBRARIES
   ${GLOG_LIBRARIES}
   -lOrbbecSDK
   -L${ORBBEC_LIBS_DIR}
-  ${RK_MPP_LIBRARIES}
+  Threads::Threads
 )
+if (USE_RK_HW_DECODER)
+  list(APPEND COMMON_LINK_LIBRARIES
+    ${RK_MPP_LIBRARIES}
+    ${RGA_LIBRARIES}
+  )
+elseif (USE_NV_HW_DECODER)
+  list(APPEND COMMON_LINK_LIBRARIES
+    ${NV_LIBRARIES}
+  )
+endif ()
 
 set(SOURCE_FILES
   src/d2c_viewer.cpp
@@ -110,7 +134,7 @@ set(SOURCE_FILES
   src/ros_service.cpp
   src/synced_imu_publisher.cpp
   src/utils.cpp
-  src/mjpeg_decoder.cpp
+  src/jpeg_decoder.cpp
 )
 
 if (USE_RK_HW_DECODER)
@@ -131,6 +155,26 @@ if (USE_RK_HW_DECODER)
   endif ()
 endif ()
 
+if (USE_NV_HW_DECODER)
+  add_definitions(-DUSE_NV_HW_DECODER)
+  list(APPEND SOURCE_FILES src/jetson_nv_decoder.cpp)
+  # append jetson_multimedia_api source files
+  list(APPEND SOURCE_FILES
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvBuffer.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvElement.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvElementProfiler.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvJpegDecoder.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvJpegEncoder.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvLogging.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvUtils.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvV4l2Element.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvV4l2ElementPlane.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvVideoDecoder.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvVideoEncoder.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvBufSurface.cpp
+  )
+endif ()
+
 macro(add_orbbec_executable TARGET SOURCE)
   add_executable(${TARGET} ${SOURCE})
   target_include_directories(${TARGET} PUBLIC ${COMMON_INCLUDE_DIRS})

orbbec_camera/include/orbbec_camera/jetson_nv_decoder.h

@@ -0,0 +1,23 @@
+#pragma once
+#include "utils.h"
+#include <opencv2/opencv.hpp>
+
+#include "jpeg_decoder.h"
+#include <NvJpegDecoder.h>
+#include <NvUtils.h>
+#include <NvV4l2Element.h>
+#include <NvJpegDecoder.h>
+#include <NvV4l2Element.h>
+
+namespace orbbec_camera {
+class JetsonNvJPEGDecoder : public JPEGDecoder {
+ public:
+  JetsonNvJPEGDecoder(int width, int height);
+  ~JetsonNvJPEGDecoder() override;
+
+  bool decode(const std::shared_ptr<ob::ColorFrame>& frame, uint8_t* dest) override;
+
+ private:
+  NvJPEGDecoder* decoder_;
+};
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/jpeg_decoder.h

@@ -5,18 +5,12 @@
 #include "libobsensor/ObSensor.hpp"
 
 namespace orbbec_camera {
-enum HWDecoder {
-  ROCKCHIP_MPP = 0,
-  NV_JPEG_DEC = 1,
-  AMLOGIC_CODEC = 2,
-  AV_CODEC = 3,
-};
 
-class MjpegDecoder {
+class JPEGDecoder {
  public:
-  MjpegDecoder(int width, int height);
+  JPEGDecoder(int width, int height);
 
-  virtual ~MjpegDecoder();
+  virtual ~JPEGDecoder();
 
   virtual bool decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) = 0;
 

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -56,7 +56,7 @@
 #include "orbbec_camera/dynamic_params.h"
 #include "orbbec_camera/d2c_viewer.h"
 #include "magic_enum/magic_enum.hpp"
-#include "mjpeg_decoder.h"
+#include "jpeg_decoder.h"
 
 #define STREAM_NAME(sip)                                                                       \
   (static_cast<std::ostringstream&&>(std::ostringstream()                                      \
@@ -412,7 +412,7 @@ class OBCameraNode {
   std::deque<IMUData> imu_history_;
   IMUData accel_data_{ACCEL, {0, 0, 0}, -1.0};
   // mjpeg decoder
-  std::shared_ptr<MjpegDecoder> mjpeg_decoder_ = nullptr;
+  std::shared_ptr<JPEGDecoder> jpeg_decoder_ = nullptr;
   uint8_t* rgb_buffer_ = nullptr;
   bool is_color_frame_decoded_ = false;
 };

orbbec_camera/include/orbbec_camera/rk_mpp_decoder.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "mjpeg_decoder.h"
+#include "jpeg_decoder.h"
 #include <rockchip/mpp_buffer.h>
 #include <rockchip/mpp_err.h>
 #include <rockchip/mpp_frame.h>
@@ -17,11 +17,11 @@
 #define MPP_ALIGN(x, a) (((x) + (a)-1) & ~((a)-1))
 
 namespace orbbec_camera {
-class RKMjpegDecoder : public MjpegDecoder {
+class RKJPEGDecoder : public JPEGDecoder {
  public:
-  RKMjpegDecoder(int width, int height);
+  RKJPEGDecoder(int width, int height);
 
-  ~RKMjpegDecoder() override;
+  ~RKJPEGDecoder() override;
 
   bool decode(const std::shared_ptr<ob::ColorFrame>& frame, uint8_t* dest) override;
 

orbbec_camera/src/jetson_nv_decoder.cpp

@@ -0,0 +1,101 @@
+#include "orbbec_camera/jetson_nv_decoder.h"
+#include <NvJpegDecoder.h>
+#include <NvV4l2Element.h>
+#include <algorithm>
+#include <nvbufsurface.h>
+#include <nvbufsurftransform.h>
+#include <NvBufSurface.h>
+#include <fstream>
+#include <libyuv.h>
+#include <rclcpp/rclcpp.hpp>
+namespace orbbec_camera {
+
+JetsonNvJPEGDecoder::JetsonNvJPEGDecoder(int width, int height) : JPEGDecoder(width, height) {}
+
+JetsonNvJPEGDecoder::~JetsonNvJPEGDecoder() { delete decoder_; }
+
+bool JetsonNvJPEGDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) {
+  if (!isValidJPEG(frame)) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Invalid JPEG frame");
+    return false;
+  }
+  uint32_t pixfmt = 0;
+  auto *data = static_cast<uint8_t *>(frame->data());
+  uint32_t width = 0;
+  uint32_t height = 0;
+  auto data_size = frame->dataSize();
+  while (data[data_size - 1] == 0) {
+    data_size--;
+  }
+  int fd = -1;
+  decoder_ = NvJPEGDecoder::createJPEGDecoder("jpegdec");
+  std::shared_ptr<int> decoder_deleter(nullptr, [&](int *) { delete decoder_; });
+  decoder_->decodeToFd(fd, data, data_size, pixfmt, width, height);
+  if (pixfmt != V4L2_PIX_FMT_YUV422M) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Unexpected pixfmt: " << pixfmt);
+    if (fd != -1) {
+      close(fd);
+    }
+    return false;
+  }
+  if (width != static_cast<uint32_t>(width_) || height != static_cast<uint32_t>(height_)) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"),
+                        "Unexpected width/height: " << width << "x" << height);
+    if (fd != -1) {
+      close(fd);
+    }
+    return false;
+  }
+  NvBufSurf::NvCommonAllocateParams nvbufParams = {0};
+
+  nvbufParams.memType = NVBUF_MEM_SURFACE_ARRAY;
+  nvbufParams.width = width;
+  nvbufParams.height = height;
+  nvbufParams.layout = NVBUF_LAYOUT_PITCH;
+  nvbufParams.colorFormat = NVBUF_COLOR_FORMAT_RGBA;
+  int rgba_fd = -1;
+  int ret = NvBufSurf::NvAllocate(&nvbufParams, 1, &rgba_fd);
+  if (ret != 0) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Failed to allocate buffer");
+    return false;
+  }
+  NvBufSurf::NvCommonTransformParams transform_params;
+  transform_params.src_top = 0;
+  transform_params.src_left = 0;
+  transform_params.src_width = width;
+  transform_params.src_height = height;
+  transform_params.dst_top = 0;
+  transform_params.dst_left = 0;
+  transform_params.dst_width = width;
+  transform_params.dst_height = height;
+  transform_params.flag = NVBUFSURF_TRANSFORM_FILTER;
+  transform_params.flip = NvBufSurfTransform_None;
+  transform_params.filter = NvBufSurfTransformInter_Nearest;
+  ret = NvBufSurf::NvTransform(&transform_params, fd, rgba_fd);
+  if (ret != 0) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Failed to transform buffer");
+    if (rgba_fd != -1) {
+      NvBufSurf::NvDestroy(rgba_fd);
+    }
+    return false;
+  }
+  NvBufSurface *nvbuf_surf = 0;
+  NvBufSurfaceFromFd(rgba_fd, (void **)&nvbuf_surf);
+  ret = NvBufSurfaceMap(nvbuf_surf, 0, 0, NVBUF_MAP_READ_WRITE);
+  if (ret < 0) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Failed to map buffer");
+    return false;
+  }
+  NvBufSurfaceSyncForCpu(nvbuf_surf, 0, 0);
+  uint8_t *rgba = (uint8_t *)nvbuf_surf->surfaceList[0].mappedAddr.addr[0];
+  int src_stride_argb = width * 4;
+  int dst_stride_rgb24 = width * 3;
+
+  libyuv::ARGBToRGB24(rgba, src_stride_argb, dest, dst_stride_rgb24, width, height);
+  NvBufSurfaceUnMap(nvbuf_surf, 0, 0);
+  if (rgba_fd != -1) {
+    NvBufSurf::NvDestroy(rgba_fd);
+  }
+  return true;
+}
+}  // namespace orbbec_camera

orbbec_camera/src/jpeg_decoder.cpp

@@ -0,0 +1,8 @@
+
+#include <orbbec_camera/jpeg_decoder.h>
+
+namespace orbbec_camera {
+JPEGDecoder::JPEGDecoder(int width, int height) : width_(width), height_(height) {}
+JPEGDecoder::~JPEGDecoder() {}
+
+}  // namespace orbbec_camera

orbbec_camera/src/mjpeg_decoder.cpp

@@ -1,8 +0,0 @@
-
-#include <orbbec_camera/mjpeg_decoder.h>
-
-namespace orbbec_camera {
-MjpegDecoder::MjpegDecoder(int width, int height) : width_(width), height_(height) {}
-MjpegDecoder::~MjpegDecoder() {}
-
-}  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -20,6 +20,8 @@
 
 #if defined(USE_RK_HW_DECODER)
 #include "orbbec_camera/rk_mpp_decoder.h"
+#elif defined(USE_NV_HW_DECODER)
+#include "orbbec_camera/jetson_nv_decoder.h"
 #endif
 
 namespace orbbec_camera {
@@ -47,7 +49,9 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   setupDefaultImageFormat();
   setupTopics();
 #if defined(USE_RK_HW_DECODER)
-  mjpeg_decoder_ = std::make_unique<RKMjpegDecoder>(width_[COLOR], height_[COLOR]);
+  jpeg_decoder_ = std::make_unique<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
+#elif defined(USE_NV_HW_DECODER)
+  jpeg_decoder_ = std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR], height_[COLOR]);
 #endif
   startStreams();
   if (enable_d2c_viewer_) {
@@ -797,11 +801,11 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
   }
 #if defined(USE_RK_HW_DECODER) || defined(USE_NV_HW_DECODER)
   if (frame && frame->format() != OB_FORMAT_RGB888) {
-    if (frame->format() == OB_FORMAT_MJPG && mjpeg_decoder_) {
+    if (frame->format() == OB_FORMAT_MJPG && jpeg_decoder_) {
-      CHECK_NOTNULL(mjpeg_decoder_.get());
+      CHECK_NOTNULL(jpeg_decoder_.get());
       CHECK_NOTNULL(rgb_buffer_);
       auto video_frame = frame->as<ob::ColorFrame>();
-      bool ret = mjpeg_decoder_->decode(video_frame, rgb_buffer_);
+      bool ret = jpeg_decoder_->decode(video_frame, rgb_buffer_);
       if (!ret) {
         RCLCPP_ERROR_STREAM(logger_, "Decode frame failed");
         is_decoded = false;

orbbec_camera/src/rk_mpp_decoder.cpp

@@ -5,7 +5,7 @@
 
 namespace orbbec_camera {
 
-RKMjpegDecoder::RKMjpegDecoder(int width, int height) : MjpegDecoder(width, height) {
+RKJPEGDecoder::RKJPEGDecoder(int width, int height) : JPEGDecoder(width, height) {
   rgb_buffer_ = new uint8_t[width_ * height_ * 3];
   MPP_RET ret = mpp_create(&mpp_ctx_, &mpp_api_);
   if (ret != MPP_OK) {
@@ -73,7 +73,7 @@ RKMjpegDecoder::RKMjpegDecoder(int width, int height) : MjpegDecoder(width, heig
   data_buffer_ = (uint8_t *)mpp_buffer_get_ptr(mpp_packet_buffer_);
 }
 
-RKMjpegDecoder::~RKMjpegDecoder() {
+RKJPEGDecoder::~RKJPEGDecoder() {
   if (mpp_frame_buffer_) {
     mpp_buffer_put(mpp_frame_buffer_);
     mpp_frame_buffer_ = nullptr;
@@ -107,7 +107,7 @@ RKMjpegDecoder::~RKMjpegDecoder() {
   }
 }
 
-bool RKMjpegDecoder::mppFrame2RGB(const MppFrame frame, uint8_t *data) {
+bool RKJPEGDecoder::mppFrame2RGB(const MppFrame frame, uint8_t *data) {
   int width = mpp_frame_get_width(frame);
   int height = mpp_frame_get_height(frame);
   MppBuffer buffer = mpp_frame_get_buffer(frame);
@@ -153,7 +153,7 @@ bool RKMjpegDecoder::mppFrame2RGB(const MppFrame frame, uint8_t *data) {
 #endif
 }
 
-bool RKMjpegDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) {
+bool RKJPEGDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) {
   MPP_RET ret = MPP_OK;
   memset(data_buffer_, 0, width_ * height_ * 3);
   memcpy(data_buffer_, frame->data(), frame->dataSize());