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

feat: v0.1.0更新

Browse Source
This commit is contained in:
YusenZhao
2026-01-14 18:07:26 +08:00
commit efd8a7cc20
55 changed files with 6200 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

183
src/core/DeviceScanner.cpp Normal file
View File

@@ -0,0 +1,183 @@
#include "DeviceScanner.h"
#include <QNetworkDatagram>
#include <QNetworkInterface>
#include <QDebug>
DeviceScanner::DeviceScanner(QObject *parent)
: QObject(parent)
, m_socket(new QUdpSocket(this))
, m_timeoutTimer(new QTimer(this))
, m_scanTimer(new QTimer(this))
, m_currentHost(HOST_START)
, m_totalHosts(HOST_END - HOST_START + 1)
, m_isScanning(false)
{
connect(m_socket, &QUdpSocket::readyRead, this, &DeviceScanner::onReadyRead);
connect(m_timeoutTimer, &QTimer::timeout, this, &DeviceScanner::onTimeout);
connect(m_scanTimer, &QTimer::timeout, this, &DeviceScanner::scanNextHost);
m_timeoutTimer->setSingleShot(true);
m_scanTimer->setSingleShot(false);
m_scanTimer->setInterval(SCAN_TIMEOUT);
}
DeviceScanner::~DeviceScanner()
{
stopScan();
}
void DeviceScanner::startScan(const QString &subnet)
{
if (m_isScanning) {
qDebug() << "Scan already in progress";
return;
}
m_subnet = subnet.isEmpty() ? getLocalSubnet() : subnet;
m_currentHost = HOST_START;
m_foundDevices.clear();
m_isScanning = true;
if (!m_socket->bind(QHostAddress::Any, 0)) {
emit scanError("Failed to bind socket");
m_isScanning = false;
return;
}
qDebug() << "Starting fast device scan on subnet:" << m_subnet;
// Send DISCOVER to all hosts at once (batch mode)
for (int host = HOST_START; host <= HOST_END; host++) {
QString ip = m_subnet + "." + QString::number(host);
sendDiscoveryPacket(ip);
}
// Wait 3 seconds for all responses
m_timeoutTimer->start(3000);
qDebug() << "Sent discovery packets to all hosts, waiting for responses...";
}
void DeviceScanner::stopScan()
{
if (!m_isScanning) {
return;
}
m_scanTimer->stop();
m_timeoutTimer->stop();
m_socket->close();
m_isScanning = false;
qDebug() << "Scan stopped. Found" << m_foundDevices.size() << "devices";
emit scanFinished(m_foundDevices.size());
}
void DeviceScanner::onReadyRead()
{
while (m_socket->hasPendingDatagrams()) {
QNetworkDatagram datagram = m_socket->receiveDatagram();
QHostAddress senderAddr = datagram.senderAddress();
// Convert IPv6-mapped IPv4 address to IPv4
if (senderAddr.protocol() == QAbstractSocket::IPv6Protocol) {
QHostAddress ipv4Addr(senderAddr.toIPv4Address());
if (!ipv4Addr.isNull()) {
senderAddr = ipv4Addr;
}
}
QString senderIp = senderAddr.toString();
QString response = QString::fromUtf8(datagram.data());
qDebug() << "Received response from" << senderIp << ":" << response;
if (response.contains("D330M_CAMERA")) {
DeviceInfo device;
device.ipAddress = senderIp;
device.deviceName = "D330M Camera";
device.port = SCAN_PORT;
device.responseTime = 0;
m_foundDevices.append(device);
emit deviceFound(device);
}
}
}
void DeviceScanner::onTimeout()
{
qDebug() << "Scan timeout reached";
stopScan();
}
void DeviceScanner::scanNextHost()
{
// This function is no longer used in batch mode
// Kept for compatibility
}
void DeviceScanner::sendDiscoveryPacket(const QString &ip)
{
QByteArray data = "DISCOVER";
m_socket->writeDatagram(data, QHostAddress(ip), SCAN_PORT);
}
QString DeviceScanner::getLocalSubnet()
{
// Get all network interfaces
QList<QNetworkInterface> interfaces = QNetworkInterface::allInterfaces();
// Priority 1: Look for Ethernet adapter (以太网)
for (const QNetworkInterface &iface : interfaces) {
QString name = iface.humanReadableName().toLower();
// Skip virtual adapters
if (name.contains("virtual") || name.contains("vmware") ||
name.contains("virtualbox") || name.contains("hyper-v") ||
name.contains("vpn") || name.contains("tap") || name.contains("tun")) {
continue;
}
// Look for Ethernet adapter
if (name.contains("ethernet") || name.contains("以太网")) {
QList<QNetworkAddressEntry> entries = iface.addressEntries();
for (const QNetworkAddressEntry &entry : entries) {
QHostAddress addr = entry.ip();
if (addr.protocol() == QAbstractSocket::IPv4Protocol && !addr.isLoopback()) {
QString ip = addr.toString();
QStringList parts = ip.split('.');
if (parts.size() == 4) {
qDebug() << "Found Ethernet adapter:" << iface.humanReadableName() << "IP:" << ip;
return parts[0] + "." + parts[1] + "." + parts[2];
}
}
}
}
}
// Priority 2: Any non-virtual adapter
for (const QNetworkInterface &iface : interfaces) {
QString name = iface.humanReadableName().toLower();
if (name.contains("virtual") || name.contains("vmware") ||
name.contains("virtualbox") || name.contains("hyper-v") ||
name.contains("vpn") || name.contains("tap") || name.contains("tun")) {
continue;
}
QList<QNetworkAddressEntry> entries = iface.addressEntries();
for (const QNetworkAddressEntry &entry : entries) {
QHostAddress addr = entry.ip();
if (addr.protocol() == QAbstractSocket::IPv4Protocol && !addr.isLoopback()) {
QString ip = addr.toString();
QStringList parts = ip.split('.');
if (parts.size() == 4) {
qDebug() << "Found adapter:" << iface.humanReadableName() << "IP:" << ip;
return parts[0] + "." + parts[1] + "." + parts[2];
}
}
}
}
return "192.168.0";
}

