fixed use nv jpeg decoder

orbbec_camera/CMakeLists.txt

@@ -13,7 +13,7 @@ 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)
+  add_compile_options(-Wall -Wextra -Werror -Wno-pedantic)
 endif ()
 
 # find dependencies
@@ -89,6 +89,8 @@ if (USE_NV_HW_DECODER)
   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/)
+  add_definitions(-DUSE_NV_HW_DECODER)
+  add_compile_options(-Wno-missing-field-initializers -Wno-unused-parameter)
   set(NV_LIBRARIES
     -lnvjpeg -lnvbufsurface -lnvbufsurftransform -lyuv -lv4l2
   )
@@ -115,16 +117,19 @@ set(COMMON_LIBRARIES
   Threads::Threads
 )
 if (USE_RK_HW_DECODER)
-  list(APPEND COMMON_LINK_LIBRARIES
+  list(APPEND COMMON_LIBRARIES
     ${RK_MPP_LIBRARIES}
     ${RGA_LIBRARIES}
   )
-elseif (USE_NV_HW_DECODER)
-  list(APPEND COMMON_LINK_LIBRARIES
-    ${NV_LIBRARIES}
-  )
+
 endif ()
 
+if (USE_NV_HW_DECODER)
+  list(APPEND COMMON_LIBRARIES
+    ${NV_LIBRARIES})
+endif ()
+
+
 set(SOURCE_FILES
   src/d2c_viewer.cpp
   src/dynamic_params.cpp
@@ -156,8 +161,9 @@ if (USE_RK_HW_DECODER)
 endif ()
 
 if (USE_NV_HW_DECODER)
-  add_definitions(-DUSE_NV_HW_DECODER)
   list(APPEND SOURCE_FILES src/jetson_nv_decoder.cpp)
+  list(APPEND COMMON_INCLUDE_DIRS ${JETSON_MULTI_MEDIA_API_INCLUDE_DIR}
+    ${LIBJPEG8B_INCLUDE_DIR})
   # append jetson_multimedia_api source files
   list(APPEND SOURCE_FILES
     ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvBuffer.cpp

orbbec_camera/include/orbbec_camera/utils.h

@@ -72,4 +72,6 @@ std::string fullAccelScaleRangeToString(const OBAccelFullScaleRange& full_scale_
 
 std::string parseUsbPort(const std::string& line);
 
+bool isValidJPEG(const std::shared_ptr<ob::ColorFrame>& frame);
+
 }  // namespace orbbec_camera

orbbec_camera/src/jetson_nv_decoder.cpp

@@ -8,6 +8,8 @@
 #include <fstream>
 #include <libyuv.h>
 #include <rclcpp/rclcpp.hpp>
+#include "orbbec_camera/utils.h"
+
 namespace orbbec_camera {
 
 JetsonNvJPEGDecoder::JetsonNvJPEGDecoder(int width, int height) : JPEGDecoder(width, height) {}
@@ -24,9 +26,6 @@ bool JetsonNvJPEGDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, u
   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_; });
@@ -46,7 +45,8 @@ bool JetsonNvJPEGDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, u
     }
     return false;
   }
-  NvBufSurf::NvCommonAllocateParams nvbufParams = {0};
+  NvBufSurf::NvCommonAllocateParams nvbufParams;
+  memset(&nvbufParams, 0, sizeof(nvbufParams));
 
   nvbufParams.memType = NVBUF_MEM_SURFACE_ARRAY;
   nvbufParams.width = width;

orbbec_camera/src/ob_camera_node.cpp

@@ -767,7 +767,7 @@ std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
   if (frame == nullptr) {
     return nullptr;
   }
-  if (frame->format() == OB_FORMAT_RGB888) {
+  if (frame->format() == OB_FORMAT_RGB || frame->format() == OB_FORMAT_BGR) {
     return frame;
   }
   if (!setupFormatConvertType(frame->format())) {
@@ -799,6 +799,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
   if (!frame) {
     return false;
   }
+
 #if defined(USE_RK_HW_DECODER) || defined(USE_NV_HW_DECODER)
   if (frame && frame->format() != OB_FORMAT_RGB888) {
     if (frame->format() == OB_FORMAT_MJPG && jpeg_decoder_) {

orbbec_camera/src/utils.cpp

@@ -161,6 +161,7 @@ tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]) {
   Eigen::Quaternionf q(m);
   return {q.x(), q.y(), q.z(), q.w()};
 }
+
 std::ostream &operator<<(std::ostream &os, const OBCameraParam &rhs) {
   auto depth_intrinsic = rhs.depthIntrinsic;
   auto rgb_intrinsic = rhs.rgbIntrinsic;
@@ -202,6 +203,7 @@ rclcpp::Time frameTimeStampToROSTime(uint64_t ms) {
   rclcpp::Time stamp(sec, nano_sec);
   return stamp;
 }
+
 std::string getObSDKVersion() {
   std::string major = std::to_string(ob::Version::getMajor());
   std::string minor = std::to_string(ob::Version::getMinor());
@@ -209,6 +211,7 @@ std::string getObSDKVersion() {
   std::string version = major + "." + minor + "." + patch;
   return version;
 }
+
 OBFormat OBFormatFromString(const std::string &format) {
   std::string fixed_format;
   std::transform(format.begin(), format.end(), std::back_inserter(fixed_format),
@@ -301,6 +304,7 @@ rmw_qos_profile_t getRMWQosProfileFromString(const std::string &str_qos) {
     return rmw_qos_profile_default;
   }
 }
+
 bool isOpenNIDevice(int pid) {
   static const std::vector<int> OPENNI_DEVICE_PIDS = {
       0x0300, 0x0301, 0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0407, 0x0601, 0x060b,
@@ -329,6 +333,7 @@ OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString(
     return OB_PRECISION_0MM8;
   }
 }
+
 OBMultiDeviceSyncMode OBSyncModeFromString(const std::string &mode) {
   if (mode == "FREE_RUN") {
     return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN;
@@ -526,4 +531,19 @@ std::string parseUsbPort(const std::string &line) {
   }
   return port_id;
 }
+
+bool isValidJPEG(const std::shared_ptr<ob::ColorFrame> &frame) {
+  if (frame->dataSize() < 2) {  // Checking both start and end markers, so minimal size is 4
+    return false;
+  }
+
+  const auto *data = static_cast<const uint8_t *>(frame->data());
+
+  // Check for JPEG start marker
+  if (data[0] != 0xFF || data[1] != 0xD8) {
+    return false;
+  }
+  return true;
+}
+
 }  // namespace orbbec_camera