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

first work gst decoder

Browse Source
This commit is contained in:
Joe Dong
2023-08-28 20:50:19 +08:00
parent 9ddeb79ed1
commit 78dc620f33
7 changed files with 117 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/gst_decoder.cpp
+85 -20
View File
@@ -2,6 +2,7 @@
#include <rclcpp/rclcpp.hpp>
#include <gst/app/gstappsrc.h>
#include <gst/app/gstappsink.h>
#include <glog/logging.h>
namespace orbbec_camera {
std::string hwDecoderToString(HWDecoder hw_decoder) {
@@ -12,6 +13,8 @@ std::string hwDecoderToString(HWDecoder hw_decoder) {
return "nvjpegdec";
case HWDecoder::AMLOGIC_CODEC:
return "amlvenc";
case HWDecoder::AV_CODEC:
return "avdec_mjpeg";
default:
return "unknown";
}
@@ -21,8 +24,11 @@ GstreamerMjpegDecoder::GstreamerMjpegDecoder(int width, int height, HWDecoder hw
: MjpegDecoder(width, height),
hw_decoder_(hwDecoderToString(hw_decoder)),
buffer_size_(width * height * 3) {
gst_init(NULL, NULL);
buffer_pool_ = gst_buffer_pool_new();
CHECK_NOTNULL(buffer_pool_);
GstStructure* config = gst_buffer_pool_get_config(buffer_pool_);
CHECK_NOTNULL(config);
gst_buffer_pool_config_set_params(config, NULL, buffer_size_, 0, 0);
if (hw_decoder_ == "unknown") {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "hw decoder is unknown");
@@ -38,15 +44,47 @@ GstreamerMjpegDecoder::GstreamerMjpegDecoder(int width, int height, HWDecoder hw
"gst buffer pool set active error");
throw std::runtime_error("gst buffer pool set active error");
}
std::string pipeline_str = "appsrc name=appsrc0 is-live=true do-timestamp=true ! " + hw_decoder_ +
" ! videoconvert ! video/x-raw,format=RGB ! appsink name=appsink0";
pipeline_ = gst_parse_launch(pipeline_str.c_str(), NULL);
// Create GStreamer elements
appsrc_ = gst_element_factory_make("appsrc", "appsrc");
jpegparse_ = gst_element_factory_make("jpegparse", "jpegparse");
jpegdec_ = gst_element_factory_make(hw_decoder_.c_str(), hw_decoder_.c_str());
videoconvert_ = gst_element_factory_make("videoconvert", "videoconvert");
appsink_ = gst_element_factory_make("appsink", "appsink");
// Check for null pointers
if (!appsrc_ || !jpegparse_ || !jpegdec_ || !videoconvert_ || !appsink_) {
throw std::runtime_error("Failed to create GStreamer elements");
}
// Set element properties
g_object_set(G_OBJECT(appsrc_), "caps",
gst_caps_new_simple("image/jpeg", "width", G_TYPE_INT, width, "height", G_TYPE_INT,
height, "framerate", GST_TYPE_FRACTION, 0, 1, NULL),
NULL);
g_object_set(G_OBJECT(appsink_), "blocksize", buffer_size_, "sync", FALSE, "max-buffers", 1,
"drop", TRUE, NULL);
g_object_set(G_OBJECT(appsink_), "caps",
gst_caps_new_simple("video/x-raw", "format", G_TYPE_STRING, "RGB", "width",
G_TYPE_INT, width, "height", G_TYPE_INT, height, NULL),
NULL);
// Create pipeline and add elements
pipeline_ = gst_pipeline_new("pipeline");
if (!pipeline_) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "gst parse launch error");
throw std::runtime_error("gst parse launch error");
throw std::runtime_error("Failed to create pipeline");
}
gst_bin_add_many(GST_BIN(pipeline_), appsrc_, jpegparse_, jpegdec_, videoconvert_, appsink_,
NULL);
if (!gst_element_link_many(appsrc_, jpegparse_, jpegdec_, videoconvert_, appsink_, NULL)) {
throw std::runtime_error("Failed to link GStreamer elements");
}
// Set pipeline to playing state
if (gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
throw std::runtime_error("Failed to set GStreamer pipeline to playing state");
}
appsrc_ = gst_bin_get_by_name(GST_BIN(pipeline_), "appsrc0");
appsink_ = gst_bin_get_by_name(GST_BIN(pipeline_), "appsink0");
}
GstreamerMjpegDecoder::~GstreamerMjpegDecoder() {
@@ -59,37 +97,64 @@ GstreamerMjpegDecoder::~GstreamerMjpegDecoder() {
gst_buffer_pool_set_active(buffer_pool_, FALSE);
g_object_unref(buffer_pool_);
}
gst_deinit();
}
bool GstreamerMjpegDecoder::decode(const std::shared_ptr<ob::ColorFrame>& frame, uint8_t* dest) {
GstBuffer* buffer = NULL;
GstMapInfo map;
// Acquire buffer from pool
if (gst_buffer_pool_acquire_buffer(buffer_pool_, &buffer, NULL) != GST_FLOW_OK) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
"gst buffer pool acquire buffer error");
return false;
}
// Map buffer and copy frame data
if (gst_buffer_map(buffer, &map, GST_MAP_WRITE)) {
if (frame->dataSize() <= map.size) {
memcpy(map.data, frame->data(), frame->dataSize());
} else {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
"Frame data size exceeds buffer size");
gst_buffer_unmap(buffer, &map);
return false;
}
gst_buffer_unmap(buffer, &map);
} else {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "Failed to map buffer");
return false;
}
gst_buffer_map(buffer, &map, GST_MAP_WRITE);
memcpy(map.data, frame->data(), frame->dataSize());
gst_buffer_unmap(buffer, &map); // Unmap the buffer after copying
// Push buffer to appsrc
GstFlowReturn flow_return = gst_app_src_push_buffer(GST_APP_SRC(appsrc_), buffer);
if (flow_return != GST_FLOW_OK) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
"Failed to push buffer to appsrc, GstFlowReturn: " << flow_return);
return false;
}
// Pull sample from appsink
GstClockTime timeout = GST_SECOND;
GstSample* sample = gst_app_sink_try_pull_sample(GST_APP_SINK(appsink_), timeout);
gst_app_src_push_buffer(GST_APP_SRC(appsrc_), buffer);
GstSample* sample = gst_app_sink_pull_sample(GST_APP_SINK(appsink_));
if (sample) {
GstBuffer* outBuffer = gst_sample_get_buffer(sample);
GstMapInfo mapInfo;
gst_buffer_map(outBuffer, &mapInfo, GST_MAP_READ);
// Copy data to destination buffer
memcpy(dest, mapInfo.data, mapInfo.size);
gst_buffer_unmap(outBuffer, &mapInfo);
if (gst_buffer_map(outBuffer, &mapInfo, GST_MAP_READ)) {
// Copy data to destination buffer
memcpy(dest, mapInfo.data, mapInfo.size);
gst_buffer_unmap(outBuffer, &mapInfo);
} else {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
"Failed to map output buffer");
gst_sample_unref(sample);
return false;
}
gst_sample_unref(sample);
return true;
} else {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "Failed to decode frame");
RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"),
"Failed to decode frame " << strerror(errno) << " " << errno);
return false;
}
}
orbbec_camera/src/ob_camera_node.cpp
+12 -5
View File
@@ -20,6 +20,8 @@
#if defined(USE_RK_HW_DECODER)
#include "orbbec_camera/rk_mpp_decoder.h"
#elif defined(USE_GST_HW_DECODER)
#include "orbbec_camera/gst_decoder.h"
#endif
namespace orbbec_camera {
@@ -45,16 +47,18 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
setupDefaultImageFormat();
setupTopics();
#if defined(USE_RK_HW_DECODER)
mjpeg_decoder_ = std::make_unique<RKMjpegDecoder>(width_[COLOR], height_[COLOR]);
#elif defined(USE_GST_HW_DECODER)
mjpeg_decoder_ =
std::make_unique<GstreamerMjpegDecoder>(width_[COLOR], height_[COLOR], hw_decoder_);
#endif
startStreams();
if (enable_d2c_viewer_) {
auto rgb_qos = getRMWQosProfileFromString(image_qos_[COLOR]);
auto depth_qos = getRMWQosProfileFromString(image_qos_[DEPTH]);
d2c_viewer_ = std::make_unique<D2CViewer>(node_, rgb_qos, depth_qos);
}
#if defined(USE_RK_HW_DECODER)
mjpeg_decoder_ = std::make_unique<RKMjpegDecoder>(width_[COLOR], height_[COLOR]);
#endif
}
template <class T>
@@ -424,6 +428,9 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(soft_filter_speckle_size_, "soft_filter_speckle_size", -1);
setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
setAndGetNodeParameter<double>(angular_vel_cov_, "angular_vel_cov", 0.02);
int hw_decoder = 0;
setAndGetNodeParameter<int>(hw_decoder, "hw_decoder", 3);
hw_decoder_ = static_cast<HWDecoder>(hw_decoder);
}
void OBCameraNode::setupTopics() {
@@ -741,7 +748,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
auto frame_format = frame->format();
if (frame->type() == OB_FRAME_COLOR && frame_format != OB_FORMAT_RGB888) {
if (frame_format == OB_FORMAT_MJPG || frame_format == OB_FORMAT_MJPEG) {
#if defined(USE_RK_HW_DECODER)
#if defined(USE_RK_HW_DECODER) || defined(USE_GST_HW_DECODER)
CHECK_NOTNULL(mjpeg_decoder_.get());
video_frame = frame->as<ob::ColorFrame>();
const auto &color_frame = frame->as<ob::ColorFrame>();