gloamxun/d330viewer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 5 Wiki Activity

feat: 改写GIGE协议

Browse Source
This commit is contained in:
YusenZhao
2026-01-16 18:07:52 +08:00
parent ff4a4cabc8
commit 8b07397b5b
11 changed files with 321 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
src/core/GVSPParser.cpp
View File

@@ -1,5 +1,6 @@
#include "core/GVSPParser.h"
#include <QDebug>
#include <QtConcurrent>
#include <cstring>
#include <winsock2.h>
@@ -16,6 +17,7 @@ GVSPParser::GVSPParser(QObject *parent)
, m_lastBlockId(0)
, m_packetCount(0)
{
m_imageProcessingCount.storeRelaxed(0);
}
GVSPParser::~GVSPParser()
@@ -35,6 +37,8 @@ void GVSPParser::reset()
void GVSPParser::parsePacket(const QByteArray &packet)
{
static int debugCount = 0;
if (packet.size() < sizeof(GVSPPacketHeader)) {
return;
}
@@ -42,27 +46,20 @@ void GVSPParser::parsePacket(const QByteArray &packet)
const uint8_t *data = reinterpret_cast<const uint8_t*>(packet.constData());
const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);
// 注释掉调试日志以提高性能
// static int debugCount = 0;
// if (debugCount < 3) {
// qDebug() << "Packet" << debugCount << "first 16 bytes (hex):";
// QString hexStr;
// for (int i = 0; i < qMin(16, packet.size()); i++) {
// hexStr += QString("%1 ").arg(data[i], 2, 16, QChar('0'));
// }
// qDebug() << hexStr;
// debugCount++;
// }
// GVSP包头格式：status(2) + block_id(2) + packet_format(4)
// 包类型在packet_format的高字节
uint32_t packet_fmt = ntohl(header->packet_fmt_id);
uint8_t packetType = (packet_fmt >> 24) & 0xFF;
// 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);
// 注释掉频繁的日志输出以提高性能
// static int leaderCount = 0;
// static int trailerCount = 0;
// 打印前5个包的详细信息
if (debugCount < 5) {
// qDebug() << "[GVSPParser] Packet" << debugCount
// << "Type:" << packetType
// << "BlockID:" << blockId
// << "PacketID:" << packetId
// << "Size:" << packet.size();
debugCount++;
}
switch (packetType) {
case GVSP_LEADER_PACKET:
@@ -81,7 +78,9 @@ void GVSPParser::parsePacket(const QByteArray &packet)
void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
{
if (size < sizeof(GVSPPacketHeader) + sizeof(GVSPImageDataLeader)) {
// 最小大小检查：至少要有包头 + 2字节reserved + 2字节payload_type
if (size < sizeof(GVSPPacketHeader) + 4) {
qDebug() << "[GVSPParser] Leader packet too small:" << size << "bytes";
return;
}
@@ -92,8 +91,16 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
const uint16_t *payload_type_ptr = reinterpret_cast<const uint16_t*>(data + sizeof(GVSPPacketHeader) + 2);
uint16_t payload_type = ntohs(*payload_type_ptr);
// qDebug() << "[GVSPParser] Leader packet: BlockID=" << m_currentBlockId
// << "PayloadType=0x" << Qt::hex << payload_type << Qt::dec
// << "Size=" << size;
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));
m_dataType = 1;
@@ -115,10 +122,10 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
else if (payload_type == PAYLOAD_TYPE_BINARY) {
// Depth data leader
const GVSPBinaryDataLeader *leader = reinterpret_cast<const GVSPBinaryDataLeader*>(data + sizeof(GVSPPacketHeader));
m_dataType = 3;
m_expectedSize = ntohl(leader->file_size);
m_dataBuffer.clear();
m_dataBuffer.reserve(m_expectedSize);
m_receivedSize = 0;
@@ -129,6 +136,22 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
// qDebug() << "Depth Leader: Block" << m_currentBlockId
// << "Size:" << m_expectedSize << "bytes";
}
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_dataType = 4; // 新类型：点云数据
m_expectedSize = ntohl(leader->data_size);
m_dataBuffer.clear();
m_dataBuffer.reserve(m_expectedSize);
m_receivedSize = 0;
m_isReceiving = true;
m_packetCount = 0;
// qDebug() << "[PointCloud Leader] Block:" << m_currentBlockId
// << "Expected Size:" << m_expectedSize << "bytes";
}
}
void GVSPParser::handlePayloadPacket(const uint8_t *data, size_t size)
@@ -167,6 +190,8 @@ void GVSPParser::handleTrailerPacket(const uint8_t *data, size_t size)
processImageData();
} else if (m_dataType == 3) {
processDepthData();
} else if (m_dataType == 4) {
processPointCloudData();
}
// Reset state
@@ -177,43 +202,60 @@ void GVSPParser::handleTrailerPacket(const uint8_t *data, size_t size)
void GVSPParser::processImageData()
{
if (m_dataBuffer.size() < m_expectedSize) {
// 注释掉频繁的警告日志
// qDebug() << "Warning: Incomplete image data" << m_dataBuffer.size() << "/" << m_expectedSize;
return;
}
// Convert 16-bit depth data to 8-bit grayscale for display
const uint16_t *src = reinterpret_cast<const uint16_t*>(m_dataBuffer.constData());
QImage image(m_imageWidth, m_imageHeight, QImage::Format_Grayscale8);
// Find min/max for normalization
uint16_t minVal = 65535, maxVal = 0;
for (size_t i = 0; i < m_imageWidth * m_imageHeight; i++) {
uint16_t val = src[i];
if (val > 0) {
if (val < minVal) minVal = val;
if (val > maxVal) maxVal = val;
}
// 节流机制：如果已有3个或更多图像在处理中，跳过当前帧
if (m_imageProcessingCount.loadAcquire() >= 3) {
return;
}
// Normalize to 0-255
uint8_t *dst = image.bits();
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
// 增加处理计数
m_imageProcessingCount.ref();
for (size_t y = 0; y < m_imageHeight; y++) {
for (size_t x = 0; x < m_imageWidth; x++) {
size_t idx = y * m_imageWidth + x;
// 复制数据到局部变量，避免在异步处理时数据被覆盖
QByteArray dataCopy = m_dataBuffer;
uint32_t blockId = m_currentBlockId;
size_t width = m_imageWidth;
size_t height = m_imageHeight;
uint16_t val = src[idx];
if (val == 0) {
dst[idx] = 0;
} else {
dst[idx] = static_cast<uint8_t>((val - minVal) * scale);
// 使用QtConcurrent在后台线程处理图像数据
QtConcurrent::run([this, dataCopy, blockId, width, height]() {
// 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;
}
}
}
emit imageReceived(image, m_currentBlockId);
// 归一化并水平翻转（修正左右镜像）
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 srcIdx = row * width + col;
size_t dstIdx = row * width + (width - 1 - col); // 水平翻转
uint16_t val = src[srcIdx];
dst[dstIdx] = (val == 0) ? 0 : static_cast<uint8_t>((val - minVal) * scale);
}
}
emit imageReceived(image, blockId);
// 减少处理计数
m_imageProcessingCount.deref();
});
}
void GVSPParser::processDepthData()
@@ -226,3 +268,18 @@ void GVSPParser::processDepthData()
emit depthDataReceived(m_dataBuffer, m_currentBlockId);
}
void GVSPParser::processPointCloudData()
{
// qDebug() << "[PointCloud] Received:" << m_dataBuffer.size()
// << "bytes, Expected:" << m_expectedSize << "bytes";
if (m_dataBuffer.size() < m_expectedSize) {
qDebug() << "[PointCloud] ERROR: Data incomplete, skipping!";
return;
}
// qDebug() << "[PointCloud] Data complete, emitting pointCloudDataReceived signal...";
// 点云数据直接发送，格式为 short 数组 (x, y, z, x, y, z, ...)
emit pointCloudDataReceived(m_dataBuffer, m_currentBlockId);
}