62
src/core/DeviceScanner.h Normal file
View File

@@ -0,0 +1,62 @@
#ifndef DEVICESCANNER_H
#define DEVICESCANNER_H
#include <QObject>
#include <QUdpSocket>
#include <QTimer>
#include <QHostAddress>
#include <QList>
#include <QString>
struct DeviceInfo {
QString ipAddress;
QString deviceName;
int port;
qint64 responseTime;
};
class DeviceScanner : public QObject
{
Q_OBJECT
public:
explicit DeviceScanner(QObject *parent = nullptr);
~DeviceScanner();
void startScan(const QString &subnet = "");
void stopScan();
bool isScanning() const { return m_isScanning; }
signals:
void deviceFound(const DeviceInfo &device);
void scanProgress(int current, int total);
void scanFinished(int devicesFound);
void scanError(const QString &error);
private slots:
void onReadyRead();
void onTimeout();
void scanNextHost();
private:
void sendDiscoveryPacket(const QString &ip);
QString getLocalSubnet();
QUdpSocket *m_socket;
QTimer *m_timeoutTimer;
QTimer *m_scanTimer;
QString m_subnet;
int m_currentHost;
int m_totalHosts;
bool m_isScanning;
QList<DeviceInfo> m_foundDevices;
static constexpr int SCAN_PORT = 6790; // Control port for device discovery
static constexpr int SCAN_TIMEOUT = 10;
static constexpr int HOST_START = 1;
static constexpr int HOST_END = 254;
};
#endif // DEVICESCANNER_H

59
src/core/DeviceScanner_getLocalSubnet.cpp Normal file
View File

