YangOne/GrabBag
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

优化雷达数据接受，不丢数据

Browse Source
This commit is contained in:
杰仔 2026-06-14 17:01:09 +08:00
parent d0f31edcd2
commit 2b49f41d13
8 changed files with 1261 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Device/RsLidarDevice/Inc/IRsLidarDevice.h
View File

@ -118,7 +118,7 @@ public:
/// 获取驱动版本
virtual std::string GetVersion() = 0;
/// 获取丢帧计数（m_stuffedQueue 满时被丢弃的帧数）
/// 获取包序号缺口/半包异常计数（用于判断接收或组包是否丢数据）
virtual uint64_t GetDroppedFrameCount() const = 0;
/// 获取当前就绪队列深度（待消费的帧数）
@ -127,8 +127,8 @@ public:
/// 获取指定 SDK ErrCode 的累计触发次数（接受原始 SDK error_code，如 0x40/0x48/0x49/0x42）
virtual uint64_t GetExceptionCount(int errCode) const = 0;
/// 获取并清零最近一个 5s 滚动窗口内用户回调耗时统计
/// 注意：内部统计字段每 5s 由 processCloudThread 重置，此接口仅返回当前累计快照
/// 获取 Start 以来用户回调耗时累计统计
/// 注意：设备内部 5s 诊断日志会输出窗口增量，此接口保留累计快照以兼容旧调用
virtual RsCallbackStats GetCallbackLatencyStats() const = 0;
/// 获取就绪队列历史峰值深度（Open 以来）

1016
Device/RsLidarDevice/Src/RsLidarDevice.cpp
View File

File diff suppressed because it is too large Load Diff

107
Device/RsLidarDevice/_Inc/RsLidarDevice.h
View File