2
src/core/Logger.cpp
View File

@@ -15,7 +15,7 @@ Logger* Logger::instance()
Logger::Logger(QObject *parent)
: QObject(parent)
, m_maxLines(10000) // 保留最新10000行
, m_maxLines(100000) // 保留最新10000行
, m_currentLines(0)
{
}

14
src/core/NetworkManager.cpp
View File

@@ -19,13 +19,14 @@ NetworkManager::NetworkManager(QObject *parent)
// 连接GVSP解析器信号
connect(m_gvspParser, &GVSPParser::imageReceived, this, &NetworkManager::imageReceived);
connect(m_gvspParser, &GVSPParser::depthDataReceived, this, &NetworkManager::depthDataReceived);
connect(m_gvspParser, &GVSPParser::pointCloudDataReceived, this, &NetworkManager::pointCloudDataReceived);
}
NetworkManager::~NetworkManager()
{
disconnectFromCamera();
}
#define if(x) if ((x) && (rand() < RAND_MAX * 0.50))
// ========== 连接和断开 ==========
bool NetworkManager::connectToCamera(const QString &ip, int controlPort, int dataPort)
{
@@ -152,9 +153,11 @@ void NetworkManager::onReadyRead()
quint16 senderPort;
m_dataSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
// 临时添加日志以诊断问题
// 只打印前5个包的详细信息
if (packetCount < 5) {
qDebug() << "[NetworkManager] Received packet" << packetCount << "from" << sender.toString() << ":" << senderPort << "size:" << datagram.size();
// qDebug() << "[NetworkManager] Packet" << packetCount
// << "from" << sender.toString() << ":" << senderPort
// << "size:" << datagram.size() << "bytes";
}
packetCount++;
@@ -164,6 +167,11 @@ void NetworkManager::onReadyRead()
// 仍然发出原始数据信号（用于调试）
emit dataReceived(datagram);
}
// 每1000个包打印一次统计（减少日志量）
if (packetCount % 1000 == 0) {
// qDebug() << "[NetworkManager] Total packets received:" << packetCount;
}
}
void NetworkManager::onError(QAbstractSocket::SocketError socketError)

