410 lines
14 KiB
C++
410 lines
14 KiB
C++
#include "core/GVSPParser.h"
|
||
#include <QDebug>
|
||
#include <QtConcurrent>
|
||
#include <cstring>
|
||
#include <winsock2.h>
|
||
|
||
GVSPParser::GVSPParser(QObject *parent)
|
||
: QObject(parent)
|
||
, m_lastBlockId(0)
|
||
, m_imageSequence(0)
|
||
{
|
||
m_imageProcessingCount.storeRelaxed(0);
|
||
}
|
||
|
||
GVSPParser::~GVSPParser()
|
||
{
|
||
}
|
||
|
||
void GVSPParser::reset()
|
||
{
|
||
m_leftIRState = StreamState();
|
||
m_rightIRState = StreamState();
|
||
m_rgbState = StreamState();
|
||
m_depthState = StreamState();
|
||
m_pointCloudState = StreamState();
|
||
m_imageSequence = 0;
|
||
}
|
||
|
||
void GVSPParser::parsePacket(const QByteArray &packet)
|
||
{
|
||
static int debugCount = 0;
|
||
|
||
if (packet.size() < sizeof(GVSPPacketHeader)) {
|
||
return;
|
||
}
|
||
|
||
const uint8_t *data = reinterpret_cast<const uint8_t*>(packet.constData());
|
||
const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);
|
||
|
||
// GigE Vision 2.1 标准 GVSP 包头解析
|
||
uint16_t blockId = ntohs(header->block_id);
|
||
uint8_t packetType = header->packet_format;
|
||
uint16_t packetId = ntohs(header->packet_id);
|
||
|
||
// 打印前5个包的详细信息
|
||
if (debugCount < 5) {
|
||
// qDebug() << "[GVSPParser] Packet" << debugCount
|
||
// << "Type:" << packetType
|
||
// << "BlockID:" << blockId
|
||
// << "PacketID:" << packetId
|
||
// << "Size:" << packet.size();
|
||
debugCount++;
|
||
}
|
||
|
||
switch (packetType) {
|
||
case GVSP_LEADER_PACKET:
|
||
handleLeaderPacket(data, packet.size());
|
||
break;
|
||
case GVSP_PAYLOAD_PACKET:
|
||
handlePayloadPacket(data, packet.size());
|
||
break;
|
||
case GVSP_TRAILER_PACKET:
|
||
handleTrailerPacket(data, packet.size());
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
|
||
{
|
||
// 最小大小检查：至少要有包头 + 2字节reserved + 2字节payload_type
|
||
if (size < sizeof(GVSPPacketHeader) + 4) {
|
||
qDebug() << "[GVSPParser] Leader packet too small:" << size << "bytes";
|
||
return;
|
||
}
|
||
|
||
const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);
|
||
uint32_t blockId = ntohs(header->block_id);
|
||
|
||
// Check payload type
|
||
const uint16_t *payload_type_ptr = reinterpret_cast<const uint16_t*>(data + sizeof(GVSPPacketHeader) + 2);
|
||
uint16_t payload_type = ntohs(*payload_type_ptr);
|
||
|
||
if (payload_type == PAYLOAD_TYPE_IMAGE) {
|
||
// Image data leader
|
||
if (size < sizeof(GVSPPacketHeader) + sizeof(GVSPImageDataLeader)) {
|
||
qDebug() << "[GVSPParser] Image leader too small";
|
||
return;
|
||
}
|
||
const GVSPImageDataLeader *leader = reinterpret_cast<const GVSPImageDataLeader*>(data + sizeof(GVSPPacketHeader));
|
||
|
||
uint32_t imageWidth = ntohl(leader->size_x);
|
||
uint32_t imageHeight = ntohl(leader->size_y);
|
||
uint32_t pixelFormat = ntohl(leader->pixel_format);
|
||
|
||
// 根据像素格式选择对应的状态
|
||
StreamState *state = nullptr;
|
||
if (pixelFormat == PIXEL_FORMAT_MONO16_LEFT || pixelFormat == PIXEL_FORMAT_MONO8_LEFT) {
|
||
state = &m_leftIRState;
|
||
} else if (pixelFormat == PIXEL_FORMAT_MONO16_RIGHT || pixelFormat == PIXEL_FORMAT_MONO8_RIGHT) {
|
||
state = &m_rightIRState;
|
||
} else if (pixelFormat == PIXEL_FORMAT_MJPEG) {
|
||
state = &m_rgbState;
|
||
} else if (pixelFormat == PIXEL_FORMAT_MONO16 || pixelFormat == PIXEL_FORMAT_12BIT_GRAY) {
|
||
// Legacy格式：根据序号分配
|
||
int imageType = m_imageSequence % 2;
|
||
state = (imageType == 0) ? &m_leftIRState : &m_rightIRState;
|
||
}
|
||
|
||
if (state) {
|
||
state->blockId = blockId;
|
||
state->imageWidth = imageWidth;
|
||
state->imageHeight = imageHeight;
|
||
state->pixelFormat = pixelFormat;
|
||
|
||
// 根据pixel format设置期望大小
|
||
if (pixelFormat == PIXEL_FORMAT_MJPEG) {
|
||
// MJPEG是压缩格式，实际大小未知，设置为0表示动态接收
|
||
state->expectedSize = 0;
|
||
} else if (pixelFormat == PIXEL_FORMAT_MONO8_LEFT || pixelFormat == PIXEL_FORMAT_MONO8_RIGHT) {
|
||
// 8-bit灰度格式（下采样）
|
||
state->expectedSize = imageWidth * imageHeight;
|
||
} else {
|
||
// 16-bit或12-bit灰度等固定格式
|
||
state->expectedSize = imageWidth * imageHeight * 2;
|
||
}
|
||
|
||
state->dataBuffer.clear();
|
||
if (state->expectedSize > 0) {
|
||
state->dataBuffer.reserve(state->expectedSize);
|
||
}
|
||
state->receivedSize = 0;
|
||
state->isReceiving = true;
|
||
state->packetCount = 0;
|
||
}
|
||
}
|
||
else if (payload_type == PAYLOAD_TYPE_BINARY) {
|
||
// Depth data leader
|
||
const GVSPBinaryDataLeader *leader = reinterpret_cast<const GVSPBinaryDataLeader*>(data + sizeof(GVSPPacketHeader));
|
||
|
||
m_depthState.blockId = blockId;
|
||
m_depthState.expectedSize = ntohl(leader->file_size);
|
||
m_depthState.dataBuffer.clear();
|
||
m_depthState.dataBuffer.reserve(m_depthState.expectedSize);
|
||
m_depthState.receivedSize = 0;
|
||
m_depthState.isReceiving = true;
|
||
m_depthState.packetCount = 0;
|
||
}
|
||
else if (payload_type == PAYLOAD_TYPE_POINTCLOUD) {
|
||
// Point cloud data leader (vendor-specific 0x8000)
|
||
const GVSPPointCloudDataLeader *leader = reinterpret_cast<const GVSPPointCloudDataLeader*>(data + sizeof(GVSPPacketHeader));
|
||
|
||
m_pointCloudState.blockId = blockId;
|
||
m_pointCloudState.expectedSize = ntohl(leader->data_size);
|
||
m_pointCloudState.dataBuffer.clear();
|
||
m_pointCloudState.dataBuffer.reserve(m_pointCloudState.expectedSize);
|
||
m_pointCloudState.receivedSize = 0;
|
||
m_pointCloudState.isReceiving = true;
|
||
m_pointCloudState.packetCount = 0;
|
||
}
|
||
}
|
||
|
||
void GVSPParser::handlePayloadPacket(const uint8_t *data, size_t size)
|
||
{
|
||
if (size < sizeof(GVSPPacketHeader)) {
|
||
return;
|
||
}
|
||
|
||
const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);
|
||
uint32_t blockId = ntohs(header->block_id);
|
||
|
||
// 查找匹配的状态
|
||
StreamState *state = nullptr;
|
||
if (m_leftIRState.isReceiving && m_leftIRState.blockId == blockId) {
|
||
state = &m_leftIRState;
|
||
} else if (m_rightIRState.isReceiving && m_rightIRState.blockId == blockId) {
|
||
state = &m_rightIRState;
|
||
} else if (m_rgbState.isReceiving && m_rgbState.blockId == blockId) {
|
||
state = &m_rgbState;
|
||
} else if (m_depthState.isReceiving && m_depthState.blockId == blockId) {
|
||
state = &m_depthState;
|
||
} else if (m_pointCloudState.isReceiving && m_pointCloudState.blockId == blockId) {
|
||
state = &m_pointCloudState;
|
||
}
|
||
|
||
if (!state) {
|
||
return; // 没有匹配的接收状态
|
||
}
|
||
|
||
if (size <= sizeof(GVSPPacketHeader)) {
|
||
return;
|
||
}
|
||
|
||
// Extract payload data (skip header)
|
||
const uint8_t *payload = data + sizeof(GVSPPacketHeader);
|
||
size_t payload_size = size - sizeof(GVSPPacketHeader);
|
||
|
||
// Append to buffer
|
||
state->dataBuffer.append(reinterpret_cast<const char*>(payload), payload_size);
|
||
state->receivedSize += payload_size;
|
||
state->packetCount++;
|
||
}
|
||
|
||
void GVSPParser::handleTrailerPacket(const uint8_t *data, size_t size)
|
||
{
|
||
if (size < sizeof(GVSPPacketHeader)) {
|
||
return;
|
||
}
|
||
|
||
const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);
|
||
uint32_t blockId = ntohs(header->block_id);
|
||
|
||
// 查找匹配的状态并处理
|
||
if (m_leftIRState.isReceiving && m_leftIRState.blockId == blockId) {
|
||
processImageData(&m_leftIRState);
|
||
m_leftIRState.isReceiving = false;
|
||
m_lastBlockId = blockId;
|
||
} else if (m_rightIRState.isReceiving && m_rightIRState.blockId == blockId) {
|
||
processImageData(&m_rightIRState);
|
||
m_rightIRState.isReceiving = false;
|
||
m_lastBlockId = blockId;
|
||
} else if (m_rgbState.isReceiving && m_rgbState.blockId == blockId) {
|
||
processImageData(&m_rgbState);
|
||
m_rgbState.isReceiving = false;
|
||
m_lastBlockId = blockId;
|
||
} else if (m_depthState.isReceiving && m_depthState.blockId == blockId) {
|
||
processDepthData(&m_depthState);
|
||
m_depthState.isReceiving = false;
|
||
m_lastBlockId = blockId;
|
||
} else if (m_pointCloudState.isReceiving && m_pointCloudState.blockId == blockId) {
|
||
processPointCloudData(&m_pointCloudState);
|
||
m_pointCloudState.isReceiving = false;
|
||
m_lastBlockId = blockId;
|
||
}
|
||
}
|
||
|
||
void GVSPParser::processImageData(GVSPParser::StreamState *state)
|
||
{
|
||
if (!state) return;
|
||
|
||
// 处理MJPEG格式（RGB相机）
|
||
if (state->pixelFormat == PIXEL_FORMAT_MJPEG) {
|
||
// RGB MJPEG
|
||
emit rgbImageReceived(state->dataBuffer, state->blockId);
|
||
m_imageSequence++;
|
||
return;
|
||
}
|
||
|
||
// 处理Mono16格式（左右红外相机原始16位数据）
|
||
// 使用像素格式直接区分左右，不依赖接收顺序
|
||
if (state->pixelFormat == PIXEL_FORMAT_MONO16_LEFT) {
|
||
// 检查数据大小
|
||
if (state->dataBuffer.size() < state->expectedSize) {
|
||
return;
|
||
}
|
||
// 左红外原始数据
|
||
emit leftImageReceived(state->dataBuffer, state->blockId);
|
||
m_imageSequence++;
|
||
return;
|
||
}
|
||
|
||
if (state->pixelFormat == PIXEL_FORMAT_MONO16_RIGHT) {
|
||
// 检查数据大小
|
||
if (state->dataBuffer.size() < state->expectedSize) {
|
||
return;
|
||
}
|
||
// 右红外原始数据
|
||
emit rightImageReceived(state->dataBuffer, state->blockId);
|
||
m_imageSequence++;
|
||
return;
|
||
}
|
||
|
||
// 处理Mono8格式（左右红外相机下采样8位数据）
|
||
if (state->pixelFormat == PIXEL_FORMAT_MONO8_LEFT) {
|
||
// 检查数据大小
|
||
if (state->dataBuffer.size() < state->expectedSize) {
|
||
return;
|
||
}
|
||
// 左红外8位数据
|
||
emit leftImageReceived(state->dataBuffer, state->blockId);
|
||
m_imageSequence++;
|
||
return;
|
||
}
|
||
|
||
if (state->pixelFormat == PIXEL_FORMAT_MONO8_RIGHT) {
|
||
// 检查数据大小
|
||
if (state->dataBuffer.size() < state->expectedSize) {
|
||
return;
|
||
}
|
||
// 右红外8位数据
|
||
emit rightImageReceived(state->dataBuffer, state->blockId);
|
||
m_imageSequence++;
|
||
return;
|
||
}
|
||
|
||
// 兼容旧版本：使用序号区分（legacy）
|
||
if (state->pixelFormat == PIXEL_FORMAT_MONO16) {
|
||
// 检查数据大小
|
||
if (state->dataBuffer.size() < state->expectedSize) {
|
||
return;
|
||
}
|
||
|
||
// 根据图像序号区分：偶数=左红外，奇数=右红外
|
||
int imageType = m_imageSequence % 2;
|
||
|
||
if (imageType == 0) {
|
||
// 左红外原始数据
|
||
emit leftImageReceived(state->dataBuffer, state->blockId);
|
||
} else {
|
||
// 右红外原始数据
|
||
emit rightImageReceived(state->dataBuffer, state->blockId);
|
||
}
|
||
|
||
m_imageSequence++;
|
||
return;
|
||
}
|
||
|
||
// 处理12-bit灰度格式 - 固定大小，需要检查
|
||
if (state->dataBuffer.size() < state->expectedSize) {
|
||
return;
|
||
}
|
||
|
||
// 处理12-bit灰度图像（左右红外相机）
|
||
if (state->pixelFormat != PIXEL_FORMAT_12BIT_GRAY) {
|
||
return;
|
||
}
|
||
|
||
// 节流机制：如果已有3个或更多图像在处理中，跳过当前帧
|
||
if (m_imageProcessingCount.loadAcquire() >= 3) {
|
||
m_imageSequence++;
|
||
return;
|
||
}
|
||
|
||
// 增加处理计数
|
||
m_imageProcessingCount.ref();
|
||
|
||
// 复制数据到局部变量
|
||
QByteArray dataCopy = state->dataBuffer;
|
||
uint32_t blockId = state->blockId;
|
||
size_t width = state->imageWidth;
|
||
size_t height = state->imageHeight;
|
||
int imageSeq = m_imageSequence;
|
||
|
||
// 使用QtConcurrent在后台线程处理图像数据
|
||
QtConcurrent::run([this, dataCopy, blockId, width, height, imageSeq]() {
|
||
// Convert 16-bit depth data to 8-bit grayscale for display
|
||
const uint16_t *src = reinterpret_cast<const uint16_t*>(dataCopy.constData());
|
||
QImage image(width, height, QImage::Format_Grayscale8);
|
||
|
||
// 优化：使用采样方式快速估算min/max（每隔16个像素采样一次）
|
||
uint16_t minVal = 65535, maxVal = 0;
|
||
size_t totalPixels = width * height;
|
||
const size_t sampleStep = 16; // 采样步长
|
||
|
||
for (size_t i = 0; i < totalPixels; i += sampleStep) {
|
||
uint16_t val = src[i];
|
||
if (val > 0) {
|
||
if (val < minVal) minVal = val;
|
||
if (val > maxVal) maxVal = val;
|
||
}
|
||
}
|
||
|
||
// 归一化（不翻转，保持原始方向）
|
||
uint8_t *dst = image.bits();
|
||
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
|
||
|
||
for (size_t row = 0; row < height; row++) {
|
||
for (size_t col = 0; col < width; col++) {
|
||
size_t idx = row * width + col;
|
||
uint16_t val = src[idx];
|
||
dst[idx] = (val == 0) ? 0 : static_cast<uint8_t>((val - minVal) * scale);
|
||
}
|
||
}
|
||
|
||
// 注意：此代码路径已废弃，下位机现在发送JPEG格式
|
||
// 仅保留兼容旧代码的信号
|
||
emit imageReceived(image, blockId);
|
||
|
||
// 减少处理计数
|
||
m_imageProcessingCount.deref();
|
||
});
|
||
|
||
// 增加图像序号
|
||
m_imageSequence++;
|
||
}
|
||
|
||
void GVSPParser::processDepthData(GVSPParser::StreamState *state)
|
||
{
|
||
if (!state) return;
|
||
|
||
if (state->dataBuffer.size() < state->expectedSize) {
|
||
return;
|
||
}
|
||
|
||
emit depthDataReceived(state->dataBuffer, state->blockId);
|
||
}
|
||
|
||
void GVSPParser::processPointCloudData(GVSPParser::StreamState *state)
|
||
{
|
||
if (!state) return;
|
||
|
||
if (state->dataBuffer.size() < state->expectedSize) {
|
||
return;
|
||
}
|
||
|
||
// 点云数据直接发送，格式为 short 数组 (x, y, z, x, y, z, ...)
|
||
emit pointCloudDataReceived(state->dataBuffer, state->blockId);
|
||
}
|