@ -33,22 +33,26 @@ inline int recvfrom(SOCKET s, unsigned char* buf, int len, int flags, sockaddr*
#include <mutex>
#include <array>
#include <chrono>
#include <cstddef>
using namespace robosense::lidar;
// 交付帧：封装一次完整扫描的数据 + 行缓冲池（p3DPoint 指向此处）
// processCloudThread 填充 → deliveryThread 消费（调用回调）→ 归还 m_frameFreeQueue
struct RsSdkDiscardPoint
{
};
// 交付帧：封装一次完整扫描的数据 + 连续点池（p3DPoint 指向此处）
// SDK packet callback 填充 → deliveryThread 消费（调用回调）→ 归还 m_frameFreeQueue
struct DeliveryFrame
{
std::vector<std::vector<SVzNLPointXYZI>> linePool;
std::vector<SVzNLPointXYZI> pointPool;
RsCloudData cloudData;
RsFrameInfo info;
void reserveLines(size_t lines, size_t pointsPerLine)
{
linePool.resize(lines);
for (auto& v : linePool)
v.reserve(pointsPerLine);
pointPool.resize(lines * pointsPerLine);
cloudData.reserve(lines);
}
};
using DeliveryFramePtr = std::shared_ptr<DeliveryFrame>;
@ -59,6 +63,10 @@ public:
CRsLidarDevice();
~CRsLidarDevice();
static void* operator new(std::size_t size);
static void operator delete(void* ptr) noexcept;
static void operator delete(void* ptr, std::size_t) noexcept;
int InitDevice() override;
int OpenDevice(const RsLidarConfig& config) override;
int CloseDevice() override;
@ -82,12 +90,9 @@ public:
private:
// SDK 内部类型别名
using SdkCloudMsg = PointCloudT<PointXYZI>;
using SdkCloudMsg = PointCloudT<RsSdkDiscardPoint>;
using SdkCloudPtr = std::shared_ptr<SdkCloudMsg>;
/// 解析线程：pop SDK帧 → NaN→0+变换+拆线 → 推入交付队列 → 归还SDK缓冲
void processCloudThread();
/// 交付线程：从交付队列取帧 → 调用用户回调（PushFrame memcpy在此发生）→ 归还交付帧
void deliveryThread();
@ -100,6 +105,25 @@ private:
/// 将 SDK Packet 转为 RsPacketInfo 后转发给用户
void onPacket(const Packet& pkt);
void resetPacketFrameBuilder();
void initializeRsem4DefaultAngles();
void updateRsem4PitchTrig();
bool processRsem4Packet(const Packet& pkt);
void processRsem4Difop(const uint8_t* data, size_t size);
bool processRsem4Msop(const uint8_t* data, size_t size);
bool processRsem4CompleteMsop(const uint8_t* data, size_t size);
bool processRsem4CompressedMsop(const uint8_t* data, size_t size);
DeliveryFramePtr acquireFrameForBuild();
void resetDeliveryFrame(const DeliveryFramePtr& frame);
void submitActiveFrame();
void resetActiveFrameTracking();
bool beginActiveFrame(bool startedAtBoundary);
bool prepareRsem4FrameForColumn(uint32_t column);
void markRsem4ColumnSegment(uint32_t column, uint8_t segmentMask, uint8_t completeMask);
void fillRsem4Point(uint32_t column, uint32_t ring, float distance,
uint8_t intensity, uint8_t surfaceIndex,
float cosYaw, float sinYaw);
/// 将 SDK Error 转为 RsExceptionInfo 后转发给用户
void onException(const Error& code);
@ -114,24 +138,63 @@ private:
PointCloudCallback m_pointCloudCallback;
PacketCallback m_packetCallback;
ExceptionCallback m_exceptionCallback;
std::atomic<bool> m_hasPacketCallback{false};
std::mutex m_callbackMutex;
std::unique_ptr<LidarDriver<SdkCloudMsg>> m_pDriver;
std::thread m_processThread;
std::thread m_deliveryThread;
SyncQueue<SdkCloudPtr, 4096> m_freeQueue;
SyncQueue<SdkCloudPtr, 4096> m_stuffedQueue;
// 交付帧流水线（深度2=解耦一帧）：processCloud 推入 → deliveryThread 消费
SyncQueue<DeliveryFramePtr, 2> m_deliveryQueue;
SyncQueue<DeliveryFramePtr, 4> m_frameFreeQueue; // 空闲帧池（3帧预分配 + 1余量）
// 交付帧流水线：SDK packet callback 填充 → deliveryThread 消费
SyncQueue<DeliveryFramePtr, 16> m_deliveryQueue;
SyncQueue<DeliveryFramePtr, 16> m_frameFreeQueue;
RSDriverParam m_param;
DeliveryFramePtr m_activeFrame;
bool m_activeFrameHasData = false;
bool m_activeFrameAllValid = true;
bool m_activeFrameStartedAtBoundary = false;
bool m_activeFramePacketLoss = false;
uint32_t m_activeCompleteColumns = 0;
uint8_t m_activeColumnCompleteMask = 0;
std::array<uint8_t, 1200> m_activeColumnMask{};
bool m_hasRsem4PrevColumn = false;
uint32_t m_prevRsem4Column = 0;
bool m_rsem4FrameSynced = false;
bool m_hasRsem4PrevSeq = false;
uint16_t m_prevRsem4Seq = 0;
std::chrono::steady_clock::time_point m_activeFrameStartTime{};
std::chrono::steady_clock::time_point m_diagLastLogTime{};
std::array<int8_t, 26> m_rsem4YawOffset{};
std::array<int16_t, 520> m_rsem4PitchAngle{};
std::array<std::array<float, 520>, 4> m_rsem4PitchSin{};
std::array<std::array<float, 520>, 4> m_rsem4PitchCos{};
std::array<int16_t, 4> m_rsem4SurfacePitchOffset{};
bool m_rsem4AnglesReady = false;
bool m_rsem4HasPartialPacket = false;
uint16_t m_rsem4PartialSeq = 0;
size_t m_rsem4PartialLen = 0;
std::array<uint8_t, 3000> m_rsem4PartialPacket{};
uint32_t m_diagPacketTick = 0;
float m_rsem4MinDistance = 0.5f;
float m_rsem4MaxDistance = 350.0f;
// 丢帧监控
std::atomic<uint64_t> m_droppedFrameCount{0};
std::atomic<uint64_t> m_totalPushCount{0};
std::atomic<uint64_t> m_parseAccumUs{0};
std::atomic<uint64_t> m_parseMaxUs{0};
std::atomic<uint64_t> m_parseCount{0};
std::atomic<uint64_t> m_frameWaitAccumUs{0};
std::atomic<uint64_t> m_frameWaitMaxUs{0};
std::atomic<uint64_t> m_frameWaitCount{0};
std::atomic<uint64_t> m_frameWaitTimeoutCount{0};
std::atomic<uint64_t> m_deliveryDropCount{0};
std::atomic<uint64_t> m_deliveredFrameCount{0};
std::atomic<size_t> m_deliveryQueuePeak{0};
// 诊断统计（每 5s 滚动窗口）
// bucket: 0=MSOPTIMEOUT(0x40) 1=PKTBUFOVERFLOW(0x48) 2=CLOUDOVERFLOW(0x49) 3=WRONGMSOPLEN(0x42) 4=other
@ -140,7 +203,19 @@ private:
std::atomic<uint64_t> m_cbMaxUs{0};
std::atomic<uint64_t> m_cbAccumUs{0};
std::atomic<uint64_t> m_cbCount{0};
std::atomic<size_t> m_qPeak{0};
uint64_t m_diagLastPushCount = 0;
uint64_t m_diagLastQueueDropCount = 0;
uint64_t m_diagLastParseCount = 0;
uint64_t m_diagLastParseAccumUs = 0;
uint64_t m_diagLastFrameWaitCount = 0;
uint64_t m_diagLastFrameWaitAccumUs = 0;
uint64_t m_diagLastFrameWaitTimeoutCount = 0;
uint64_t m_diagLastDeliveryDropCount = 0;
uint64_t m_diagLastDeliveredFrameCount = 0;
uint64_t m_diagLastCbCount = 0;
uint64_t m_diagLastCbAccumUs = 0;
std::array<uint64_t, EXC_BUCKET_COUNT> m_diagLastExceptionCounts{};
/// 将 SDK ErrCode 映射到统计桶下标，未知返回 4 (other)
static size_t errCodeToBucket(int errCode);

133
Module/FFMediaStream/Src/CVrFFMediaPuller.cpp
View File

@ -2,6 +2,7 @@
#include "VrError.h"
#include "VrLog.h"
#include <cctype>
#include <cstring>
#if !defined(FFMEDIA_PLATFORM_DUMMY)
@ -73,6 +74,23 @@ int CVrFFMediaPuller::UnInit()
// ---- 辅助函数 ----
namespace
{
bool startsWithNoCase(const std::string& s, const char* prefix)
{
const size_t n = std::strlen(prefix);
if (s.size() < n) return false;
for (size_t i = 0; i < n; ++i)
{
const unsigned char a = static_cast<unsigned char>(s[i]);
const unsigned char b = static_cast<unsigned char>(prefix[i]);
if (std::tolower(a) != std::tolower(b))
return false;
}
return true;
}
}
MppCodingType CVrFFMediaPuller::AvCodecIdToMppCoding(AVCodecID id)
{
switch (id)
@ -108,11 +126,67 @@ unsigned int CVrFFMediaPuller::PixelBpp(EFFPullPixelFormat fmt)
// ---- MPP 解码器初始化 ----
int CVrFFMediaPuller::initBitstreamFilter()
{
if (!m_fmtCtx || m_videoStreamIndex < 0) return SUCCESS;
AVStream* stream = m_fmtCtx->streams[m_videoStreamIndex];
if (!stream || !stream->codecpar) return SUCCESS;
const char* filterName = nullptr;
switch (stream->codecpar->codec_id)
{
case AV_CODEC_ID_H264:
filterName = "h264_mp4toannexb";
break;
case AV_CODEC_ID_HEVC:
filterName = "hevc_mp4toannexb";
break;
default:
return SUCCESS;
}
const AVBitStreamFilter* filter = av_bsf_get_by_name(filterName);
if (!filter)
{
LOG_ERROR("FFMediaPuller: bitstream filter not found %s\n", filterName);
return ERR_CODE(DEV_OPEN_ERR);
}
int ret = av_bsf_alloc(filter, &m_bsfCtx);
if (ret < 0 || !m_bsfCtx)
{
LOG_ERROR("FFMediaPuller: av_bsf_alloc fail %d (%s)\n", ret, filterName);
return ERR_CODE(DEV_OPEN_ERR);
}
ret = avcodec_parameters_copy(m_bsfCtx->par_in, stream->codecpar);
if (ret < 0)
{
LOG_ERROR("FFMediaPuller: avcodec_parameters_copy fail %d\n", ret);
av_bsf_free(&m_bsfCtx);
return ERR_CODE(DEV_OPEN_ERR);
}
m_bsfCtx->time_base_in = stream->time_base;
ret = av_bsf_init(m_bsfCtx);
if (ret < 0)
{
LOG_ERROR("FFMediaPuller: av_bsf_init fail %d (%s)\n", ret, filterName);
av_bsf_free(&m_bsfCtx);
return ERR_CODE(DEV_OPEN_ERR);
}
LOG_INFO("FFMediaPuller: bitstream filter enabled %s\n", filterName);
return SUCCESS;
}
int CVrFFMediaPuller::initMppDecoder()
{
if (!m_fmtCtx) return ERR_CODE(DEV_OPEN_ERR);
AVCodecParameters* codecpar = m_fmtCtx->streams[m_videoStreamIndex]->codecpar;
AVCodecParameters* codecpar = m_bsfCtx ? m_bsfCtx->par_out
: m_fmtCtx->streams[m_videoStreamIndex]->codecpar;
MppCodingType coding = AvCodecIdToMppCoding(codecpar->codec_id);
// 1) 创建 MPP 上下文
@ -253,6 +327,23 @@ void CVrFFMediaPuller::decodeLoop()
continue;
}
if (m_bsfCtx)
{
ret = av_bsf_send_packet(m_bsfCtx, avpkt);
av_packet_unref(avpkt);
if (ret < 0)
continue;
ret = av_bsf_receive_packet(m_bsfCtx, avpkt);
if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF)
continue;
if (ret < 0)
{
av_packet_unref(avpkt);
continue;
}
}
// 1) 构造 MppPacket
MppPacket mppPkt = nullptr;
mpp_packet_init(&mppPkt, avpkt->data, static_cast<size_t>(avpkt->size));
@ -378,22 +469,25 @@ int CVrFFMediaPuller::Init(const VrFFPullConfig& config)
avformat_network_init();
av_log_set_level(AV_LOG_ERROR);
// 2) 打开 RTSP 流
// 2) 打开输入流
AVDictionary* opts = nullptr;
if (config.useTcp)
const bool isRtsp = startsWithNoCase(config.rtspUrl, "rtsp://");
const bool isRtmp = startsWithNoCase(config.rtspUrl, "rtmp://");
if (isRtsp)
{
av_dict_set(&opts, "rtsp_transport", "tcp", 0);
av_dict_set(&opts, "rtsp_transport", config.useTcp ? "tcp" : "udp", 0);
av_dict_set(&opts, "timeout", "3000000", 0);
av_dict_set(&opts, "stimeout", "3000000", 0);
}
else
if (isRtmp)
{
av_dict_set(&opts, "rtsp_transport", "udp", 0);
av_dict_set(&opts, "rtmp_live", "live", 0);
}
// socket 读超时（微秒）。FFmpeg 6.x RTSP demuxer 用 "timeout"；
// 同时设置旧名 "stimeout" 以兼容（未知项被忽略，无害）。
av_dict_set(&opts, "timeout", "5000000", 0);
av_dict_set(&opts, "stimeout", "5000000", 0);
// 降低拉流端到端延迟
av_dict_set(&opts, "max_delay", "500000", 0);
av_dict_set(&opts, "rw_timeout", "3000000", 0);
av_dict_set(&opts, "probesize", "32768", 0);
av_dict_set(&opts, "analyzeduration", "100000", 0);
av_dict_set(&opts, "fflags", "nobuffer", 0);
av_dict_set(&opts, "max_delay", "100000", 0);
int ret = avformat_open_input(&m_fmtCtx, config.rtspUrl.c_str(), nullptr, &opts);
av_dict_free(&opts);
@ -441,11 +535,20 @@ int CVrFFMediaPuller::Init(const VrFFPullConfig& config)
m_decHorStride = codecpar->width; // 先猜测，info_change 后会更新
m_decVerStride = codecpar->height;
ret = initBitstreamFilter();
if (ret != SUCCESS)
{
LOG_ERROR("FFMediaPuller: initBitstreamFilter fail\n");
avformat_close_input(&m_fmtCtx);
return ret;
}
// 6) 初始化 MPP 解码器
ret = initMppDecoder();
if (ret != SUCCESS)
{
LOG_ERROR("FFMediaPuller: initMppDecoder fail\n");
av_bsf_free(&m_bsfCtx);
avformat_close_input(&m_fmtCtx);
return ret;
}
@ -455,6 +558,7 @@ int CVrFFMediaPuller::Init(const VrFFPullConfig& config)
if (ret != SUCCESS)
{
LOG_ERROR("FFMediaPuller: initSwscale fail\n");
av_bsf_free(&m_bsfCtx);
avformat_close_input(&m_fmtCtx);
return ret;
}
@ -515,6 +619,11 @@ int CVrFFMediaPuller::UnInit()
m_fmtCtx = nullptr;
}
if (m_bsfCtx)
{
av_bsf_free(&m_bsfCtx);
}
// 销毁 MPP 解码器上下文
if (m_mppCtx)
{

190
Module/FFMediaStream/Src/CVrFFMediaPusher.cpp
View File

@ -5,6 +5,7 @@
#include <algorithm>
#include <chrono>
#include <cstring>
#include <limits>
#if !defined(FFMEDIA_PLATFORM_DUMMY)
#include <linux/videodev2.h>
@ -130,8 +131,13 @@ int CVrFFMediaPusher::initMppEncoder()
MppCodingType coding = ToMppCoding(m_config.encodeType);
ret = m_mppApi->control(m_mppCtx, MPP_SET_INPUT_TIMEOUT, nullptr);
ret = m_mppApi->control(m_mppCtx, MPP_SET_OUTPUT_TIMEOUT, nullptr);
RK_S64 timeout = 1000;
ret = m_mppApi->control(m_mppCtx, MPP_SET_INPUT_TIMEOUT, &timeout);
if (ret != MPP_SUCCESS)
LOG_WARN("FFMediaPusher: MPP_SET_INPUT_TIMEOUT fail %d\n", ret);
ret = m_mppApi->control(m_mppCtx, MPP_SET_OUTPUT_TIMEOUT, &timeout);
if (ret != MPP_SUCCESS)
LOG_WARN("FFMediaPusher: MPP_SET_OUTPUT_TIMEOUT fail %d\n", ret);
ret = mpp_init(m_mppCtx, MPP_CTX_ENC, coding);
if (ret != MPP_SUCCESS)
@ -229,19 +235,19 @@ int CVrFFMediaPusher::initMppEncoder()
return SUCCESS;
}
// ---- RTSP 输出初始化 ----
// ---- RTMP 输出初始化 ----
int CVrFFMediaPusher::initRtspOutput()
{
// 1) 构建 RTSP URL: 主动推流到 mediamtx (rtsp://{ip}:{port}{path})
// 不再使用 0.0.0.0:listen 模式，而是推到 mediamtx 的接收端口
// 1) 构建 RTMP URL: 主动推流到 mediamtx (rtmp://{ip}:{port}{path})
// RTMP 比 RTSP 更稳定，避免 interleaved frame 等兼容性问题
char url[256];
snprintf(url, sizeof(url), "rtsp://%s:%u%s",
snprintf(url, sizeof(url), "rtmp://%s:%u%s",
m_config.rtspHost.empty() ? "127.0.0.1" : m_config.rtspHost.c_str(),
m_config.rtspPort, m_config.rtspPath.c_str());
// 2) 创建 RTSP 输出上下文
int ret = avformat_alloc_output_context2(&m_fmtCtx, nullptr, "rtsp", url);
// 2) 创建 RTMP 输出上下文（格式改为 flv，RTMP 基于 FLV 容器）
int ret = avformat_alloc_output_context2(&m_fmtCtx, nullptr, "flv", url);
if (ret < 0 || !m_fmtCtx)
{
LOG_ERROR("FFMediaPusher: avformat_alloc_output_context2 fail %d\n", ret);
@ -271,6 +277,8 @@ int CVrFFMediaPusher::initRtspOutput()
codecpar->width = m_encWidth;
codecpar->height = m_encHeight;
codecpar->format = AV_PIX_FMT_YUV420P; // NV12 对应的 FFmpeg 格式
codecpar->bit_rate = static_cast<int64_t>(m_config.bitrateKbps) * 1000; // RTMP muxer 需要知道码率
// 设置流时基为微秒（PushFrame 以微秒传入 PTS，写帧时再 rescale 到 muxer 实际时基）
stream->time_base = AVRational{1, 1000000};
@ -397,15 +405,25 @@ int CVrFFMediaPusher::Start()
if (!m_bInited.load()) return ERR_CODE(DEV_NO_OPEN);
if (m_bStarted.load()) return SUCCESS;
// 主动推流到 mediamtx：直接调用 avformat_write_header（不再开线程等 listen）
AVDictionary* opts = nullptr;
av_dict_set(&opts, "rtsp_transport", "tcp", 0);
int ret = avformat_write_header(m_fmtCtx, &opts);
av_dict_free(&opts);
// RTMP 需要先打开 IO（RTSP 不需要，但 RTMP/FLV 必须）
if (!(m_fmtCtx->oformat->flags & AVFMT_NOFILE))
{
int ret = avio_open(&m_fmtCtx->pb, m_fmtCtx->url, AVIO_FLAG_WRITE);
if (ret < 0)
{
LOG_ERROR("FFMediaPusher: avio_open fail %d url=%s\n", ret, m_fmtCtx->url);
return ERR_CODE(DEV_OPEN_ERR);
}
}
// 写 RTMP header
int ret = avformat_write_header(m_fmtCtx, nullptr);
if (ret < 0)
{
LOG_ERROR("FFMediaPusher: avformat_write_header fail %d (mediamtx 未运行或端口不对？)\n", ret);
if (m_fmtCtx->pb)
avio_closep(&m_fmtCtx->pb);
return ERR_CODE(DEV_OPEN_ERR);
}
@ -423,12 +441,7 @@ int CVrFFMediaPusher::PushFrame(const void* data, size_t size, int64_t ptsUs)
if (!data || size == 0)
return ERR_CODE(DEV_ARG_INVAILD);
// 生成 PTS
if (ptsUs == 0)
{
auto now = std::chrono::steady_clock::now().time_since_epoch();
ptsUs = std::chrono::duration_cast<std::chrono::microseconds>(now).count();
}
// 使用调用方传入的 PTS（由 DroneScrewServerPresenter 按帧计数递增生成）
m_lastPtsUs = ptsUs;
// 1) 分配 MPP 输入缓冲区（hor_stride × ver_stride 对齐布局），直接把数据写进去
@ -446,6 +459,7 @@ int CVrFFMediaPusher::PushFrame(const void* data, size_t size, int64_t ptsUs)
mpp_buffer_put(mppBuf);
return ERR_CODE(DEV_CTRL_ERR);
}
mpp_buffer_sync_begin(mppBuf);
uint8_t* dstY = dst; // Y 平面
uint8_t* dstUV = dst + m_encHorStride * m_encVerStride; // UV 平面（按 ver_stride 偏移）
@ -493,6 +507,8 @@ int CVrFFMediaPusher::PushFrame(const void* data, size_t size, int64_t ptsUs)
srcUV + static_cast<size_t>(y) * srcLn, copyW);
}
mpp_buffer_sync_end(mppBuf);
// 2) 构建 MppFrame
MppFrame frame = nullptr;
mpp_frame_init(&frame);
@ -504,39 +520,126 @@ int CVrFFMediaPusher::PushFrame(const void* data, size_t size, int64_t ptsUs)
mpp_frame_set_buffer(frame, mppBuf);
mpp_frame_set_pts(frame, ptsUs);
// 3) 同步编码
// 3) 编码。H.264/H.265 在当前 MPP 版本上需要显式提供输出 packet buffer，
// 否则 encode() 可能持续返回 success + null packet，RTMP 只有 header 没有媒体数据。
MppPacket packet = nullptr;
mret = m_mppApi->encode(m_mppCtx, frame, &packet);
mpp_frame_deinit(&frame);
mpp_buffer_put(mppBuf);
MppPacket outPacket = nullptr;
MppBuffer pktBuf = nullptr;
const size_t pktBufSize = static_cast<size_t>(frameSize);
if (mret != MPP_SUCCESS || !packet)
mret = mpp_buffer_get(m_mppBufGroup, &pktBuf, pktBufSize);
if (mret != MPP_SUCCESS || !pktBuf)
{
LOG_DEBUG("FFMediaPusher: encode fail %d\n", mret);
mpp_frame_deinit(&frame);
mpp_buffer_put(mppBuf);
LOG_ERROR("FFMediaPusher: packet buffer get fail %d\n", mret);
return ERR_CODE(DEV_CTRL_ERR);
}
void* pktData = mpp_packet_get_data(packet);
size_t pktSize = mpp_packet_get_length(packet);
mret = mpp_packet_init_with_buffer(&packet, pktBuf);
if (mret != MPP_SUCCESS || !packet)
{
mpp_frame_deinit(&frame);
mpp_buffer_put(mppBuf);
mpp_buffer_put(pktBuf);
LOG_ERROR("FFMediaPusher: packet init fail %d\n", mret);
return ERR_CODE(DEV_CTRL_ERR);
}
mpp_packet_set_length(packet, 0);
mpp_packet_set_pos(packet, mpp_packet_get_data(packet));
if (!pktData || pktSize == 0)
MppMeta meta = mpp_frame_get_meta(frame);
if (meta)
mpp_meta_set_packet(meta, KEY_OUTPUT_PACKET, packet);
mret = m_mppApi->encode_put_frame(m_mppCtx, frame);
mpp_frame_deinit(&frame);
mpp_buffer_put(mppBuf);
if (mret != MPP_SUCCESS)
{
mpp_packet_deinit(&packet);
return SUCCESS; // 编码器可能吞帧（如缓冲未就绪），正常情况
mpp_buffer_put(pktBuf);
LOG_DEBUG("FFMediaPusher: encode_put_frame fail %d\n", mret);
return ERR_CODE(DEV_CTRL_ERR);
}
mret = m_mppApi->encode_get_packet(m_mppCtx, &outPacket);
if (mret != MPP_SUCCESS)
{
mpp_packet_deinit(&packet);
mpp_buffer_put(pktBuf);
LOG_DEBUG("FFMediaPusher: encode_get_packet fail %d\n", mret);
return ERR_CODE(DEV_CTRL_ERR);
}
MppPacket encodedPacket = outPacket ? outPacket : packet;
// 编码器预热期可能无输出，跳过本帧（返回 SUCCESS，不算失败）
if (!encodedPacket || mpp_packet_get_length(encodedPacket) == 0)
{
mpp_packet_deinit(&packet);
mpp_buffer_put(pktBuf);
LOG_DEBUG("FFMediaPusher: encode produced no packet (warmup)\n");
return SUCCESS; // 跳过，不算失败
}
MppBuffer encodedBuf = mpp_packet_get_buffer(encodedPacket);
if (encodedBuf)
mpp_buffer_sync_ro_begin(encodedBuf);
auto releaseEncodedPacket = [&]() {
if (encodedBuf)
{
mpp_buffer_sync_ro_end(encodedBuf);
encodedBuf = nullptr;
}
if (outPacket && outPacket != packet)
mpp_packet_deinit(&outPacket);
if (packet)
mpp_packet_deinit(&packet);
if (pktBuf)
{
mpp_buffer_put(pktBuf);
pktBuf = nullptr;
}
};
void* pktData = mpp_packet_get_pos(encodedPacket);
if (!pktData)
pktData = mpp_packet_get_data(encodedPacket);
size_t pktSize = mpp_packet_get_length(encodedPacket);
// 双重检查：确保有效数据（理论上前面已检查过，这里是保险）
if (!pktData || pktSize == 0)
{
releaseEncodedPacket();
LOG_DEBUG("FFMediaPusher: encode produced empty packet data\n");
return SUCCESS; // 跳过，不算失败
}
// 4) 写入 RTSP（独立 AVPacket，pts rescale 到 muxer 时基）
AVPacket* avpkt = av_packet_alloc();
if (!avpkt)
{
mpp_packet_deinit(&packet);
releaseEncodedPacket();
return ERR_CODE(DEV_CTRL_ERR);
}
avpkt->data = static_cast<uint8_t*>(pktData);
avpkt->size = static_cast<int>(pktSize);
if (pktSize > static_cast<size_t>(std::numeric_limits<int>::max()) ||
av_new_packet(avpkt, static_cast<int>(pktSize)) < 0)
{
av_packet_free(&avpkt);
releaseEncodedPacket();
return ERR_CODE(DEV_CTRL_ERR);
}
memcpy(avpkt->data, pktData, pktSize);
avpkt->stream_index = 0;
avpkt->pts = ptsUs;
avpkt->dts = ptsUs;
avpkt->duration = m_config.fps > 0
? static_cast<int64_t>(1000000 / m_config.fps)
: 0;
// 判断关键帧（I/IDR 帧）
if (pktSize >= 5)
@ -561,6 +664,8 @@ int CVrFFMediaPusher::PushFrame(const void* data, size_t size, int64_t ptsUs)
}
}
releaseEncodedPacket();
int aret;
{
std::lock_guard<std::mutex> wlk(m_writeMutex);
@ -569,7 +674,13 @@ int CVrFFMediaPusher::PushFrame(const void* data, size_t size, int64_t ptsUs)
// 微秒 → muxer 实际时基
av_packet_rescale_ts(avpkt, AVRational{1, 1000000},
m_fmtCtx->streams[0]->time_base);
aret = av_interleaved_write_frame(m_fmtCtx, avpkt);
aret = av_interleaved_write_frame(m_fmtCtx, avpkt); // 使用 interleaved，处理时间戳排序
// RTMP 推流需要强制刷新缓冲区，否则数据会积压不发送
if (aret >= 0 && m_fmtCtx->pb)
{
avio_flush(m_fmtCtx->pb);
}
}
else
{
@ -578,7 +689,6 @@ int CVrFFMediaPusher::PushFrame(const void* data, size_t size, int64_t ptsUs)
}
av_packet_free(&avpkt);
mpp_packet_deinit(&packet);
if (aret < 0)
{
@ -596,12 +706,18 @@ int CVrFFMediaPusher::Stop()
// 先置 started=false，使并发的 PushFrame 尽快早退
m_bStarted = false;
// 主动推流模式无 accept 线程，直接发送 trailer（与 PushFrame 写帧互斥）
// 发送 trailer 并关闭 IO（与 PushFrame 写帧互斥）
{
std::lock_guard<std::mutex> wlk(m_writeMutex);
if (m_headerWritten.load() && m_fmtCtx)
{
av_write_trailer(m_fmtCtx); // RTSP muxer 为 AVFMT_NOFILE，内部自动关闭连接
av_write_trailer(m_fmtCtx);
// RTMP 需要手动关闭 IO（RTSP 不需要，AVFMT_NOFILE）
if (m_fmtCtx->pb)
{
avio_closep(&m_fmtCtx->pb);
}
}
m_headerWritten = false;
}
@ -661,7 +777,7 @@ int CVrFFMediaPusher::UnInit()
std::string CVrFFMediaPusher::GetRtspUrl(const std::string& localIp) const
{
char buf[256];
snprintf(buf, sizeof(buf), "rtsp://%s:%u%s",
snprintf(buf, sizeof(buf), "rtmp://%s:%u%s",
localIp.c_str(), m_config.rtspPort, m_config.rtspPath.c_str());
return std::string(buf);
}

4
Module/FFMediaStream/_Inc/CVrFFMediaPuller.h
View File

@ -16,6 +16,8 @@
#include "mpp_buffer.h"
extern "C" {
#include <libavcodec/avcodec.h>
#include <libavcodec/bsf.h>
#include <libavformat/avformat.h>
#include <libswscale/swscale.h>
#include <libavutil/imgutils.h>
@ -50,6 +52,7 @@ private:
// FFmpeg RTSP 输入
AVFormatContext* m_fmtCtx{nullptr};
AVBSFContext* m_bsfCtx{nullptr};
int m_videoStreamIndex{-1};
// 解码器输出尺寸（从 info_change 获取）
@ -76,6 +79,7 @@ private:
static MppCodingType AvCodecIdToMppCoding(AVCodecID id);
static int ToAvPixelFormat(EFFPullPixelFormat fmt);
static unsigned int PixelBpp(EFFPullPixelFormat fmt);
int initBitstreamFilter();
int initMppDecoder();
int initSwscale();
void decodeLoop();

15
SDK/Lidar/rslidar/rs_driver/src/rs_driver/driver/decoder/decoder_RSEM4.hpp
View File

@ -396,6 +396,14 @@ inline bool DecoderRSEM4<T_PointCloud>::decodeCompPkt(const uint8_t* packet, siz
this->first_point_ts_ = pkt_ts;
ret = true;
}
using PointT = typename T_PointCloud::PointT;
if (!RS_HAS_MEMBER(PointT, x))
{
this->prev_point_ts_ = pkt_ts;
this->prev_pkt_ts_ = pkt_ts;
packet_info_.clear();
return ret;
}
rleDecodeMethod(packet, size, pkt_ts);
packet_info_.clear();
return ret;
@ -546,6 +554,13 @@ inline bool DecoderRSEM4<T_PointCloud>::decodeGeneralPkt(const uint8_t* packet,
this->first_point_ts_ = pkt_ts;
ret = true;
}
using PointT = typename T_PointCloud::PointT;
if (!RS_HAS_MEMBER(PointT, x))
{
this->prev_point_ts_ = pkt_ts;
this->prev_pkt_ts_ = pkt_ts;
return ret;
}
const uint16_t blocks_per_pkt = this->const_param_.BLOCKS_PER_PKT;
const uint16_t channels_per_block = this->const_param_.CHANNELS_PER_BLOCK;
const float distance_res = this->const_param_.DISTANCE_RES;

20
Test/RsLidarTest/main.cpp
View File

@ -156,7 +156,15 @@ static void StatsLoop()
<< " | pps=" << std::fixed << std::setprecision(0)
<< (static_cast<double>(g_nTotalPoints) / elapsed);
if (g_pDevice)
oss << " | drop=" << g_pDevice->GetDroppedFrameCount();
{
const auto cb = g_pDevice->GetCallbackLatencyStats();
oss << " | drop=" << g_pDevice->GetDroppedFrameCount()
<< " | q=" << g_pDevice->GetStuffedQueueDepth()
<< "/" << g_pDevice->GetStuffedQueuePeak()
<< " | cb_us=" << cb.avgUs << "/" << cb.maxUs
<< " | exc48=" << g_pDevice->GetExceptionCount(0x48)
<< " | exc49=" << g_pDevice->GetExceptionCount(0x49);
}
PrintLog(oss.str());
}
}
@ -236,7 +244,8 @@ int main(int argc, char* argv[])
// 4. 注册回调（PointCloudCallback：Device 已完成 NaN→0 + 坐标变换 + 米→毫米）
pDevice->SetPointCloudCallback(OnPointCloud);
pDevice->SetPacketCallback(OnPacket);
if (g_bVerbose)
pDevice->SetPacketCallback(OnPacket);
pDevice->SetExceptionCallback(OnException);
// 5. 启动
@ -250,10 +259,11 @@ int main(int argc, char* argv[])
// 7. 显示窗口
if (g_bDisplay && g_pCloudShow)
{
if (g_pCloudShow->Start("LiDAR PointCloud v1.0.2") == 0)
if (g_pCloudShow->Start("LiDAR PointCloud v1.1.1") == 0)
PrintLog("PCL 窗口已打开 — START=连续保存 S=单帧 Q=退出");
else
{ PrintLog("PCL 窗口创建失败，继续无显示运行"); g_bDisplay = false; }
else{
PrintLog("PCL 窗口创建失败，继续无显示运行"); g_bDisplay = false;
}
}
// 8. 主循环