@@ -0,0 +1,59 @@
QString DeviceScanner::getLocalSubnet()
{
// Get all network interfaces
QList<QNetworkInterface> interfaces = QNetworkInterface::allInterfaces();
// Priority 1: Look for Ethernet adapter (以太网)
for (const QNetworkInterface &iface : interfaces) {
QString name = iface.humanReadableName().toLower();
// Skip virtual adapters
if (name.contains("virtual") || name.contains("vmware") ||
name.contains("virtualbox") || name.contains("hyper-v") ||
name.contains("vpn") || name.contains("tap") || name.contains("tun")) {
continue;
}
// Look for Ethernet adapter
if (name.contains("ethernet") || name.contains("以太网")) {
QList<QNetworkAddressEntry> entries = iface.addressEntries();
for (const QNetworkAddressEntry &entry : entries) {
QHostAddress addr = entry.ip();
if (addr.protocol() == QAbstractSocket::IPv4Protocol && !addr.isLoopback()) {
QString ip = addr.toString();
QStringList parts = ip.split('.');
if (parts.size() == 4) {
qDebug() << "Found Ethernet adapter:" << iface.humanReadableName() << "IP:" << ip;
return parts[0] + "." + parts[1] + "." + parts[2];
}
}
}
}
}
// Priority 2: Any non-virtual adapter
for (const QNetworkInterface &iface : interfaces) {
QString name = iface.humanReadableName().toLower();
if (name.contains("virtual") || name.contains("vmware") ||
name.contains("virtualbox") || name.contains("hyper-v") ||
name.contains("vpn") || name.contains("tap") || name.contains("tun")) {
continue;
}
QList<QNetworkAddressEntry> entries = iface.addressEntries();
for (const QNetworkAddressEntry &entry : entries) {
QHostAddress addr = entry.ip();
if (addr.protocol() == QAbstractSocket::IPv4Protocol && !addr.isLoopback()) {
QString ip = addr.toString();
QStringList parts = ip.split('.');
if (parts.size() == 4) {
qDebug() << "Found adapter:" << iface.humanReadableName() << "IP:" << ip;
return parts[0] + "." + parts[1] + "." + parts[2];
}
}
}
}
return "192.168.0";
}

229
src/core/GVSPParser.cpp Normal file
View File

@@ -0,0 +1,229 @@
#include "core/GVSPParser.h"
#include <QDebug>
#include <cstring>
#include <winsock2.h>
GVSPParser::GVSPParser(QObject *parent)
: QObject(parent)
, m_isReceiving(false)
, m_dataType(0)
, m_currentBlockId(0)
, m_expectedSize(0)
, m_receivedSize(0)
, m_imageWidth(0)
, m_imageHeight(0)
, m_pixelFormat(0)
, m_lastBlockId(0)
, m_packetCount(0)
{
}
GVSPParser::~GVSPParser()
{
}
void GVSPParser::reset()
{
m_isReceiving = false;
m_dataType = 0;
m_currentBlockId = 0;
m_dataBuffer.clear();
m_expectedSize = 0;
m_receivedSize = 0;
m_packetCount = 0;
}
void GVSPParser::parsePacket(const QByteArray &packet)
{
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);
// 注释掉调试日志以提高性能
// 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;
uint16_t blockId = ntohs(header->block_id);
// 注释掉频繁的日志输出以提高性能
// static int leaderCount = 0;
// static int trailerCount = 0;
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)
{
if (size < sizeof(GVSPPacketHeader) + sizeof(GVSPImageDataLeader)) {
return;
}
const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);
m_currentBlockId = 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
const GVSPImageDataLeader *leader = reinterpret_cast<const GVSPImageDataLeader*>(data + sizeof(GVSPPacketHeader));
m_dataType = 1;
m_imageWidth = ntohl(leader->size_x);
m_imageHeight = ntohl(leader->size_y);
m_pixelFormat = ntohl(leader->pixel_format);
m_expectedSize = m_imageWidth * m_imageHeight * 2; // 12-bit packed in 16-bit
m_dataBuffer.clear();
m_dataBuffer.reserve(m_expectedSize);
m_receivedSize = 0;
m_isReceiving = true;
m_packetCount = 0;
// 注释掉频繁的日志输出
// qDebug() << "Image Leader: Block" << m_currentBlockId
// << "Size:" << m_imageWidth << "x" << m_imageHeight;
}
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;
m_isReceiving = true;
m_packetCount = 0;
// 注释掉频繁的日志输出
// qDebug() << "Depth Leader: Block" << m_currentBlockId
// << "Size:" << m_expectedSize << "bytes";
}
}
void GVSPParser::handlePayloadPacket(const uint8_t *data, size_t size)
{
if (!m_isReceiving) {
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
m_dataBuffer.append(reinterpret_cast<const char*>(payload), payload_size);
m_receivedSize += payload_size;
m_packetCount++;
}
void GVSPParser::handleTrailerPacket(const uint8_t *data, size_t size)
{
if (!m_isReceiving) {
return;
}
// 注释掉频繁的日志输出
// qDebug() << "Trailer received: Block" << m_currentBlockId
// << "Received" << m_receivedSize << "/" << m_expectedSize << "bytes"
// << "Packets:" << m_packetCount;
// Process complete data
if (m_dataType == 1) {
processImageData();
} else if (m_dataType == 3) {
processDepthData();
}
// Reset state
m_isReceiving = false;
m_lastBlockId = m_currentBlockId;
}
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;
}
}
// Normalize to 0-255 and flip vertically
uint8_t *dst = image.bits();
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
for (size_t y = 0; y < m_imageHeight; y++) {
for (size_t x = 0; x < m_imageWidth; x++) {
size_t src_idx = y * m_imageWidth + x;
size_t dst_idx = (m_imageHeight - 1 - y) * m_imageWidth + x; // 垂直翻转
uint16_t val = src[src_idx];
if (val == 0) {
dst[dst_idx] = 0;
} else {
dst[dst_idx] = static_cast<uint8_t>((val - minVal) * scale);
}
}
}
emit imageReceived(image, m_currentBlockId);
}
void GVSPParser::processDepthData()
{
if (m_dataBuffer.size() < m_expectedSize) {
// 注释掉频繁的警告日志
// qDebug() << "Warning: Incomplete depth data" << m_dataBuffer.size() << "/" << m_expectedSize;
return;
}
emit depthDataReceived(m_dataBuffer, m_currentBlockId);
}