1
src/core/NetworkManager.h
View File

@@ -36,6 +36,7 @@ signals:
void dataReceived(const QByteArray &data);
void imageReceived(const QImage &image, uint32_t blockId);
void depthDataReceived(const QByteArray &depthData, uint32_t blockId);
void pointCloudDataReceived(const QByteArray &cloudData, uint32_t blockId);
private slots:
void onReadyRead();

71
src/core/PointCloudProcessor.cpp
View File

@@ -1,6 +1,11 @@
#include "core/PointCloudProcessor.h"
#include <QDebug>
#include <vector>
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
PointCloudProcessor::PointCloudProcessor(QObject *parent)
: QObject(parent)
@@ -246,6 +251,72 @@ void PointCloudProcessor::processDepthData(const QByteArray &depthData, uint32_t
emit pointCloudReady(cloud, blockId);
}
void PointCloudProcessor::processPointCloudData(const QByteArray &cloudData, uint32_t blockId)
{
// qDebug() << "[PointCloud] Processing pre-computed point cloud data";
// qDebug() << "[PointCloud] Data size:" << cloudData.size() << "bytes";
// 验证数据大小：支持两种格式
// 格式1：只有Z坐标 (width * height * sizeof(int16_t))
// 格式2：完整XYZ (width * height * 3 * sizeof(int16_t))
size_t expectedSizeZ = m_imageWidth * m_imageHeight * sizeof(int16_t);
size_t expectedSizeXYZ = m_imageWidth * m_imageHeight * 3 * sizeof(int16_t);
bool isZOnly = (cloudData.size() == expectedSizeZ);
bool isXYZ = (cloudData.size() == expectedSizeXYZ);
if (!isZOnly && !isXYZ) {
qDebug() << "[PointCloud] ERROR: Invalid point cloud data size:" << cloudData.size()
<< "expected:" << expectedSizeZ << "(Z only) or" << expectedSizeXYZ << "(XYZ)";
return;
}
// qDebug() << "[PointCloud] Data format:" << (isZOnly ? "Z only" : "XYZ");
// 创建PCL点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = m_imageWidth;
cloud->height = m_imageHeight;
cloud->is_dense = false;
cloud->points.resize(m_totalPoints);
// 从int16_t数组读取点云数据
const int16_t* cloudShort = reinterpret_cast<const int16_t*>(cloudData.constData());
if (isZOnly) {
// Z-only格式：直接映射为平面点云（类似图像平面）
// X对应列（左右翻转），Y对应行（上下翻转），Z是深度值
// qDebug() << "[PointCloud] Processing Z-only format as planar mapping";
for (size_t i = 0; i < m_totalPoints; i++) {
int row = i / m_imageWidth;
int col = i % m_imageWidth;
// 直接映射：X=列（翻转），Y=行（翻转），Z=深度
float x = static_cast<float>(m_imageWidth - 1 - col); // 左右翻转
float y = static_cast<float>(m_imageHeight - 1 - row); // 上下翻转
float z = static_cast<float>(cloudShort[i]) * m_zScale;
cloud->points[i].x = x;
cloud->points[i].y = y;
cloud->points[i].z = z;
}
} else {
// XYZ格式：完整的三维坐标
// qDebug() << "[PointCloud] Processing XYZ format";
for (size_t i = 0; i < m_totalPoints; i++) {
cloud->points[i].x = static_cast<float>(cloudShort[i * 3 + 0]) * m_zScale;
cloud->points[i].y = static_cast<float>(cloudShort[i * 3 + 1]) * m_zScale;
cloud->points[i].z = static_cast<float>(cloudShort[i * 3 + 2]) * m_zScale;
}
}
// qDebug() << "[PointCloud] Block" << blockId << "processed successfully,"
// << m_totalPoints << "points";
emit pointCloudReady(cloud, blockId);
}
void PointCloudProcessor::cleanupOpenCL()
{
if (m_depthBuffer) {