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

add jetson hardware decoder

Browse Source
This commit is contained in:
Joe Dong
2023-09-07 16:46:59 +08:00
parent 43118e286a
commit ee8b077fdd
10 changed files with 199 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/jetson_nv_decoder.cpp
+101
View File
@@ -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
+8
View File
@@ -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
-8
View File
@@ -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
+8 -4
View File
@@ -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_) {
CHECK_NOTNULL(mjpeg_decoder_.get());
if (frame->format() == OB_FORMAT_MJPG && jpeg_decoder_) {
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
+4 -4
View File
@@ -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());