130
src/core/Logger.cpp Normal file
View File

@@ -0,0 +1,130 @@
#include "core/Logger.h"
#include <QDateTime>
#include <QFileInfo>
#include <QDir>
Logger* Logger::s_instance = nullptr;
Logger* Logger::instance()
{
if (!s_instance) {
s_instance = new Logger();
}
return s_instance;
}
Logger::Logger(QObject *parent)
: QObject(parent)
, m_maxLines(10000) // 保留最新10000行
, m_currentLines(0)
{
}
Logger::~Logger()
{
}
void Logger::setLogFile(const QString &filePath)
{
QMutexLocker locker(&m_mutex);
m_logFilePath = filePath;
// Count existing lines
QFile file(m_logFilePath);
if (file.open(QIODevice::ReadOnly | QIODevice::Text)) {
m_currentLines = 0;
while (!file.atEnd()) {
file.readLine();
m_currentLines++;
}
file.close();
}
}
void Logger::setMaxLines(int maxLines)
{
QMutexLocker locker(&m_mutex);
m_maxLines = maxLines;
}
void Logger::log(const QString &message)
{
writeLog("LOG", message);
}
void Logger::debug(const QString &message)
{
writeLog("DEBUG", message);
}
void Logger::info(const QString &message)
{
writeLog("INFO", message);
}
void Logger::warning(const QString &message)
{
writeLog("WARN", message);
}
void Logger::error(const QString &message)
{
writeLog("ERROR", message);
}
void Logger::writeLog(const QString &level, const QString &message)
{
QMutexLocker locker(&m_mutex);
if (m_logFilePath.isEmpty()) {
return;
}
QString timestamp = QDateTime::currentDateTime().toString("[yyyy-MM-dd hh:mm:ss.zzz]");
QString logLine = QString("%1 [%2] %3\n").arg(timestamp).arg(level).arg(message);
QFile file(m_logFilePath);
if (file.open(QIODevice::Append | QIODevice::Text)) {
QTextStream out(&file);
out << logLine;
file.close();
m_currentLines++;
// Check if we need to trim the log
if (m_currentLines > m_maxLines) {
checkAndTrimLog();
}
}
}
void Logger::checkAndTrimLog()
{
// Read all lines
QFile file(m_logFilePath);
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
return;
}
QStringList lines;
QTextStream in(&file);
while (!in.atEnd()) {
lines.append(in.readLine());
}
file.close();
// Keep only the last m_maxLines
if (lines.size() > m_maxLines) {
lines = lines.mid(lines.size() - m_maxLines);
}
// Write back
if (file.open(QIODevice::WriteOnly | QIODevice::Text | QIODevice::Truncate)) {
QTextStream out(&file);
for (const QString &line : lines) {
out << line << "\n";
}
file.close();
m_currentLines = lines.size();
}
}

