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

Added H26x decoding node for femto_mega

Browse Source
This commit is contained in:
jj
2024-09-18 22:21:04 +08:00
parent cdd1c9fbd9
commit 3cf375fb07
6 changed files with 342 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/CMakeLists.txt
+5 -1
View File
@@ -56,7 +56,7 @@ foreach (dep IN LISTS dependencies)
endforeach ()
find_package(PkgConfig REQUIRED)
pkg_check_modules(FFMPEG REQUIRED libavcodec libavformat libavutil libswscale)
if (USE_RK_HW_DECODER)
pkg_search_module(RK_MPP REQUIRED rockchip_mpp)
@@ -110,12 +110,14 @@ set(COMMON_INCLUDE_DIRS
$<INSTALL_INTERFACE:include>
${ORBBEC_INCLUDE_DIR}
${OpenCV_INCLUDED_DIRS}
${FFMPEG_INCLUDE_DIRS}
${CMAKE_CURRENT_SOURCE_DIR}/tools
)
set(COMMON_LIBRARIES
${ORBBEC_SDK_LIBRARIES}
${OpenCV_LIBS}
${FFMPEG_LIBRARIES}
Eigen3::Eigen
-lOrbbecSDK
-L${ORBBEC_LIBS_DIR}
@@ -216,6 +218,7 @@ add_orbbec_executable(list_depth_work_mode_node tools/list_depth_work_mode.cpp)
add_orbbec_executable(list_camera_profile_mode_node tools/list_camera_profile.cpp)
add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp)
add_orbbec_executable(mega_h26x_decode_node tools/mega_h26x_decode_node.cpp)
add_library(frame_latency SHARED tools/frame_latency.cpp)
@@ -250,6 +253,7 @@ install(TARGETS list_devices_node
list_depth_work_mode_node
list_camera_profile_mode_node
topic_statistics_node
mega_h26x_decode_node
DESTINATION lib/${PROJECT_NAME}/)
if (BUILD_TESTING)
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+2
View File
@@ -393,6 +393,8 @@ class OBCameraNode {
std::map<stream_index_pair, bool> flip_stream_;
std::map<stream_index_pair, std::string> stream_name_;
std::map<stream_index_pair, std::shared_ptr<image_publisher>> image_publishers_;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr>
camera_h26x_publishers_;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
camera_info_publishers_;
orbbec_camera/src/ob_camera_node.cpp
+56 -31
View File
@@ -388,8 +388,8 @@ void OBCameraNode::setupDevices() {
RCLCPP_INFO_STREAM(logger_, "Setting color brightness to " << color_brightness_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BRIGHTNESS_INT, color_brightness_);
}
// ir ae max
if (device_->isPropertySupported(OB_PROP_IR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
// ir ae max
if (device_->isPropertySupported(OB_PROP_IR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting IR AE max exposure to " << ir_ae_max_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_AE_MAX_EXPOSURE_INT, ir_ae_max_exposure_);
}
@@ -546,14 +546,14 @@ void OBCameraNode::setupDepthPostProcessFilter() {
} else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams();
RCLCPP_INFO_STREAM(logger_, "Default noise removal filter params: "
<< "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size);
RCLCPP_INFO_STREAM(
logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size);
params.disp_diff = noise_removal_filter_min_diff_;
params.max_size = noise_removal_filter_max_size_;
RCLCPP_INFO_STREAM(logger_, "Set noise removal filter params: "
<< "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size);
RCLCPP_INFO_STREAM(logger_,
"Set noise removal filter params: " << "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size);
if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
noise_removal_filter->setFilterParams(params);
}
@@ -562,11 +562,11 @@ void OBCameraNode::setupDepthPostProcessFilter() {
hdr_merge_gain_2_ != -1) {
auto hdr_merge_filter = filter->as<ob::HdrMerge>();
hdr_merge_filter->enable(true);
RCLCPP_INFO_STREAM(logger_, "Set HDR merge filter params: "
<< "exposure_1: " << hdr_merge_exposure_1_
<< ", gain_1: " << hdr_merge_gain_1_
<< ", exposure_2: " << hdr_merge_exposure_2_
<< ", gain_2: " << hdr_merge_gain_2_);
RCLCPP_INFO_STREAM(
logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
<< ", gain_1: " << hdr_merge_gain_1_
<< ", exposure_2: " << hdr_merge_exposure_2_
<< ", gain_2: " << hdr_merge_gain_2_);
auto config = OBHdrConfig();
config.enable = true;
config.exposure_1 = hdr_merge_exposure_1_;
@@ -649,10 +649,10 @@ void OBCameraNode::setupProfiles() {
throw std::runtime_error("Failed cast profile to VideoStreamProfile");
}
RCLCPP_DEBUG_STREAM(
logger_, "Sensor profile: "
<< "stream_type: " << magic_enum::enum_name(profile->type())
<< "Format: " << profile->format() << ", Width: " << profile->width()
<< ", Height: " << profile->height() << ", FPS: " << profile->fps());
logger_,
"Sensor profile: " << "stream_type: " << magic_enum::enum_name(profile->type())
<< "Format: " << profile->format() << ", Width: " << profile->width()
<< ", Height: " << profile->height() << ", FPS: " << profile->fps());
supported_profiles_[elem].emplace_back(profile);
}
std::shared_ptr<ob::VideoStreamProfile> selected_profile;
@@ -1263,6 +1263,13 @@ void OBCameraNode::setupPublishers() {
camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>(
topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile));
auto image_h264_qos_profile = getRMWQosProfileFromString(image_qos);
camera_h26x_publishers_[stream_index] =
node_->create_publisher<sensor_msgs::msg::CompressedImage>(
"/camera/color/h26x_encoded_data",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_h264_qos_profile),
image_h264_qos_profile));
if (isGemini335PID(pid)) {
metadata_publishers_[stream_index] =
node_->create_publisher<orbbec_camera_msgs::msg::Metadata>(
@@ -1383,7 +1390,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
return;
}
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
if(!depth_frame_) {
if (!depth_frame_) {
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
return;
}
@@ -1802,6 +1809,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
}
CHECK_NOTNULL(image_publishers_[COLOR]);
bool has_subscriber = image_publishers_[COLOR]->get_subscription_count() > 0;
has_subscriber = has_subscriber || camera_h26x_publishers_[COLOR]->get_subscription_count() > 0;
if (enable_colored_point_cloud_ && depth_registration_cloud_pub_->get_subscription_count() > 0) {
has_subscriber = true;
}
@@ -1838,7 +1846,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
}
}
#endif
if (!is_decoded) {
if (!is_decoded && !(frame->format() == OB_FORMAT_H264 || frame->format() == OB_FORMAT_H265)) {
auto video_frame = softwareDecodeColorFrame(frame);
if (!video_frame) {
RCLCPP_ERROR_STREAM(logger_, "Failed to convert frame to video frame");
@@ -1897,6 +1905,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
has_subscriber =
has_subscriber || (metadata_publishers_.count(stream_index) &&
metadata_publishers_[stream_index]->get_subscription_count() > 0);
has_subscriber =
has_subscriber || camera_h26x_publishers_[stream_index]->get_subscription_count() > 0;
if (!has_subscriber) {
return;
}
@@ -1975,7 +1985,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
publishMetadata(frame, stream_index, camera_info.header);
}
CHECK_NOTNULL(image_publishers_[stream_index]);
if (image_publishers_[stream_index]->get_subscription_count() == 0) {
if (image_publishers_[stream_index]->get_subscription_count() == 0 &&
camera_h26x_publishers_[stream_index]->get_subscription_count() == 0) {
return;
}
auto &image = images_[stream_index];
@@ -1995,18 +2006,32 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
image = image * depth_scale;
}
sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
if (frame->type() == OB_FRAME_COLOR &&
(frame->format() == OB_FORMAT_H264 || frame->format() == OB_FORMAT_H265)) {
sensor_msgs::msg::CompressedImage h264_image_msg;
h264_image_msg.header.stamp = timestamp;
if (frame->format() == OB_FORMAT_H264) {
h264_image_msg.format = "h264";
} else {
h264_image_msg.format = "h265";
}
h264_image_msg.data.resize(video_frame->dataSize());
memcpy(h264_image_msg.data.data(), video_frame->data(), video_frame->dataSize());
camera_h26x_publishers_[stream_index]->publish(std::move(h264_image_msg));
} else {
sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
.toImageMsg(*image_msg);
CHECK_NOTNULL(image_msg.get());
image_msg->header.stamp = timestamp;
image_msg->is_bigendian = false;
image_msg->step = width * unit_step_size_[stream_index];
image_msg->header.frame_id = frame_id;
CHECK(image_publishers_.count(stream_index) > 0);
saveImageToFile(stream_index, image, *image_msg);
image_publishers_[stream_index]->publish(std::move(image_msg));
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
.toImageMsg(*image_msg);
CHECK_NOTNULL(image_msg.get());
image_msg->header.stamp = timestamp;
image_msg->is_bigendian = false;
image_msg->step = width * unit_step_size_[stream_index];
image_msg->header.frame_id = frame_id;
CHECK(image_publishers_.count(stream_index) > 0);
saveImageToFile(stream_index, image, *image_msg);
image_publishers_[stream_index]->publish(std::move(image_msg));
}
if (stream_index == COLOR && enable_color_undistortion_ &&
color_undistortion_publisher_->get_subscription_count() > 0) {
auto undistorted_image = undistortImage(image, intrinsic, distortion);
orbbec_camera/tools/mega_h26x_decode_node.cpp
+145
View File
@@ -0,0 +1,145 @@
extern "C" {
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libswscale/swscale.h>
#include <libavutil/imgutils.h>
#include <libavutil/time.h>
}
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "orbbec_camera/ob_camera_node.h"
#include <thread>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include "orbbec_camera/utils.h"
#include <filesystem>
#include <fstream>
#include "diagnostic_msgs/msg/diagnostic_status.hpp"
#include "libobsensor/hpp/Utils.hpp"
class H264DecoderNode : public rclcpp::Node {
public:
H264DecoderNode() : Node("h264_decoder_node") {
avformat_network_init();
rclcpp::QoS qos_settings(30);
qos_settings.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
qos_settings.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
qos_settings.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST);
compressed_image_subscriber_ = this->create_subscription<sensor_msgs::msg::CompressedImage>(
"/camera/color/h26x_encoded_data", qos_settings,
std::bind(&H264DecoderNode::compressedImageCallback, this, std::placeholders::_1));
rgb_image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>(
"/camera/color/h26x_decoder/image_raw", qos_settings);
}
private:
void decode_init(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
if (msg->format == "h264") {
codec_ = std::shared_ptr<const AVCodec>(avcodec_find_decoder(AV_CODEC_ID_H264),
[](const AVCodec*) {});
codec_context_ = std::shared_ptr<AVCodecContext>(avcodec_alloc_context3(codec_.get()),
[](AVCodecContext* ctx) {
if (ctx) {
avcodec_free_context(&ctx);
}
});
if (avcodec_open2(codec_context_.get(), codec_.get(), nullptr) < 0) {
RCLCPP_ERROR(this->get_logger(), "Failed to open codec");
return;
}
frame_ = std::shared_ptr<AVFrame>(av_frame_alloc(), [](AVFrame* f) {
if (f) av_frame_free(&f);
});
packet_ = std::shared_ptr<AVPacket>(av_packet_alloc(), [](AVPacket* p) {
if (p) av_packet_free(&p);
});
codec_init_ = 0;
} else if (msg->format == "h265") {
codec_ = std::shared_ptr<const AVCodec>(avcodec_find_decoder(AV_CODEC_ID_HEVC),
[](const AVCodec*) {});
codec_context_ = std::shared_ptr<AVCodecContext>(avcodec_alloc_context3(codec_.get()),
[](AVCodecContext* ctx) {
if (ctx) {
avcodec_free_context(&ctx);
}
});
if (avcodec_open2(codec_context_.get(), codec_.get(), nullptr) < 0) {
RCLCPP_ERROR(this->get_logger(), "Failed to open codec");
return;
}
frame_ = std::shared_ptr<AVFrame>(av_frame_alloc(), [](AVFrame* f) {
if (f) av_frame_free(&f);
});
packet_ = std::shared_ptr<AVPacket>(av_packet_alloc(), [](AVPacket* p) {
if (p) av_packet_free(&p);
});
codec_init_ = 0;
}
}
void decode_frame(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
av_packet_unref(packet_.get());
packet_->data = const_cast<uint8_t*>(msg->data.data());
packet_->size = msg->data.size();
std::stringstream ss;
const size_t bytes_to_print = std::min<size_t>(msg->data.size(), 32);
for (size_t i = 0; i < bytes_to_print; ++i) {
ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(msg->data[i]) << " ";
}
RCLCPP_INFO(this->get_logger(), "Data (hex): %s", ss.str().c_str());
send_ret_ = avcodec_send_packet(codec_context_.get(), packet_.get());
if (send_ret_ >= 0) {
receive_ret_ = avcodec_receive_frame(codec_context_.get(), frame_.get());
if (receive_ret_ >= 0) {
cv::Mat rgb_image(frame_->height, frame_->width, CV_8UC3);
SwsContext* sws_context = sws_getContext(frame_->width, frame_->height,
static_cast<AVPixelFormat>(frame_->format),
frame_->width, frame_->height, AV_PIX_FMT_BGR24,
SWS_BILINEAR, nullptr, nullptr, nullptr);
uint8_t* dest[1] = {rgb_image.data};
int linesize[1] = {static_cast<int>(rgb_image.step1())};
sws_scale(sws_context, frame_->data, frame_->linesize, 0, frame_->height, dest, linesize);
sws_freeContext(sws_context);
auto rgb_image_msg =
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb_image).toImageMsg();
rgb_image_msg->header.stamp = this->now();
rgb_image_publisher_->publish(*rgb_image_msg);
}
}
}
void compressedImageCallback(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
RCLCPP_INFO(this->get_logger(), "Format: %s", msg->format.c_str());
if (codec_init_) {
decode_init(msg);
}
decode_frame(msg);
}
rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_image_subscriber_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb_image_publisher_;
std::shared_ptr<const AVCodec> codec_;
std::shared_ptr<AVCodecContext> codec_context_;
std::shared_ptr<AVFrame> frame_;
std::shared_ptr<AVPacket> packet_;
int codec_init_ = 1;
int send_ret_;
int receive_ret_;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<H264DecoderNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}