174
src/core/NetworkManager.cpp Normal file
View File

@@ -0,0 +1,174 @@
#include "core/NetworkManager.h"
#include "core/GVSPParser.h"
#include <QDebug>
#include <QVariant>
NetworkManager::NetworkManager(QObject *parent)
: QObject(parent)
, m_controlSocket(new QUdpSocket(this))
, m_dataSocket(new QUdpSocket(this))
, m_gvspParser(new GVSPParser(this))
, m_controlPort(6790)
, m_dataPort(3957)
, m_isConnected(false)
{
// 连接数据接收信号
connect(m_dataSocket, &QUdpSocket::readyRead, this, &NetworkManager::onReadyRead);
connect(m_dataSocket, &QUdpSocket::errorOccurred, this, &NetworkManager::onError);
// 连接GVSP解析器信号
connect(m_gvspParser, &GVSPParser::imageReceived, this, &NetworkManager::imageReceived);
connect(m_gvspParser, &GVSPParser::depthDataReceived, this, &NetworkManager::depthDataReceived);
}
NetworkManager::~NetworkManager()
{
disconnectFromCamera();
}
// ========== 连接和断开 ==========
bool NetworkManager::connectToCamera(const QString &ip, int controlPort, int dataPort)
{
m_cameraIp = ip;
m_controlPort = controlPort;
m_dataPort = dataPort;
// 绑定控制Socket到任意端口（让系统自动分配）
if (!m_controlSocket->bind(QHostAddress::Any, 0)) {
QString error = QString("Failed to bind control socket: %1")
.arg(m_controlSocket->errorString());
qDebug() << error;
emit errorOccurred(error);
return false;
}
qDebug() << "Successfully bound control socket to port" << m_controlSocket->localPort();
// 绑定数据接收端口
if (!m_dataSocket->bind(QHostAddress::Any, m_dataPort)) {
QString error = QString("Failed to bind data port %1: %2")
.arg(m_dataPort)
.arg(m_dataSocket->errorString());
qDebug() << error;
emit errorOccurred(error);
return false;
}
// 设置UDP接收缓冲区大小为64MB（减少丢包）
m_dataSocket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, QVariant(64 * 1024 * 1024));
qDebug() << "Successfully bound data port" << m_dataPort;
qDebug() << "Data socket state:" << m_dataSocket->state();
qDebug() << "UDP receive buffer size:" << m_dataSocket->socketOption(QAbstractSocket::ReceiveBufferSizeSocketOption).toInt();
m_isConnected = true;
qDebug() << "Connected to camera:" << m_cameraIp << "Control port:" << m_controlPort << "Data port:" << m_dataPort;
// Send STOP command to register client IP on camera
sendStopCommand();
qDebug() << "Sent STOP command to register client IP";
emit connected();
return true;
}
void NetworkManager::disconnectFromCamera()
{
if (m_isConnected) {
m_controlSocket->close();
m_dataSocket->close();
m_isConnected = false;
qDebug() << "Disconnected from camera";
emit disconnected();
}
}
bool NetworkManager::isConnected() const
{
return m_isConnected;
}
// ========== 发送控制命令 ==========
bool NetworkManager::sendCommand(const QString &command)
{
if (!m_isConnected) {
qDebug() << "Not connected to camera";
return false;
}
QByteArray data = command.toUtf8();
qDebug() << "Sending command to" << m_cameraIp << ":" << m_controlPort << "data:" << command;
qDebug() << "Control socket state:" << m_controlSocket->state();
qDebug() << "Control socket error:" << m_controlSocket->errorString();
qDebug() << "Data to send (hex):" << data.toHex();
qDebug() << "Data size:" << data.size();
qint64 sent = m_controlSocket->writeDatagram(data, QHostAddress(m_cameraIp), m_controlPort);
qDebug() << "writeDatagram returned:" << sent;
if (sent == -1) {
QString error = QString("Failed to send command: %1").arg(m_controlSocket->errorString());
qDebug() << error;
qDebug() << "Socket error code:" << m_controlSocket->error();
emit errorOccurred(error);
return false;
}
qDebug() << "Command sent successfully, bytes:" << sent;
qDebug() << "Sent command:" << command;
return true;
}
bool NetworkManager::sendStartCommand()
{
return sendCommand("START");
}
bool NetworkManager::sendStopCommand()
{
return sendCommand("STOP");
}
bool NetworkManager::sendOnceCommand()
{
return sendCommand("ONCE");
}
bool NetworkManager::sendExposureCommand(int exposureTime)
{
QString command = QString("EXPOSURE:%1").arg(exposureTime);
return sendCommand(command);
}
// ========== 槽函数 ==========
void NetworkManager::onReadyRead()
{
static int packetCount = 0;
while (m_dataSocket->hasPendingDatagrams()) {
QByteArray datagram;
datagram.resize(m_dataSocket->pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
m_dataSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
// 临时添加日志以诊断问题
if (packetCount < 5) {
qDebug() << "[NetworkManager] Received packet" << packetCount << "from" << sender.toString() << ":" << senderPort << "size:" << datagram.size();
}
packetCount++;
// 将数据包传递给GVSP解析器
m_gvspParser->parsePacket(datagram);
// 仍然发出原始数据信号（用于调试）
emit dataReceived(datagram);
}
}
void NetworkManager::onError(QAbstractSocket::SocketError socketError)
{
QString error = QString("Socket error: %1").arg(m_dataSocket->errorString());
qDebug() << error;
emit errorOccurred(error);
}

55
src/core/NetworkManager.h Normal file
View File

@@ -0,0 +1,55 @@
#ifndef NETWORKMANAGER_H
#define NETWORKMANAGER_H
#include <QObject>
#include <QUdpSocket>
#include <QString>
#include <QImage>
#include <memory>
class GVSPParser;
class NetworkManager : public QObject
{
Q_OBJECT
public:
explicit NetworkManager(QObject *parent = nullptr);
~NetworkManager();
// 连接和断开
bool connectToCamera(const QString &ip, int controlPort, int dataPort);
void disconnectFromCamera();
bool isConnected() const;
// 发送控制命令
bool sendCommand(const QString &command);
bool sendStartCommand();
bool sendStopCommand();
bool sendOnceCommand();
bool sendExposureCommand(int exposureTime);
signals:
void connected();
void disconnected();
void errorOccurred(const QString &error);
void dataReceived(const QByteArray &data);
void imageReceived(const QImage &image, uint32_t blockId);
void depthDataReceived(const QByteArray &depthData, uint32_t blockId);
private slots:
void onReadyRead();
void onError(QAbstractSocket::SocketError socketError);
private:
QUdpSocket *m_controlSocket;
QUdpSocket *m_dataSocket;
GVSPParser *m_gvspParser;
QString m_cameraIp;
int m_controlPort;
int m_dataPort;
bool m_isConnected;
};
#endif // NETWORKMANAGER_H

277
src/core/PointCloudProcessor.cpp Normal file
View File

@@ -0,0 +1,277 @@
#include "core/PointCloudProcessor.h"
#include <QDebug>
#include <vector>
PointCloudProcessor::PointCloudProcessor(QObject *parent)
: QObject(parent)
, m_fx(1432.8957f)
, m_fy(1432.6590f)
, m_cx(637.5117f)
, m_cy(521.8720f)
, m_zScale(0.2f)
, m_imageWidth(1224)
, m_imageHeight(1024)
, m_totalPoints(1224 * 1024)
, m_platform(nullptr)
, m_device(nullptr)
, m_context(nullptr)
, m_queue(nullptr)
, m_program(nullptr)
, m_kernel(nullptr)
, m_depthBuffer(nullptr)
, m_xyzBuffer(nullptr)
, m_clInitialized(false)
{
}
PointCloudProcessor::~PointCloudProcessor()
{
cleanupOpenCL();
}
void PointCloudProcessor::setCameraIntrinsics(float fx, float fy, float cx, float cy)
{
m_fx = fx;
m_fy = fy;
m_cx = cx;
m_cy = cy;
}
void PointCloudProcessor::setZScaleFactor(float scale)
{
m_zScale = scale;
}
bool PointCloudProcessor::initializeOpenCL()
{
if (m_clInitialized) {
return true;
}
cl_int err;
qDebug() << "[OpenCL] Starting initialization...";
// 1. 获取平台
err = clGetPlatformIDs(1, &m_platform, nullptr);
if (err != CL_SUCCESS) {
emit errorOccurred("Failed to get OpenCL platform");
return false;
}
// 2. 获取设备（优先GPU）
err = clGetDeviceIDs(m_platform, CL_DEVICE_TYPE_GPU, 1, &m_device, nullptr);
if (err != CL_SUCCESS) {
qDebug() << "[OpenCL] GPU not available, using CPU";
err = clGetDeviceIDs(m_platform, CL_DEVICE_TYPE_CPU, 1, &m_device, nullptr);
if (err != CL_SUCCESS) {
emit errorOccurred("Failed to get OpenCL device");
return false;
}
}
// 3. 创建上下文
m_context = clCreateContext(nullptr, 1, &m_device, nullptr, nullptr, &err);
if (err != CL_SUCCESS) {
emit errorOccurred("Failed to create OpenCL context");
return false;
}
// 4. 创建命令队列
#if CL_TARGET_OPENCL_VERSION >= 200
cl_queue_properties props[] = { CL_QUEUE_ON_DEVICE_DEFAULT, 0 };
m_queue = clCreateCommandQueueWithProperties(m_context, m_device, props, &err);
#else
m_queue = clCreateCommandQueue(m_context, m_device, 0, &err);
#endif
if (err != CL_SUCCESS) {
emit errorOccurred("Failed to create command queue");
cleanupOpenCL();
return false;
}
qDebug() << "[OpenCL] Platform, device, context, and queue created successfully";
// 5. 创建并编译OpenCL程序
const char* kernelSource =
"__kernel void compute_xyz(__global const float* depth, "
"__global float* xyz, int width, int height, "
"float inv_fx, float inv_fy, float cx, float cy, float z_scale) { "
"int idx = get_global_id(0); "
"if (idx >= width * height) return; "
"int y = idx / width; "
"int x = idx % width; "
"float z = depth[idx] * z_scale; "
// 完全平面的圆柱投影：X和Y直接使用像素坐标，缩放到合适的范围
"xyz[idx*3] = (x - cx) * 2.0f; " // X坐标，缩放系数2.0
"xyz[idx*3+1] = (y - cy) * 2.0f; " // Y坐标，缩放系数2.0
"xyz[idx*3+2] = z; "
"}";
m_program = clCreateProgramWithSource(m_context, 1, &kernelSource, nullptr, &err);
if (err != CL_SUCCESS) {
emit errorOccurred("Failed to create OpenCL program");
cleanupOpenCL();
return false;
}
err = clBuildProgram(m_program, 1, &m_device, nullptr, nullptr, nullptr);
if (err != CL_SUCCESS) {
char buildLog[4096];
clGetProgramBuildInfo(m_program, m_device, CL_PROGRAM_BUILD_LOG, sizeof(buildLog), buildLog, nullptr);
emit errorOccurred(QString("Failed to build OpenCL program: %1").arg(buildLog));
cleanupOpenCL();
return false;
}
qDebug() << "[OpenCL] Program compiled successfully";
// 6. 创建kernel
m_kernel = clCreateKernel(m_program, "compute_xyz", &err);
if (err != CL_SUCCESS) {
emit errorOccurred("Failed to create OpenCL kernel");
cleanupOpenCL();
return false;
}
// 7. 创建缓冲区
m_depthBuffer = clCreateBuffer(m_context, CL_MEM_READ_ONLY,
sizeof(float) * m_totalPoints, nullptr, &err);
if (err != CL_SUCCESS) {
emit errorOccurred("Failed to create depth buffer");
cleanupOpenCL();
return false;
}
m_xyzBuffer = clCreateBuffer(m_context, CL_MEM_WRITE_ONLY,
sizeof(float) * m_totalPoints * 3, nullptr, &err);
if (err != CL_SUCCESS) {
emit errorOccurred("Failed to create XYZ buffer");
cleanupOpenCL();
return false;
}
m_clInitialized = true;
qDebug() << "[OpenCL] Initialization complete";
return true;
}
void PointCloudProcessor::processDepthData(const QByteArray &depthData, uint32_t blockId)
{
if (!m_clInitialized) {
if (!initializeOpenCL()) {
return;
}
}
// 验证数据大小
if (depthData.size() != m_totalPoints * sizeof(int16_t)) {
qDebug() << "[PointCloud] Invalid depth data size:" << depthData.size()
<< "expected:" << (m_totalPoints * sizeof(int16_t));
return;
}
// 转换int16_t到float
const int16_t* depthShort = reinterpret_cast<const int16_t*>(depthData.constData());
std::vector<float> depthFloat(m_totalPoints);
for (size_t i = 0; i < m_totalPoints; i++) {
depthFloat[i] = static_cast<float>(depthShort[i]);
}
// 注释掉频繁的日志输出
// qDebug() << "[PointCloud] Processing block" << blockId << "with" << m_totalPoints << "points";
// 上传深度数据到GPU
cl_int err = clEnqueueWriteBuffer(m_queue, m_depthBuffer, CL_TRUE, 0,
sizeof(float) * m_totalPoints, depthFloat.data(),
0, nullptr, nullptr);
if (err != CL_SUCCESS) {
emit errorOccurred(QString("Failed to upload depth data: %1").arg(err));
return;
}
// 设置kernel参数
float inv_fx = 1.0f / m_fx;
float inv_fy = 1.0f / m_fy;
int width = m_imageWidth;
int height = m_imageHeight;
clSetKernelArg(m_kernel, 0, sizeof(cl_mem), &m_depthBuffer);
clSetKernelArg(m_kernel, 1, sizeof(cl_mem), &m_xyzBuffer);
clSetKernelArg(m_kernel, 2, sizeof(int), &width);
clSetKernelArg(m_kernel, 3, sizeof(int), &height);
clSetKernelArg(m_kernel, 4, sizeof(float), &inv_fx);
clSetKernelArg(m_kernel, 5, sizeof(float), &inv_fy);
clSetKernelArg(m_kernel, 6, sizeof(float), &m_cx);
clSetKernelArg(m_kernel, 7, sizeof(float), &m_cy);
clSetKernelArg(m_kernel, 8, sizeof(float), &m_zScale);
// 执行kernel
size_t local = 256;
size_t global = ((m_totalPoints + local - 1) / local) * local;
err = clEnqueueNDRangeKernel(m_queue, m_kernel, 1, nullptr,
&global, &local, 0, nullptr, nullptr);
if (err != CL_SUCCESS) {
emit errorOccurred(QString("Failed to execute kernel: %1").arg(err));
return;
}
clFinish(m_queue);
// 读取结果
std::vector<float> xyz(m_totalPoints * 3);
err = clEnqueueReadBuffer(m_queue, m_xyzBuffer, CL_TRUE, 0,
sizeof(float) * m_totalPoints * 3, xyz.data(),
0, nullptr, nullptr);
if (err != CL_SUCCESS) {
emit errorOccurred(QString("Failed to read XYZ data: %1").arg(err));
return;
}
// 创建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);
// 填充点云数据（过滤全零点）
for (size_t i = 0; i < m_totalPoints; i++) {
cloud->points[i].x = xyz[i * 3];
cloud->points[i].y = xyz[i * 3 + 1];
cloud->points[i].z = xyz[i * 3 + 2];
}
// 注释掉频繁的日志输出
// qDebug() << "[PointCloud] Block" << blockId << "processed successfully";
emit pointCloudReady(cloud, blockId);
}
void PointCloudProcessor::cleanupOpenCL()
{
if (m_depthBuffer) {
clReleaseMemObject(m_depthBuffer);
m_depthBuffer = nullptr;
}
if (m_xyzBuffer) {
clReleaseMemObject(m_xyzBuffer);
m_xyzBuffer = nullptr;
}
if (m_kernel) {
clReleaseKernel(m_kernel);
m_kernel = nullptr;
}
if (m_program) {
clReleaseProgram(m_program);
m_program = nullptr;
}
if (m_queue) {
clReleaseCommandQueue(m_queue);
m_queue = nullptr;
}
if (m_context) {
clReleaseContext(m_context);
m_context = nullptr;
}
m_clInitialized = false;
qDebug() << "[OpenCL] Cleanup complete";
}