1744 lines
62 KiB
C++
1744 lines
62 KiB
C++
#include "VrCameraSimulator.h"
|
|
#include "LaserDataLoader.h"
|
|
|
|
#include <QDebug>
|
|
#include <QDir>
|
|
#include <QFileInfo>
|
|
#include <QDirIterator>
|
|
|
|
#include <cstring>
|
|
#include <random>
|
|
#include <chrono>
|
|
#include <algorithm>
|
|
|
|
namespace {
|
|
|
|
/// Enumerate local adapters and return the first non-loopback IPv4 address.
|
|
/// Used so the discovery response advertises an IP the SDK can actually
|
|
/// connect to on TCP 6679 (getsockname on an INADDR_ANY socket gives 0.0.0.0).
|
|
bool GetFirstLocalIPv4(unsigned char out[4])
|
|
{
|
|
#ifdef _WIN32
|
|
char hostname[256] = {};
|
|
if (gethostname(hostname, sizeof(hostname)) != 0)
|
|
return false;
|
|
|
|
addrinfo hints = {};
|
|
hints.ai_family = AF_INET;
|
|
hints.ai_socktype = SOCK_DGRAM;
|
|
hints.ai_flags = AI_PASSIVE;
|
|
|
|
addrinfo* pResult = nullptr;
|
|
if (getaddrinfo(hostname, nullptr, &hints, &pResult) != 0)
|
|
return false;
|
|
|
|
bool found = false;
|
|
for (addrinfo* p = pResult; p != nullptr; p = p->ai_next) {
|
|
if (p->ai_family == AF_INET) {
|
|
const sockaddr_in* sa = reinterpret_cast<const sockaddr_in*>(p->ai_addr);
|
|
const unsigned long ip = ntohl(sa->sin_addr.s_addr);
|
|
// Skip loopback (127.x.x.x) and link-local (169.254.x.x).
|
|
if (ip != 0x7f000001 && (ip & 0xffff0000) != 0xa9fe0000) {
|
|
out[0] = static_cast<unsigned char>((ip >> 24) & 0xFF);
|
|
out[1] = static_cast<unsigned char>((ip >> 16) & 0xFF);
|
|
out[2] = static_cast<unsigned char>((ip >> 8) & 0xFF);
|
|
out[3] = static_cast<unsigned char>(ip & 0xFF);
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
freeaddrinfo(pResult);
|
|
return found;
|
|
#else
|
|
(void)out;
|
|
return false;
|
|
#endif
|
|
}
|
|
|
|
} // namespace
|
|
|
|
// ============================================================================
|
|
// Constructor / Destructor
|
|
// ============================================================================
|
|
|
|
VrCameraSimulator::VrCameraSimulator(QObject* parent)
|
|
: QObject(parent)
|
|
{
|
|
InitWinsock();
|
|
InitRegisterMap();
|
|
|
|
// Generate a deterministic-but-unique serial number and MAC
|
|
m_deviceIP = QString("%1.%2.%3.%4")
|
|
.arg(m_deviceIPBytes[0]).arg(m_deviceIPBytes[1])
|
|
.arg(m_deviceIPBytes[2]).arg(m_deviceIPBytes[3]);
|
|
|
|
m_deviceMAC = QString("%1:%2:%3:%4:%5:%6")
|
|
.arg(m_deviceMACBytes[0], 2, 16, QChar('0'))
|
|
.arg(m_deviceMACBytes[1], 2, 16, QChar('0'))
|
|
.arg(m_deviceMACBytes[2], 2, 16, QChar('0'))
|
|
.arg(m_deviceMACBytes[3], 2, 16, QChar('0'))
|
|
.arg(m_deviceMACBytes[4], 2, 16, QChar('0'))
|
|
.arg(m_deviceMACBytes[5], 2, 16, QChar('0'));
|
|
|
|
m_serialNumber = QString::fromLatin1(
|
|
reinterpret_cast<const char*>(m_deviceSNBytes), 8);
|
|
|
|
// Default calibration matrix: identity 4x4 (16 doubles)
|
|
m_calibMatrix.resize(16 * sizeof(double));
|
|
double* mat = reinterpret_cast<double*>(m_calibMatrix.data());
|
|
for (int i = 0; i < 16; i++) mat[i] = (i % 5 == 0) ? 1.0 : 0.0;
|
|
}
|
|
|
|
VrCameraSimulator::~VrCameraSimulator()
|
|
{
|
|
Stop();
|
|
}
|
|
|
|
// ============================================================================
|
|
// Configuration
|
|
// ============================================================================
|
|
|
|
void VrCameraSimulator::SetImageWidth(int w) { if (w > 0 && w <= 4096) m_imageWidth = w; }
|
|
void VrCameraSimulator::SetImageHeight(int h) { if (h > 0 && h <= 4096) m_imageHeight = h; }
|
|
void VrCameraSimulator::SetFrameRate(int fps) { if (fps > 0 && fps <= 120) m_frameRate = fps; }
|
|
|
|
// ============================================================================
|
|
// Lifecycle: Start / Stop
|
|
// ============================================================================
|
|
|
|
bool VrCameraSimulator::Start()
|
|
{
|
|
if (m_running) return true;
|
|
|
|
// Create UDP socket for discovery responder
|
|
if (!CreateUdpSocket()) {
|
|
emit LogMessage("Failed to bind UDP port 6789", true);
|
|
return false;
|
|
}
|
|
|
|
// Create TCP listening socket for command channel
|
|
if (!CreateTcpListenSocket()) {
|
|
emit LogMessage("Failed to bind TCP port 6679", true);
|
|
closesocket(m_udpSocket);
|
|
m_udpSocket = INVALID_SOCKET;
|
|
return false;
|
|
}
|
|
|
|
m_running = true;
|
|
|
|
// Launch threads
|
|
m_udpThread = std::thread(&VrCameraSimulator::UdpListenerLoop, this);
|
|
m_tcpThread = std::thread(&VrCameraSimulator::TcpServerLoop, this);
|
|
|
|
emit StatusChanged("Running");
|
|
emit LogMessage(QString("Virtual camera started on %1 (UDP:%2, TCP:%3)")
|
|
.arg(m_deviceIP).arg(m_udpPort).arg(m_tcpPort));
|
|
return true;
|
|
}
|
|
|
|
void VrCameraSimulator::Stop()
|
|
{
|
|
if (!m_running) return;
|
|
|
|
m_running = false;
|
|
m_streaming = false;
|
|
|
|
// Close all sockets to unblock the server/client threads.
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_clientMutex);
|
|
for (SOCKET s : m_clientSockets) {
|
|
closesocket(s);
|
|
}
|
|
m_clientSockets.clear();
|
|
if (m_tcpClientSocket != INVALID_SOCKET) {
|
|
closesocket(m_tcpClientSocket);
|
|
m_tcpClientSocket = INVALID_SOCKET;
|
|
}
|
|
}
|
|
if (m_tcpListenSocket != INVALID_SOCKET) {
|
|
closesocket(m_tcpListenSocket);
|
|
m_tcpListenSocket = INVALID_SOCKET;
|
|
}
|
|
if (m_udpSocket != INVALID_SOCKET) {
|
|
closesocket(m_udpSocket);
|
|
m_udpSocket = INVALID_SOCKET;
|
|
}
|
|
|
|
// Join threads
|
|
if (m_udpThread.joinable()) m_udpThread.join();
|
|
if (m_tcpThread.joinable()) m_tcpThread.join();
|
|
if (m_clientThread.joinable()) m_clientThread.join();
|
|
if (m_streamThread.joinable()) m_streamThread.join();
|
|
|
|
m_connectedClients = 0;
|
|
emit StatusChanged("Stopped");
|
|
emit LogMessage("Virtual camera stopped");
|
|
}
|
|
|
|
// ============================================================================
|
|
// Winsock Initialization
|
|
// ============================================================================
|
|
|
|
bool VrCameraSimulator::InitWinsock()
|
|
{
|
|
#ifdef _WIN32
|
|
WSADATA wsaData;
|
|
return WSAStartup(MAKEWORD(2, 2), &wsaData) == 0;
|
|
#else
|
|
return true;
|
|
#endif
|
|
}
|
|
|
|
void VrCameraSimulator::SetSocketReuseAddr(SOCKET s)
|
|
{
|
|
int opt = 1;
|
|
setsockopt(s, SOL_SOCKET, SO_REUSEADDR,
|
|
#ifdef _WIN32
|
|
reinterpret_cast<const char*>(&opt), sizeof(opt));
|
|
#else
|
|
&opt, sizeof(opt));
|
|
#endif
|
|
}
|
|
|
|
bool VrCameraSimulator::CreateUdpSocket()
|
|
{
|
|
m_udpSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
|
|
if (m_udpSocket == INVALID_SOCKET) return false;
|
|
|
|
SetSocketReuseAddr(m_udpSocket);
|
|
|
|
// Enable broadcast reception
|
|
int broadcast = 1;
|
|
setsockopt(m_udpSocket, SOL_SOCKET, SO_BROADCAST,
|
|
#ifdef _WIN32
|
|
reinterpret_cast<const char*>(&broadcast), sizeof(broadcast));
|
|
#else
|
|
&broadcast, sizeof(broadcast));
|
|
#endif
|
|
|
|
sockaddr_in addr = {};
|
|
addr.sin_family = AF_INET;
|
|
addr.sin_port = htons(static_cast<uint16_t>(m_udpPort));
|
|
addr.sin_addr.s_addr = INADDR_ANY;
|
|
|
|
if (bind(m_udpSocket, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) == SOCKET_ERROR) {
|
|
closesocket(m_udpSocket);
|
|
m_udpSocket = INVALID_SOCKET;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool VrCameraSimulator::CreateTcpListenSocket()
|
|
{
|
|
m_tcpListenSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
|
|
if (m_tcpListenSocket == INVALID_SOCKET) return false;
|
|
|
|
SetSocketReuseAddr(m_tcpListenSocket);
|
|
|
|
sockaddr_in addr = {};
|
|
addr.sin_family = AF_INET;
|
|
addr.sin_port = htons(static_cast<uint16_t>(m_tcpPort));
|
|
addr.sin_addr.s_addr = INADDR_ANY;
|
|
|
|
if (bind(m_tcpListenSocket, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) == SOCKET_ERROR) {
|
|
closesocket(m_tcpListenSocket);
|
|
m_tcpListenSocket = INVALID_SOCKET;
|
|
return false;
|
|
}
|
|
|
|
if (listen(m_tcpListenSocket, 10) == SOCKET_ERROR) {
|
|
closesocket(m_tcpListenSocket);
|
|
m_tcpListenSocket = INVALID_SOCKET;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
// ============================================================================
|
|
// UDP Discovery Listener Thread
|
|
// ============================================================================
|
|
|
|
void VrCameraSimulator::UdpListenerLoop()
|
|
{
|
|
emit LogMessage("UDP discovery listener started on port " + QString::number(m_udpPort));
|
|
|
|
// Set receive timeout so we can check m_running periodically
|
|
#ifdef _WIN32
|
|
DWORD timeout = 500;
|
|
setsockopt(m_udpSocket, SOL_SOCKET, SO_RCVTIMEO,
|
|
reinterpret_cast<const char*>(&timeout), sizeof(timeout));
|
|
#else
|
|
struct timeval tv = {0, 500000};
|
|
setsockopt(m_udpSocket, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
|
|
#endif
|
|
|
|
while (m_running) {
|
|
sockaddr_in senderAddr = {};
|
|
socklen_t addrLen = sizeof(senderAddr);
|
|
|
|
int n = recvfrom(m_udpSocket,
|
|
#ifdef _WIN32
|
|
reinterpret_cast<char*>(m_udpRecvBuf),
|
|
#else
|
|
m_udpRecvBuf,
|
|
#endif
|
|
UDP_BUF_SIZE, 0,
|
|
reinterpret_cast<sockaddr*>(&senderAddr), &addrLen);
|
|
|
|
if (n <= 0) {
|
|
// Timeout or error — loop back to check m_running
|
|
continue;
|
|
}
|
|
|
|
// Check for discovery request: preamble 0xe1 0xe2 0xe3 0xe4
|
|
if (n >= 34 && m_udpRecvBuf[0] == 0xe1 && m_udpRecvBuf[1] == 0xe2 &&
|
|
m_udpRecvBuf[2] == 0xe3 && m_udpRecvBuf[3] == 0xe4) {
|
|
|
|
// Check for "SB" packet start marker at offset 12
|
|
if (m_udpRecvBuf[12] == 0x53 && m_udpRecvBuf[13] == 0x42) {
|
|
// cmdType at offset 14-15 (big-endian)
|
|
uint16_t cmdType = (static_cast<uint16_t>(m_udpRecvBuf[14]) << 8)
|
|
| static_cast<uint16_t>(m_udpRecvBuf[15]);
|
|
|
|
if (cmdType == 0x0002) { // SearchDev
|
|
char senderIP[INET_ADDRSTRLEN];
|
|
inet_ntop(AF_INET, &senderAddr.sin_addr, senderIP, sizeof(senderIP));
|
|
|
|
emit LogMessage(QString("Discovery request from %1").arg(senderIP));
|
|
|
|
QByteArray response = BuildDiscoveryResponse(
|
|
QByteArray(reinterpret_cast<const char*>(m_udpRecvBuf), n),
|
|
senderAddr);
|
|
|
|
if (!response.isEmpty()) {
|
|
sendto(m_udpSocket, response.constData(), response.size(), 0,
|
|
reinterpret_cast<const sockaddr*>(&senderAddr), sizeof(senderAddr));
|
|
emit LogMessage("Sent discovery response to " + QString(senderIP));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
emit LogMessage("UDP listener stopped");
|
|
}
|
|
|
|
// ============================================================================
|
|
// TCP Server Thread (accept loop)
|
|
// ============================================================================
|
|
|
|
void VrCameraSimulator::TcpServerLoop()
|
|
{
|
|
emit LogMessage("TCP server listening on port " + QString::number(m_tcpPort));
|
|
|
|
// Set accept timeout so we can poll m_running.
|
|
#ifdef _WIN32
|
|
DWORD timeout = 500;
|
|
setsockopt(m_tcpListenSocket, SOL_SOCKET, SO_RCVTIMEO,
|
|
reinterpret_cast<const char*>(&timeout), sizeof(timeout));
|
|
#else
|
|
struct timeval tv = {0, 500000};
|
|
setsockopt(m_tcpListenSocket, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
|
|
#endif
|
|
|
|
while (m_running) {
|
|
sockaddr_in clientAddr = {};
|
|
socklen_t addrLen = sizeof(clientAddr);
|
|
|
|
SOCKET clientSock = accept(m_tcpListenSocket,
|
|
reinterpret_cast<sockaddr*>(&clientAddr),
|
|
&addrLen);
|
|
|
|
if (clientSock == INVALID_SOCKET) {
|
|
// Timeout or error — loop back to check m_running
|
|
continue;
|
|
}
|
|
|
|
char clientIP[INET_ADDRSTRLEN];
|
|
inet_ntop(AF_INET, &clientAddr.sin_addr, clientIP, sizeof(clientIP));
|
|
|
|
emit LogMessage(QString("TCP client connected from %1").arg(clientIP));
|
|
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_clientMutex);
|
|
// Replace the current push target but keep old client sockets alive
|
|
// so they can still receive their own heartbeat responses.
|
|
m_tcpClientSocket = clientSock;
|
|
m_clientSockets.push_back(clientSock);
|
|
m_connectedClients = static_cast<int>(m_clientSockets.size());
|
|
}
|
|
|
|
m_frameSeq = 0;
|
|
emit ClientConnected(QString(clientIP));
|
|
emit StatusChanged("Client connected");
|
|
|
|
// Handle this client on its own thread so a slow/stuck client never
|
|
// blocks accept() for new connections.
|
|
std::thread(&VrCameraSimulator::TcpClientLoop, this, clientSock).detach();
|
|
}
|
|
}
|
|
|
|
void VrCameraSimulator::RemoveClientSocket(SOCKET s)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_clientMutex);
|
|
auto it = std::find(m_clientSockets.begin(), m_clientSockets.end(), s);
|
|
if (it != m_clientSockets.end()) {
|
|
m_clientSockets.erase(it);
|
|
}
|
|
if (m_tcpClientSocket == s) {
|
|
m_tcpClientSocket = INVALID_SOCKET;
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// TCP Client Handler
|
|
// ============================================================================
|
|
|
|
void VrCameraSimulator::TcpClientLoop(SOCKET clientSock)
|
|
{
|
|
uint8_t buf[TCP_BUF_SIZE];
|
|
|
|
// On Windows an accepted socket INHERITS the listen socket's SO_RCVTIMEO
|
|
// (500 ms here). SDK heartbeats arrive every few seconds, so a short recv
|
|
// timeout makes us drop a healthy client. Clear the timeout so recv blocks
|
|
// indefinitely; Stop() closes this socket to wake recv.
|
|
#ifdef _WIN32
|
|
DWORD clientTimeout = 0;
|
|
setsockopt(clientSock, SOL_SOCKET, SO_RCVTIMEO,
|
|
reinterpret_cast<const char*>(&clientTimeout), sizeof(clientTimeout));
|
|
#else
|
|
struct timeval clientTimeout = {0, 0};
|
|
setsockopt(clientSock, SOL_SOCKET, SO_RCVTIMEO, &clientTimeout, sizeof(clientTimeout));
|
|
#endif
|
|
|
|
// Each client thread keeps its own frame buffer.
|
|
std::vector<uint8_t> recvBuf(TCP_BUF_SIZE);
|
|
int recvPos = 0;
|
|
|
|
while (m_running) {
|
|
int n = recv(clientSock,
|
|
#ifdef _WIN32
|
|
reinterpret_cast<char*>(buf),
|
|
#else
|
|
buf,
|
|
#endif
|
|
TCP_BUF_SIZE, 0);
|
|
|
|
if (n <= 0) {
|
|
emit LogMessage("TCP client disconnected (recv returned " + QString::number(n) + ")");
|
|
break; // Connection closed or error
|
|
}
|
|
|
|
// Append new data to this client's receive buffer
|
|
if (recvPos + n > TCP_BUF_SIZE) {
|
|
recvPos = 0; // safety: drop stale data
|
|
}
|
|
memcpy(recvBuf.data() + recvPos, buf, n);
|
|
recvPos += n;
|
|
|
|
// Try to parse complete frames from the buffer
|
|
while (m_running) {
|
|
TcpOperaType op;
|
|
uint32_t cmd, seq;
|
|
QByteArray payload;
|
|
int consumed = 0;
|
|
|
|
bool parsed = ParseVzebFrame(recvBuf.data(), recvPos, consumed,
|
|
op, cmd, seq, payload);
|
|
if (parsed && consumed > 0) {
|
|
// Shift remaining data
|
|
int remaining = recvPos - consumed;
|
|
if (remaining > 0) {
|
|
memmove(recvBuf.data(), recvBuf.data() + consumed, remaining);
|
|
}
|
|
recvPos = remaining;
|
|
}
|
|
|
|
if (!parsed) break;
|
|
|
|
if (op == TcpOperaType::Request) {
|
|
emit LogMessage(QString("TCP Cmd %1 seq=%2 payload=%3 bytes")
|
|
.arg(cmd).arg(seq).arg(payload.size()));
|
|
|
|
QByteArray response = HandleCommand(cmd, seq, payload);
|
|
|
|
if (!response.isEmpty()) {
|
|
std::lock_guard<std::mutex> lock(m_sendMutex);
|
|
int sent = send(clientSock, response.constData(), response.size(), 0);
|
|
if (sent <= 0) {
|
|
emit LogMessage("TCP send failed", true);
|
|
}
|
|
}
|
|
}
|
|
// Post (push) messages from camera are handled in StreamLoop
|
|
}
|
|
}
|
|
|
|
// Clean up this client's socket.
|
|
RemoveClientSocket(clientSock);
|
|
closesocket(clientSock);
|
|
}
|
|
|
|
// ============================================================================
|
|
// Stream Thread
|
|
// ============================================================================
|
|
|
|
void VrCameraSimulator::StreamLoop()
|
|
{
|
|
emit LogMessage("Stream engine started");
|
|
|
|
int frameIdx = 0;
|
|
bool lastLine = false;
|
|
using Clock = std::chrono::steady_clock;
|
|
auto nextFrameTime = Clock::now();
|
|
|
|
while (m_running && m_streaming) {
|
|
// Grab the next laser line (cycles through loaded files; fall back to
|
|
// an empty line if no laser data was loaded). GetNextLaserLine locks
|
|
// the laser-data mutex itself.
|
|
CachedLaserLine line;
|
|
if (!m_laserFileLines.empty()) {
|
|
line = GetNextLaserLine();
|
|
}
|
|
|
|
// Grab the current stereo image pair (loaded or checkerboard fallback).
|
|
QImage leftImg, rightImg;
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_imageMutex);
|
|
if (!m_leftImages.empty()) {
|
|
int idx = frameIdx % static_cast<int>(m_leftImages.size());
|
|
leftImg = m_leftImages[idx];
|
|
rightImg = (idx < static_cast<int>(m_rightImages.size()))
|
|
? m_rightImages[idx] : m_rightImages[0];
|
|
}
|
|
}
|
|
if (leftImg.isNull()) {
|
|
leftImg = GenerateCheckerboardQImage(frameIdx, 0, 0);
|
|
rightImg = GenerateCheckerboardQImage(frameIdx, 32, 32);
|
|
}
|
|
|
|
emit ImageGenerated(leftImg, rightImg);
|
|
|
|
// Mark the last line of a scan cycle so the app sees bEndOnceScan.
|
|
if (m_scanLineIdx > 0 && m_scanLineIdx % m_scanLinesPerCycle == 0) {
|
|
lastLine = true;
|
|
} else {
|
|
lastLine = false;
|
|
}
|
|
|
|
// Build and push the SDK-compatible 3D laser frame.
|
|
QByteArray frame = BuildLaserFrame(line, frameIdx, leftImg, rightImg, lastLine);
|
|
if (!frame.isEmpty()) {
|
|
SendPostCommand(Cmd_PushLaserResult, frame);
|
|
}
|
|
|
|
m_frameSeq = frameIdx;
|
|
m_scanLineIdx++;
|
|
frameIdx++;
|
|
|
|
// Frame rate pacing
|
|
nextFrameTime += std::chrono::milliseconds(1000 / (std::max)(1, m_frameRate));
|
|
auto now = Clock::now();
|
|
if (nextFrameTime > now) {
|
|
std::this_thread::sleep_until(nextFrameTime);
|
|
} else {
|
|
nextFrameTime = now + std::chrono::milliseconds(1000 / (std::max)(1, m_frameRate));
|
|
}
|
|
}
|
|
|
|
emit LogMessage("Stream engine stopped");
|
|
}
|
|
|
|
// ============================================================================
|
|
// 3D Laser Frame Builder (SDK-compatible PushLaserResult payload)
|
|
// ============================================================================
|
|
// Layout (laser product, n3DTotleHeadLength=256, n3DFrameHeadLength=104):
|
|
// [0..139] index head (140 = 256 - 20 - 96). pOffsetVal[0]=left image
|
|
// offset, [1]=right image offset, [2]=3D offset (all relative to
|
|
// pFrameInfoData = payload + 140).
|
|
// [140..] pFrameInfoData:
|
|
// [+0] frame idx (uint; low 16 bits since LaserDataVersion>=1)
|
|
// [+4] label = 4 (uint)
|
|
// [+8] encode info (uint): low 16 = encodeNo, bit0x00010000 = last line
|
|
// [+12] timestamp (uint)
|
|
// [+16] RX (ushort)
|
|
// [+18] LX (ushort)
|
|
// [+20] height (ushort)
|
|
// [+22] width (ushort)
|
|
// [+24] 4 unread bytes
|
|
// [+28] nPointCnt (ushort)
|
|
// [+30] Y (ushort)
|
|
// [+32..63] left image info (label=1)
|
|
// [+64..95] right image info (label=2)
|
|
// [+96..127] center image info (zeros)
|
|
// [+128..] data area:
|
|
// 3D points (nPointCnt * 16, each x/y/z float + nUV uint)
|
|
// left image pixels (h*w, 8-bit gray)
|
|
// right image pixels (h*w, 8-bit gray)
|
|
QByteArray VrCameraSimulator::BuildLaserFrame(const CachedLaserLine& line, int frameIdx,
|
|
const QImage& leftImg, const QImage& rightImg,
|
|
bool lastLine)
|
|
{
|
|
const int nIndexHeadSize = 140; // 256 - 20 - 96
|
|
const int nInfoHeadSize = 128; // 3D info(32) + left(32) + right(32) + center(32)
|
|
const int n3DOffset = nInfoHeadSize; // 128, relative to pFrameInfoData
|
|
|
|
// Clamp point count to SDK's VZ_MAX_POINT_COUNT (3072).
|
|
int nPointCnt = line.pointCount;
|
|
if (nPointCnt > 3072) nPointCnt = 3072;
|
|
if (nPointCnt < 0) nPointCnt = 0;
|
|
|
|
// Image pixel size (clamp to something small so frames stay manageable).
|
|
int imgW = leftImg.width();
|
|
int imgH = leftImg.height();
|
|
if (imgW <= 0 || imgH <= 0) { imgW = 320; imgH = 240; }
|
|
if (imgW > 960) imgW = 960;
|
|
if (imgH > 960) imgH = 960;
|
|
const int imgSize = imgW * imgH;
|
|
|
|
const int nLeftImageOffset = n3DOffset + nPointCnt * 16;
|
|
const int nRightImageOffset = nLeftImageOffset + imgSize;
|
|
|
|
const int totalFrameSize = nIndexHeadSize + nInfoHeadSize + nPointCnt * 16 + 2 * imgSize;
|
|
|
|
QByteArray frame(totalFrameSize, 0);
|
|
uint8_t* d = reinterpret_cast<uint8_t*>(frame.data());
|
|
|
|
// ---- Index head (offsets relative to pFrameInfoData) ----
|
|
uint32_t* pOffsetVal = reinterpret_cast<uint32_t*>(d);
|
|
pOffsetVal[0] = static_cast<uint32_t>(nLeftImageOffset);
|
|
pOffsetVal[1] = static_cast<uint32_t>(nRightImageOffset);
|
|
pOffsetVal[2] = static_cast<uint32_t>(n3DOffset);
|
|
|
|
uint8_t* pInfo = d + nIndexHeadSize; // pFrameInfoData
|
|
|
|
// ---- 3D info header ----
|
|
uint32_t ts = static_cast<uint32_t>(m_frameSeq * 1000 + frameIdx);
|
|
uint32_t encodeInfo = (static_cast<uint32_t>(frameIdx) & 0xffff) |
|
|
(lastLine ? 0x00010000u : 0u);
|
|
uint32_t fIdx16 = static_cast<uint32_t>(frameIdx) & 0xffff;
|
|
|
|
uint32_t* pI = reinterpret_cast<uint32_t*>(pInfo);
|
|
pI[0] = fIdx16; // frame idx
|
|
pI[1] = 4; // label = 4 (3D)
|
|
pI[2] = encodeInfo; // encode info
|
|
pI[3] = ts; // time stamp
|
|
uint16_t* pS = reinterpret_cast<uint16_t*>(pInfo + 16);
|
|
pS[0] = 0; // RX
|
|
pS[1] = 0; // LX
|
|
pS[2] = static_cast<uint16_t>(imgH); // height
|
|
pS[3] = static_cast<uint16_t>(imgW); // width
|
|
pI[6] = 0; // 4 unread bytes
|
|
uint16_t* pCnt = reinterpret_cast<uint16_t*>(pInfo + 28);
|
|
pCnt[0] = static_cast<uint16_t>(nPointCnt);
|
|
pCnt[1] = 0; // Y
|
|
|
|
// ---- Left / right image info ----
|
|
auto fillImageInfo = [&](uint8_t* pImgInfo, uint32_t label) {
|
|
uint32_t* pi = reinterpret_cast<uint32_t*>(pImgInfo);
|
|
pi[0] = fIdx16;
|
|
pi[1] = label;
|
|
uint16_t* ps = reinterpret_cast<uint16_t*>(pImgInfo + 8);
|
|
ps[0] = static_cast<uint16_t>(imgH);
|
|
ps[1] = static_cast<uint16_t>(imgW);
|
|
pi[3] = ts;
|
|
uint16_t* po = reinterpret_cast<uint16_t*>(pImgInfo + 20);
|
|
po[0] = 0; // oriY
|
|
po[1] = 0; // oriX
|
|
};
|
|
fillImageInfo(pInfo + 32, 1); // left, label=1
|
|
fillImageInfo(pInfo + 64, 2); // right, label=2
|
|
|
|
// ---- 3D points (16 bytes each: x,y,z float + nUV uint) ----
|
|
uint8_t* p3D = pInfo + n3DOffset;
|
|
for (int i = 0; i < nPointCnt; i++) {
|
|
float x = (i < static_cast<int>(line.x.size())) ? line.x[i] : 0.0f;
|
|
float y = (i < static_cast<int>(line.y.size())) ? line.y[i] : 0.0f;
|
|
float z = (i < static_cast<int>(line.z.size())) ? line.z[i] : 0.0f;
|
|
memcpy(p3D + i * 16 + 0, &x, 4);
|
|
memcpy(p3D + i * 16 + 4, &y, 4);
|
|
memcpy(p3D + i * 16 + 8, &z, 4);
|
|
// nUV = 0
|
|
}
|
|
|
|
// ---- Image pixels (8-bit gray) ----
|
|
auto copyGray = [](const QImage& img, uint8_t* dst, int w, int h) {
|
|
QImage gray = img.convertToFormat(QImage::Format_Grayscale8);
|
|
QImage scaled = gray.scaled(w, h, Qt::IgnoreAspectRatio, Qt::SmoothTransformation);
|
|
for (int y = 0; y < h; y++) {
|
|
const uint8_t* src = scaled.constScanLine(y);
|
|
memcpy(dst + y * w, src, static_cast<size_t>(w));
|
|
}
|
|
};
|
|
copyGray(leftImg, pInfo + nLeftImageOffset, imgW, imgH);
|
|
copyGray(rightImg, pInfo + nRightImageOffset, imgW, imgH);
|
|
|
|
return frame;
|
|
}
|
|
|
|
void VrCameraSimulator::SendPostCommand(uint32_t cmd, const QByteArray& payload)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_sendMutex);
|
|
SOCKET client = m_tcpClientSocket;
|
|
if (client == INVALID_SOCKET) return;
|
|
|
|
QByteArray frame = PackTcpFrame(TcpOperaType::Post, cmd, m_frameSeq, payload);
|
|
int sent = send(client, frame.constData(), frame.size(), 0);
|
|
if (sent <= 0) {
|
|
emit LogMessage("TCP push send failed", true);
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Command Handlers
|
|
// ============================================================================
|
|
|
|
QByteArray VrCameraSimulator::HandleCommand(uint32_t cmd, uint32_t seq,
|
|
const QByteArray& payload)
|
|
{
|
|
switch (cmd) {
|
|
case Cmd_OpenDevice: return HandleOpenDevice(seq, payload);
|
|
case Cmd_ReadRegister: return HandleReadRegister(seq, payload);
|
|
case Cmd_WriteRegister: return HandleWriteRegister(seq, payload);
|
|
case Cmd_DeviceOption: return HandleDeviceOption(seq, payload);
|
|
case Cmd_ReadData: return HandleReadData(seq, payload);
|
|
case Cmd_WriteData: return HandleWriteData(seq, payload);
|
|
case Cmd_ExtDevice: return HandleExtDevice(seq, payload);
|
|
|
|
case Cmd_StartStream:
|
|
emit LogMessage("StartStream command received");
|
|
// Stop any previous stream thread before starting a new one.
|
|
m_streaming = false;
|
|
if (m_streamThread.joinable()) m_streamThread.join();
|
|
m_streaming = true;
|
|
m_scanLineIdx = 0;
|
|
m_streamThread = std::thread(&VrCameraSimulator::StreamLoop, this);
|
|
return BuildTcpResponse(seq, cmd, QByteArray(), true);
|
|
|
|
case Cmd_StopStream:
|
|
emit LogMessage("StopStream command received");
|
|
m_streaming = false;
|
|
if (m_streamThread.joinable()) m_streamThread.join();
|
|
return BuildTcpResponse(seq, cmd, QByteArray(), true);
|
|
|
|
case Cmd_GetImage: {
|
|
// Synchronous single-frame grab: return a full 3D frame as the
|
|
// response data (SDK parses it identically to a pushed frame).
|
|
emit LogMessage("GetImage command received");
|
|
CachedLaserLine line;
|
|
if (!m_laserFileLines.empty()) line = GetNextLaserLine();
|
|
QImage leftImg, rightImg;
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_imageMutex);
|
|
if (!m_leftImages.empty()) {
|
|
int idx = m_frameSeq % static_cast<int>(m_leftImages.size());
|
|
leftImg = m_leftImages[idx];
|
|
rightImg = (idx < static_cast<int>(m_rightImages.size()))
|
|
? m_rightImages[idx] : m_rightImages[0];
|
|
}
|
|
}
|
|
if (leftImg.isNull()) {
|
|
leftImg = GenerateCheckerboardQImage(m_frameSeq, 0, 0);
|
|
rightImg = GenerateCheckerboardQImage(m_frameSeq, 32, 32);
|
|
}
|
|
QByteArray frame = BuildLaserFrame(line, m_frameSeq, leftImg, rightImg, false);
|
|
m_frameSeq++;
|
|
return BuildTcpResponse(seq, cmd, frame, true);
|
|
}
|
|
|
|
case Cmd_Trigger:
|
|
case Cmd_TriggerN:
|
|
case Cmd_PushLaserResult:
|
|
case Cmd_PushDataEx:
|
|
// These are usually sent from camera to host.
|
|
// When received as a request, acknowledge.
|
|
return BuildTcpResponse(seq, cmd, QByteArray(), true);
|
|
|
|
default:
|
|
emit LogMessage(QString("Unknown command %1, ack'ing anyway").arg(cmd), true);
|
|
return BuildTcpResponse(seq, cmd, QByteArray(), true);
|
|
}
|
|
}
|
|
|
|
QByteArray VrCameraSimulator::HandleOpenDevice(uint32_t seq, const QByteArray& /*payload*/)
|
|
{
|
|
// Build a device capability response.
|
|
// The real device returns a capability struct.
|
|
// We return: resolution (w,h), frame rate range, capability flags.
|
|
QByteArray data;
|
|
data.resize(64); // padded
|
|
|
|
// Resolution
|
|
uint32_t* p = reinterpret_cast<uint32_t*>(data.data());
|
|
p[0] = static_cast<uint32_t>(m_imageWidth);
|
|
p[1] = static_cast<uint32_t>(m_imageHeight);
|
|
|
|
// Capability flags (uint32 bitfield matching SVzXilCapability)
|
|
uint32_t cap = 0;
|
|
cap |= (1 << 1); // bIsSupportRGBSensor
|
|
cap |= (1 << 9); // bSupportSwingMotor
|
|
cap |= (1 << 10); // bSupportDynamicRGBD
|
|
p[2] = cap;
|
|
|
|
// Device type: LaserRobotEye = 3
|
|
p[3] = 3;
|
|
|
|
// Version
|
|
p[4] = 0x01000000; // 1.0.0.0
|
|
|
|
// Product type: LaserEye = 1
|
|
p[5] = 1;
|
|
|
|
emit LogMessage("OpenDevice: returned device capabilities");
|
|
return BuildTcpResponse(seq, Cmd_OpenDevice, data, true);
|
|
}
|
|
|
|
QByteArray VrCameraSimulator::HandleReadRegister(uint32_t seq, const QByteArray& payload)
|
|
{
|
|
// Request payload: 4-byte register address only (see _GetCustomAddr in the SDK).
|
|
if (payload.size() < 4) {
|
|
return BuildTcpResponse(seq, Cmd_ReadRegister, QByteArray(), false);
|
|
}
|
|
|
|
uint32_t addr;
|
|
memcpy(&addr, payload.constData(), 4);
|
|
|
|
// Return the exact-length value stored for this register. The SDK checks
|
|
// that the returned block size matches the caller's expected size.
|
|
QByteArray value = ReadRegister(addr);
|
|
|
|
emit LogMessage(QString("ReadRegister addr=0x%1 len=%2")
|
|
.arg(addr, 8, 16, QChar('0')).arg(value.size()));
|
|
return BuildTcpResponse(seq, Cmd_ReadRegister, value, true);
|
|
}
|
|
|
|
QByteArray VrCameraSimulator::HandleWriteRegister(uint32_t seq, const QByteArray& payload)
|
|
{
|
|
// Request payload: 4-byte register address + register data (see _SetCustomAddr).
|
|
if (payload.size() < 4) {
|
|
return BuildTcpResponse(seq, Cmd_WriteRegister, QByteArray(), false);
|
|
}
|
|
|
|
uint32_t addr;
|
|
memcpy(&addr, payload.constData(), 4);
|
|
|
|
QByteArray value = payload.mid(4);
|
|
WriteRegister(addr, value);
|
|
|
|
// Apply config side-effects
|
|
if (addr == 0x00400014 && value.size() >= 4) { // FPS
|
|
int fps;
|
|
memcpy(&fps, value.constData(), 4);
|
|
if (fps > 0 && fps <= 120) m_frameRate = fps;
|
|
} else if (addr == 0x00400018 && value.size() >= 4) { // EXP
|
|
uint32_t expo;
|
|
memcpy(&expo, value.constData(), 4);
|
|
(void)expo;
|
|
}
|
|
|
|
emit LogMessage(QString("WriteRegister addr=0x%1 len=%2")
|
|
.arg(addr, 8, 16, QChar('0')).arg(value.size()));
|
|
return BuildTcpResponse(seq, Cmd_WriteRegister, QByteArray(), true);
|
|
}
|
|
|
|
QByteArray VrCameraSimulator::HandleDeviceOption(uint32_t seq, const QByteArray& payload)
|
|
{
|
|
// TCP DeviceOption request payload starts with a 2-byte option id
|
|
// (EVzXilinxDeviceOption), optionally followed by config data.
|
|
if (payload.size() < 2) {
|
|
return BuildTcpResponse(seq, Cmd_DeviceOption, QByteArray(), false);
|
|
}
|
|
|
|
uint16_t option;
|
|
memcpy(&option, payload.constData(), 2);
|
|
|
|
QByteArray response;
|
|
|
|
switch (option) {
|
|
case 1: // Query IP
|
|
response.resize(4);
|
|
memcpy(response.data(), m_deviceIPBytes, 4);
|
|
break;
|
|
case 5: // Query IP type (1=Static)
|
|
response.resize(4);
|
|
*reinterpret_cast<uint32_t*>(response.data()) = 1; // Static IP
|
|
break;
|
|
case 6: // Query limit info (frame rate, exposure, gain ranges)
|
|
response.resize(24);
|
|
{
|
|
uint32_t* r = reinterpret_cast<uint32_t*>(response.data());
|
|
r[0] = 1; r[1] = 120; // frame rate min/max
|
|
r[2] = 1; r[3] = 100000; // exposure min/max (us)
|
|
r[4] = 0; r[5] = 255; // gain min/max
|
|
}
|
|
break;
|
|
case 7: // Config network info - acknowledge
|
|
response.resize(4);
|
|
response.fill(0);
|
|
break;
|
|
case 8: // QueryConfigNetWorkInfo: [IPType(4)][IP(4)][mask(4)][gateway(4)]
|
|
response.resize(16);
|
|
{
|
|
uint32_t* r = reinterpret_cast<uint32_t*>(response.data());
|
|
r[0] = 1; // IPType = StaticIP
|
|
memcpy(r + 1, m_deviceIPBytes, 4);
|
|
// mask 255.255.255.0
|
|
response[8] = static_cast<char>(255);
|
|
response[9] = static_cast<char>(255);
|
|
response[10] = static_cast<char>(255);
|
|
response[11] = 0;
|
|
// gateway: same subnet, .1
|
|
response[12] = m_deviceIPBytes[0];
|
|
response[13] = m_deviceIPBytes[1];
|
|
response[14] = m_deviceIPBytes[2];
|
|
response[15] = 1;
|
|
}
|
|
break;
|
|
default:
|
|
response.resize(4);
|
|
response.fill(0);
|
|
break;
|
|
}
|
|
|
|
emit LogMessage(QString("DeviceOption option=%1").arg(option));
|
|
return BuildTcpResponse(seq, Cmd_DeviceOption, response, true);
|
|
}
|
|
|
|
QByteArray VrCameraSimulator::HandleReadData(uint32_t seq, const QByteArray& payload)
|
|
{
|
|
if (payload.size() < 8) {
|
|
return BuildTcpResponse(seq, Cmd_ReadData, QByteArray(), false);
|
|
}
|
|
|
|
const uint32_t* p = reinterpret_cast<const uint32_t*>(payload.constData());
|
|
uint32_t addr = p[0]; // start address
|
|
uint32_t dataLen = p[1]; // bytes to read
|
|
|
|
QByteArray response;
|
|
response.resize(static_cast<int>(dataLen));
|
|
response.fill(0);
|
|
|
|
// Return calibration matrix for known ranges
|
|
if (addr >= 0x00300000 && addr < 0x00400000) {
|
|
// Calibration data region
|
|
int offset = static_cast<int>(addr - 0x00300000);
|
|
if (offset < m_calibMatrix.size()) {
|
|
int copyLen = (std::min)(static_cast<int>(dataLen),
|
|
m_calibMatrix.size() - offset);
|
|
memcpy(response.data(), m_calibMatrix.constData() + offset, copyLen);
|
|
}
|
|
} else if (addr >= 0x40000000 && addr < 0x48000000) {
|
|
// User data region
|
|
int offset = static_cast<int>(addr - 0x40000000);
|
|
if (offset < m_userData.size()) {
|
|
int copyLen = (std::min)(static_cast<int>(dataLen),
|
|
m_userData.size() - offset);
|
|
memcpy(response.data(), m_userData.constData() + offset, copyLen);
|
|
}
|
|
}
|
|
|
|
emit LogMessage(QString("ReadData addr=0x%1 len=%2").arg(addr, 8, 16, QChar('0')).arg(dataLen));
|
|
return BuildTcpResponse(seq, Cmd_ReadData, response, true);
|
|
}
|
|
|
|
QByteArray VrCameraSimulator::HandleWriteData(uint32_t seq, const QByteArray& payload)
|
|
{
|
|
if (payload.size() < 8) {
|
|
return BuildTcpResponse(seq, Cmd_WriteData, QByteArray(), false);
|
|
}
|
|
|
|
const uint32_t* p = reinterpret_cast<const uint32_t*>(payload.constData());
|
|
uint32_t addr = p[0];
|
|
|
|
int dataLen = payload.size() - 8;
|
|
QByteArray data = payload.mid(8, dataLen);
|
|
|
|
// Store in user data region
|
|
if (addr >= 0x40000000 && addr < 0x48000000) {
|
|
int offset = static_cast<int>(addr - 0x40000000);
|
|
if (m_userData.size() < offset + dataLen) {
|
|
m_userData.resize(offset + dataLen);
|
|
}
|
|
memcpy(m_userData.data() + offset, data.constData(), dataLen);
|
|
}
|
|
|
|
emit LogMessage(QString("WriteData addr=0x%1 len=%2").arg(addr, 8, 16, QChar('0')).arg(dataLen));
|
|
return BuildTcpResponse(seq, Cmd_WriteData, QByteArray(), true);
|
|
}
|
|
|
|
// ============================================================================
|
|
// External Device handler (swing motor over ExtDevice command).
|
|
// Request payload: [extType uint][5 x ushort swing command] (14 bytes).
|
|
// The swing command bytes are: [0]=read/write, [1]=command code (LE ushort
|
|
// at payload[6..7]), [2]=register addr, [3..4]=value. See VzSwingMotorAPI.h
|
|
// c_sSwingMotorCommand[]. Response data = 4-byte result value.
|
|
// ============================================================================
|
|
QByteArray VrCameraSimulator::HandleExtDevice(uint32_t seq, const QByteArray& payload)
|
|
{
|
|
if (payload.size() < 14) {
|
|
// Could be a config write without an expected data block; still ack.
|
|
return BuildTcpResponse(seq, Cmd_ExtDevice, QByteArray(), true);
|
|
}
|
|
|
|
// extType at [0..3] (0 = swing motor)
|
|
uint32_t extType = 0;
|
|
memcpy(&extType, payload.constData(), 4);
|
|
|
|
// Swing sub-command code: ushort[1] at payload[6..7] (little-endian).
|
|
uint16_t swingCmd = 0;
|
|
memcpy(&swingCmd, payload.constData() + 6, 2);
|
|
// Read/write flag: ushort[0] at payload[4..5].
|
|
uint16_t rwFlag = 0;
|
|
memcpy(&rwFlag, payload.constData() + 4, 2);
|
|
|
|
uint32_t result = 0;
|
|
|
|
// Command codes from VzSwingMotorAPI.h c_sSwingMotorCommand[].
|
|
switch (swingCmd) {
|
|
case 0x0024: // GetVersion
|
|
result = 8; // version >= 8 enables GetMotorMaxAngle path
|
|
break;
|
|
case 0x0025: // GetMotorMaxAngle (hardware max angle, degrees*10)
|
|
result = 750;
|
|
break;
|
|
case 0x001B: // GetMaxAngle (degrees*10)
|
|
result = 750;
|
|
break;
|
|
case 0x0020: // GetReductionRatio
|
|
// Must succeed (non-failing); affects speed-range calc: ratio=reduction/36.
|
|
// 360 -> ratio 10 -> speed range ~16.7..133.3 deg/s.
|
|
result = 360;
|
|
break;
|
|
case 0x000F: // QueryStatus
|
|
// bit15 = 1 marks a valid status word; bit14 = 1 means Busy, 0 = Idle.
|
|
// Without bit15 the SDK's _QueryValidStatus returns SwingMotor_Err.
|
|
result = 0x8000; // valid + idle
|
|
break;
|
|
case 0x001F: // GetMotorSpeed
|
|
result = 36; // current speed (deg/s)
|
|
break;
|
|
case 0x001C: // GetMotorStartPos
|
|
result = 0;
|
|
break;
|
|
case 0x001D: // GetMotorEndPos
|
|
result = 0;
|
|
break;
|
|
case 0x0012: // GetCurPos
|
|
result = 0;
|
|
break;
|
|
case 0x001E: // GetLaserDeviceLight
|
|
result = 0;
|
|
break;
|
|
default:
|
|
// Write commands and unknown reads simply return 0 (success).
|
|
result = 0;
|
|
break;
|
|
}
|
|
|
|
QByteArray respData(reinterpret_cast<const char*>(&result), sizeof(result));
|
|
emit LogMessage(QString("ExtDevice swingCmd=0x%1 rw=%2 -> %3")
|
|
.arg(swingCmd, 4, 16, QChar('0')).arg(rwFlag).arg(result));
|
|
return BuildTcpResponse(seq, Cmd_ExtDevice, respData, true);
|
|
}
|
|
|
|
// ============================================================================
|
|
// TCP Protocol Framing (VZEB / VZEE)
|
|
// ============================================================================
|
|
|
|
QByteArray VrCameraSimulator::PackTcpFrame(TcpOperaType op, uint32_t cmd,
|
|
uint32_t seq, const QByteArray& payload)
|
|
{
|
|
uint32_t totalLen = TCP_HEADER_SIZE + static_cast<uint32_t>(payload.size()) + VZEE_TAIL_LEN;
|
|
|
|
QByteArray frame;
|
|
frame.resize(static_cast<int>(totalLen));
|
|
uint8_t* d = reinterpret_cast<uint8_t*>(frame.data());
|
|
|
|
// "VZEB" head
|
|
d[0] = 'V'; d[1] = 'Z'; d[2] = 'E'; d[3] = 'B';
|
|
|
|
// Total length (little-endian)
|
|
memcpy(d + 4, &totalLen, 4);
|
|
|
|
// Operation type
|
|
uint32_t opVal = static_cast<uint32_t>(op);
|
|
memcpy(d + 8, &opVal, 4);
|
|
|
|
// Command
|
|
memcpy(d + 12, &cmd, 4);
|
|
|
|
// Sequence number
|
|
memcpy(d + 16, &seq, 4);
|
|
|
|
// Payload
|
|
if (!payload.isEmpty()) {
|
|
memcpy(d + TCP_HEADER_SIZE, payload.constData(), payload.size());
|
|
}
|
|
|
|
// "VZEE" tail
|
|
int tailOff = TCP_HEADER_SIZE + payload.size();
|
|
d[tailOff] = 'V';
|
|
d[tailOff + 1] = 'Z';
|
|
d[tailOff + 2] = 'E';
|
|
d[tailOff + 3] = 'E';
|
|
|
|
return frame;
|
|
}
|
|
|
|
bool VrCameraSimulator::UnpackTcpFrame(const QByteArray& frame, TcpOperaType& op,
|
|
uint32_t& cmd, uint32_t& seq,
|
|
QByteArray& payload)
|
|
{
|
|
if (frame.size() < TCP_HEADER_SIZE + VZEE_TAIL_LEN) return false;
|
|
|
|
const uint8_t* d = reinterpret_cast<const uint8_t*>(frame.constData());
|
|
|
|
// Check head
|
|
if (d[0] != 'V' || d[1] != 'Z' || d[2] != 'E' || d[3] != 'B')
|
|
return false;
|
|
|
|
uint32_t totalLen;
|
|
memcpy(&totalLen, d + 4, 4);
|
|
if (totalLen != static_cast<uint32_t>(frame.size())) return false;
|
|
|
|
memcpy(reinterpret_cast<uint32_t*>(&op), d + 8, 4);
|
|
memcpy(&cmd, d + 12, 4);
|
|
memcpy(&seq, d + 16, 4);
|
|
|
|
int payloadLen = frame.size() - TCP_HEADER_SIZE - VZEE_TAIL_LEN;
|
|
if (payloadLen > 0) {
|
|
payload = frame.mid(TCP_HEADER_SIZE, payloadLen);
|
|
} else {
|
|
payload.clear();
|
|
}
|
|
|
|
// Check tail
|
|
int tailOff = TCP_HEADER_SIZE + payloadLen;
|
|
if (d[tailOff] != 'V' || d[tailOff+1] != 'Z' ||
|
|
d[tailOff+2] != 'E' || d[tailOff+3] != 'E')
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool VrCameraSimulator::ParseVzebFrame(const uint8_t* data, int len, int& consumed,
|
|
TcpOperaType& op, uint32_t& cmd,
|
|
uint32_t& seq, QByteArray& payload)
|
|
{
|
|
consumed = 0;
|
|
if (len < TCP_HEADER_SIZE + VZEE_TAIL_LEN) return false;
|
|
|
|
// Find "VZEB" head
|
|
int headPos = -1;
|
|
for (int i = 0; i <= len - 4; i++) {
|
|
if (data[i] == 'V' && data[i+1] == 'Z' &&
|
|
data[i+2] == 'E' && data[i+3] == 'B') {
|
|
headPos = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (headPos < 0) return false;
|
|
|
|
// Read total length from header
|
|
if (headPos + 8 > len) return false;
|
|
uint32_t totalLen;
|
|
memcpy(&totalLen, data + headPos + 4, 4);
|
|
|
|
// Sanity check
|
|
if (totalLen < TCP_HEADER_SIZE + VZEE_TAIL_LEN || totalLen > 1024 * 1024)
|
|
return false;
|
|
|
|
if (headPos + static_cast<int>(totalLen) > len) return false;
|
|
|
|
// Build a QByteArray from this segment and unpack
|
|
QByteArray frame(reinterpret_cast<const char*>(data + headPos),
|
|
static_cast<int>(totalLen));
|
|
|
|
if (!UnpackTcpFrame(frame, op, cmd, seq, payload)) return false;
|
|
|
|
consumed = headPos + static_cast<int>(totalLen);
|
|
return true;
|
|
}
|
|
|
|
// ============================================================================
|
|
// Build response helpers
|
|
// ============================================================================
|
|
|
|
QByteArray VrCameraSimulator::BuildTcpResponse(uint32_t seqNum, uint32_t command,
|
|
const QByteArray& payload, bool success)
|
|
{
|
|
// The SDK expects the response payload to start with a 4-byte error/return
|
|
// code, followed by the returned data block (_RecviceDataWithCheck strips
|
|
// the first 4 bytes as errCode and exposes the rest as the data block).
|
|
QByteArray body;
|
|
uint32_t errCode = success ? 0 : 1;
|
|
body.append(reinterpret_cast<const char*>(&errCode), sizeof(errCode));
|
|
body.append(payload);
|
|
return PackTcpFrame(TcpOperaType::Respond, command, seqNum, body);
|
|
}
|
|
|
|
// ============================================================================
|
|
// UDP Discovery Response Builder
|
|
// ============================================================================
|
|
|
|
QByteArray VrCameraSimulator::BuildDiscoveryResponse(const QByteArray& request,
|
|
const sockaddr_in& /*senderAddr*/)
|
|
{
|
|
// Advertise our real local IP so the SDK can reach back on TCP 6679.
|
|
// The hard-coded 192.168.1.200 default only works if this machine owns
|
|
// that address; otherwise the SDK's Open() TCP connect fails with
|
|
// keErrorCode_NetworkConnectFailed (-69999). getsockname() on a socket
|
|
// bound to INADDR_ANY returns 0.0.0.0, so enumerate adapters instead.
|
|
{
|
|
uint8_t localIP[4] = {0, 0, 0, 0};
|
|
if (GetFirstLocalIPv4(localIP)) {
|
|
m_deviceIPBytes[0] = localIP[0];
|
|
m_deviceIPBytes[1] = localIP[1];
|
|
m_deviceIPBytes[2] = localIP[2];
|
|
m_deviceIPBytes[3] = localIP[3];
|
|
m_deviceIP = QString("%1.%2.%3.%4")
|
|
.arg(localIP[0]).arg(localIP[1])
|
|
.arg(localIP[2]).arg(localIP[3]);
|
|
}
|
|
}
|
|
|
|
// Parse the request to extract the serial number field (bytes 4-11)
|
|
QByteArray sn(8, 0);
|
|
if (request.size() >= 12) {
|
|
sn = request.mid(4, 8);
|
|
}
|
|
|
|
// Build the response matching SVzXilinxDeviceInfo layout (68 bytes, MSVC
|
|
// default packing with 3 bytes padding after the 1-byte product type):
|
|
// [0] eProductType (1)
|
|
// [1..3] padding
|
|
// [4..7] nVersionCode (LE)
|
|
// [8..39] szDeviceVersion (32)
|
|
// [40..41] nResolutionWidth (LE)
|
|
// [42..43] nResolutionHeight (LE)
|
|
// [44] eIPType
|
|
// [45..48] byDeviceIP
|
|
// [49..54] byDeviceMAC
|
|
// [55..58] byDeviceGW
|
|
// [59..66] szSN
|
|
// total 68 bytes
|
|
|
|
QByteArray devInfo;
|
|
devInfo.resize(68);
|
|
devInfo.fill(0);
|
|
uint8_t* d = reinterpret_cast<uint8_t*>(devInfo.data());
|
|
|
|
// eProductType = 1 (LaserEye). The SDK ignores this and hard-codes
|
|
// eDeviceType = keDeviceType_LaserRobotEye.
|
|
d[0] = 1;
|
|
|
|
// nVersionCode (offset 4, little-endian)
|
|
uint32_t verCode = 0x01000000;
|
|
memcpy(d + 4, &verCode, 4);
|
|
|
|
// szDeviceVersion (offset 8, 32 bytes) — null-terminated
|
|
const char* verStr = "VrVirtualCam v1.0";
|
|
strncpy(reinterpret_cast<char*>(d + 8), verStr, 31);
|
|
|
|
// nResolutionWidth / nResolutionHeight (offset 40/42, little-endian)
|
|
uint16_t w = static_cast<uint16_t>(m_imageWidth);
|
|
uint16_t h = static_cast<uint16_t>(m_imageHeight);
|
|
memcpy(d + 40, &w, 2);
|
|
memcpy(d + 42, &h, 2);
|
|
|
|
// eIPType (offset 44) = 1 (Static IP)
|
|
d[44] = 1;
|
|
|
|
// byDeviceIP (offset 45)
|
|
memcpy(d + 45, m_deviceIPBytes, 4);
|
|
|
|
// byDeviceMAC (offset 49)
|
|
memcpy(d + 49, m_deviceMACBytes, 6);
|
|
|
|
// byDeviceGW (offset 55) = gateway (same subnet, .1)
|
|
d[55] = m_deviceIPBytes[0];
|
|
d[56] = m_deviceIPBytes[1];
|
|
d[57] = m_deviceIPBytes[2];
|
|
d[58] = 1;
|
|
|
|
// szSN (offset 59)
|
|
memcpy(d + 59, m_deviceSNBytes, 8);
|
|
|
|
// Now build the full UDP packet
|
|
QByteArray packet;
|
|
|
|
// Preamble (little-endian: bytes are e1,e2,e3,e4)
|
|
packet.append('\xe1');
|
|
packet.append('\xe2');
|
|
packet.append('\xe3');
|
|
packet.append('\xe4');
|
|
|
|
// SN (8 bytes)
|
|
packet.append(sn);
|
|
|
|
// RecvUDPDataType = 0x0040 (Command) — big-endian: 0x00 0x40
|
|
packet.append(static_cast<char>(0x00));
|
|
packet.append(static_cast<char>(0x40));
|
|
|
|
// shCommand + shCommandLength (2+2 bytes, zero) — SDK reads these but does
|
|
// not validate them.
|
|
packet.append(4, '\0');
|
|
|
|
// shCommandSequen (2 bytes, not validated) — "SB"
|
|
packet.append(static_cast<char>(0x42));
|
|
packet.append(static_cast<char>(0x53));
|
|
|
|
// nReserveData (4 bytes)
|
|
packet.append(4, '\0');
|
|
|
|
// shPackageHead "SB" (2 bytes)
|
|
packet.append(static_cast<char>(0x42));
|
|
packet.append(static_cast<char>(0x53));
|
|
|
|
// AckType = 0x0004 (CommandData) — big-endian: 0x00 0x04
|
|
packet.append(static_cast<char>(0x00));
|
|
packet.append(static_cast<char>(0x04));
|
|
|
|
// Command Length (big-endian).
|
|
// SDK: shRemainLen = shCommandLen - 14; requires >= sizeof(SVzXilinxDeviceInfo)=68
|
|
// to parse the device info. So shCommandLen must be >= 82.
|
|
uint16_t cmdLen = static_cast<uint16_t>(14 + devInfo.size()); // = 82
|
|
packet.append(static_cast<char>((cmdLen >> 8) & 0xFF));
|
|
packet.append(static_cast<char>(cmdLen & 0xFF));
|
|
|
|
// Checksum (4 bytes, placeholder = 0)
|
|
packet.append(4, '\0');
|
|
|
|
// Command Sequence (2 bytes, match request seq if available)
|
|
uint16_t cmdSeq = 1;
|
|
if (request.size() >= 36) {
|
|
cmdSeq = (static_cast<uint16_t>(static_cast<uint8_t>(request[34])) << 8)
|
|
| static_cast<uint16_t>(static_cast<uint8_t>(request[35]));
|
|
}
|
|
packet.append(static_cast<char>((cmdSeq >> 8) & 0xFF));
|
|
packet.append(static_cast<char>(cmdSeq & 0xFF));
|
|
|
|
// Device Info payload
|
|
packet.append(devInfo);
|
|
|
|
// Package end "EB" = 0x4245. After the device info exactly 2 bytes must
|
|
// remain so the SDK takes the "shRemainLen == 2" branch (reads this tail).
|
|
// The SDK reads one more package end unconditionally afterwards; that read
|
|
// runs past the buffer end and fails harmlessly (device already parsed).
|
|
packet.append(static_cast<char>(0x42));
|
|
packet.append(static_cast<char>(0x45));
|
|
|
|
return packet;
|
|
}
|
|
|
|
// ============================================================================
|
|
// Register Map
|
|
// ============================================================================
|
|
|
|
void VrCameraSimulator::InitRegisterMap()
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_regMutex);
|
|
m_registers.clear();
|
|
|
|
// Helper: store a uint32 value as a 4-byte little-endian blob.
|
|
auto putU32 = [this](uint32_t addr, uint32_t value) {
|
|
QByteArray data(reinterpret_cast<const char*>(&value), sizeof(value));
|
|
m_registers[addr] = data;
|
|
};
|
|
|
|
// ---- Camera registers ----
|
|
putU32(0x00100000, 0x00000003); // DevType: 3 = LaserRobotEye
|
|
putU32(0x0010000C, 0x00000008); // HardwareVersion: 8
|
|
putU32(0x00100010, 0x00000001); // AlgoIPVersion
|
|
putU32(0x00100014, 0x01000000); // PSVersion
|
|
putU32(0x0010002C, 0x00000000); // DataMode: 0 = Data (not Data+Image)
|
|
putU32(0x00100030, 0x00000001); // DeviceID
|
|
// Capability: bit1 = bIsSupportRGBSensor, bit9 = bSupportSwingMotor.
|
|
// (RGB bit lets SetRGBDExposeThres succeed; RGB camera init is best-effort.)
|
|
putU32(0x00100034, 0x00000202);
|
|
putU32(0x00100094, 0x01000000); // PLVersion
|
|
putU32(0x00100090, 0x00000000); // EnableCalibROIFlag = 0 (no calib ROI)
|
|
putU32(0x00100078, 0x00000001); // LaserDataVersion = 1
|
|
|
|
// Serial number (8 bytes)
|
|
m_registers[0x00100004] = QByteArray(reinterpret_cast<const char*>(m_deviceSNBytes), 8);
|
|
|
|
// ---- Status registers ----
|
|
// Phase status: 16 bytes each, all-zero (means valid, != 0xff)
|
|
m_registers[0x00210000] = QByteArray(16, 0); // LeftPhase
|
|
m_registers[0x00210010] = QByteArray(16, 0); // RightPhase
|
|
m_registers[0x00210020] = QByteArray(16, 0); // CenterPhase
|
|
putU32(0x00200018, 0x00000000); // Status_Stream = 0
|
|
|
|
// ---- Sensor registers ----
|
|
// Sensor ROI (5x uint32): [Width, Height, Y, LeftX, RightX]
|
|
QByteArray roi(20, 0);
|
|
uint32_t* roiArr = reinterpret_cast<uint32_t*>(roi.data());
|
|
roiArr[0] = static_cast<uint32_t>(m_imageWidth);
|
|
roiArr[1] = static_cast<uint32_t>(m_imageHeight);
|
|
roiArr[2] = 0; // Y
|
|
roiArr[3] = 0; // LeftX
|
|
roiArr[4] = 0; // RightX
|
|
m_registers[0x00400000] = roi;
|
|
|
|
putU32(0x00400014, static_cast<uint32_t>(m_frameRate)); // FPS
|
|
putU32(0x00400018, 5000); // EXP
|
|
putU32(0x0040001C, 100); // GAIN Left
|
|
putU32(0x00400020, 100); // GAIN Right
|
|
|
|
// RGB sensor registers (read by _UpdateColorPixelFormat when RGB is enabled)
|
|
putU32(0x00400138, 0); // Center_Type = Color (0)
|
|
putU32(0x0040013C, 0); // Center_Pixel_Format = RGGB (0)
|
|
putU32(0x004000CC, 0); // CenterAutoExposeThres (float)
|
|
|
|
// FullDetectROI: 4 x SVzNLROIRect (left/right/calib-left/calib-right).
|
|
// SVzNLROIRect = 4 ints (left,right,top,bottom) = 16 bytes each, 64 total.
|
|
{
|
|
QByteArray fullROI(4 * 16, 0);
|
|
int* r = reinterpret_cast<int*>(fullROI.data());
|
|
// Left / right full-frame ROI.
|
|
r[0] = 0; r[1] = m_imageWidth; r[2] = 0; r[3] = m_imageHeight; // left
|
|
r[4] = 0; r[5] = m_imageWidth; r[6] = 0; r[7] = m_imageHeight; // right
|
|
// Calib left/right left as 0.
|
|
m_registers[0x0040006C] = fullROI;
|
|
}
|
|
|
|
// ---- Calibration ----
|
|
// QMatrix: 16 doubles (128 bytes), identity matrix
|
|
QByteArray qmat(16 * sizeof(double), 0);
|
|
double* qm = reinterpret_cast<double*>(qmat.data());
|
|
for (int i = 0; i < 16; i++) qm[i] = (i % 5 == 0) ? 1.0 : 0.0;
|
|
m_registers[0x00300000] = qmat;
|
|
// ConvertMatrixData (GetCalibMatrix): 16 doubles, identity
|
|
m_registers[0x00600088] = qmat;
|
|
putU32(0x00300080, 0x00000000); // Parallax_Offset
|
|
|
|
// ---- Product registers ----
|
|
putU32(0x00700000, 0x00000000); // ProductType: keProjectType_None
|
|
putU32(0x00700004, 256); // 3DDataHeadLength
|
|
putU32(0x00700008, 104); // 3DDataFrameLength
|
|
// SupportImageWidth must be >= the full-frame ROI width (1280), otherwise
|
|
// BeginGetAutoDetect aborts Data-mode laser detect with
|
|
// keErrorCode_Device_NoSupport_Current_ROI (-79923).
|
|
putU32(0x00710004, 2048); // SupportMaxROIWidth
|
|
|
|
// ---- External device: swing motor ----
|
|
putU32(0x00500000, 0x00000001); // IsSupportSwing = 1
|
|
putU32(0x00500004, 0x00000001); // EnableSwing = 1
|
|
putU32(0x00500008, 0x00000000); // SwingScanMode: Once
|
|
// SwingWorkRange: 2 floats (near/far distance, mm)
|
|
{
|
|
float fRange[2] = {500.0f, 1500.0f};
|
|
m_registers[0x0050000C] = QByteArray(reinterpret_cast<const char*>(fRange),
|
|
sizeof(fRange));
|
|
}
|
|
}
|
|
|
|
QByteArray VrCameraSimulator::ReadRegister(uint32_t addr) const
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_regMutex);
|
|
auto it = m_registers.find(addr);
|
|
if (it != m_registers.end()) {
|
|
return it->second;
|
|
}
|
|
// Unknown register: return 4 zero bytes (matches common register width).
|
|
return QByteArray(4, 0);
|
|
}
|
|
|
|
void VrCameraSimulator::WriteRegister(uint32_t addr, const QByteArray& data)
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_regMutex);
|
|
m_registers[addr] = data;
|
|
}
|
|
|
|
bool VrCameraSimulator::HasRegister(uint32_t addr) const
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_regMutex);
|
|
return m_registers.find(addr) != m_registers.end();
|
|
}
|
|
|
|
// ============================================================================
|
|
// Image & Laser Data Loading
|
|
// ============================================================================
|
|
|
|
void VrCameraSimulator::SetImageDirectory(const QString& dir)
|
|
{
|
|
m_imageDir = dir;
|
|
LoadImagesFromDirectory();
|
|
}
|
|
|
|
void VrCameraSimulator::LoadImagesFromDirectory()
|
|
{
|
|
std::lock_guard<std::mutex> lock(m_imageMutex);
|
|
m_leftImages.clear();
|
|
m_rightImages.clear();
|
|
m_imageFiles.clear();
|
|
m_imageReplayIdx = 0;
|
|
|
|
if (m_imageDir.isEmpty()) {
|
|
emit ImageDirectoryChanged(0);
|
|
emit LogMessage("Image directory cleared");
|
|
return;
|
|
}
|
|
|
|
QDir dir(m_imageDir);
|
|
if (!dir.exists()) {
|
|
emit LogMessage("Image directory not found: " + m_imageDir, true);
|
|
emit ImageDirectoryChanged(0);
|
|
return;
|
|
}
|
|
|
|
// Find all *-L.* files and match with *-R.* counterparts
|
|
QStringList filters = {"*.png", "*.bmp", "*.jpg", "*.jpeg", "*.PNG", "*.BMP", "*.JPG", "*.JPEG"};
|
|
QStringList allFiles;
|
|
|
|
QStringList nameFilters;
|
|
for (const auto& f : filters) {
|
|
nameFilters << f;
|
|
}
|
|
|
|
QDirIterator it(m_imageDir, nameFilters, QDir::Files);
|
|
while (it.hasNext()) {
|
|
allFiles << it.next();
|
|
}
|
|
|
|
// Find left images and match right pairs
|
|
QSet<QString> matchedBases;
|
|
for (const QString& filePath : allFiles) {
|
|
QFileInfo fi(filePath);
|
|
QString name = fi.completeBaseName(); // e.g. "image-L" or "image-R"
|
|
|
|
if (name.endsWith("-L") || name.endsWith("-l")) {
|
|
QString base = name.left(name.size() - 2); // remove "-L"
|
|
if (matchedBases.contains(base)) continue;
|
|
|
|
// Find corresponding right image
|
|
QString rightPath;
|
|
for (const QString& rp : allFiles) {
|
|
QFileInfo rfi(rp);
|
|
QString rname = rfi.completeBaseName();
|
|
if ((rname == base + "-R" || rname == base + "-r") && rp != filePath) {
|
|
rightPath = rp;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!rightPath.isEmpty()) {
|
|
QImage leftImg(filePath);
|
|
QImage rightImg(rightPath);
|
|
if (!leftImg.isNull() && !rightImg.isNull()) {
|
|
m_imageFiles << base;
|
|
m_leftImages.push_back(leftImg);
|
|
m_rightImages.push_back(rightImg);
|
|
matchedBases.insert(base);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
emit LogMessage(QString("Loaded %1 stereo image pairs from %2")
|
|
.arg(m_leftImages.size()).arg(m_imageDir));
|
|
emit ImageDirectoryChanged(static_cast<int>(m_leftImages.size()));
|
|
}
|
|
|
|
void VrCameraSimulator::SetLaserDataDirectory(const QString& dir)
|
|
{
|
|
m_laserDataDir = dir;
|
|
LoadLaserDataFromDirectory();
|
|
}
|
|
|
|
int VrCameraSimulator::LaserDataTotalLines() const
|
|
{
|
|
int total = 0;
|
|
for (const auto& fileLines : m_laserFileLines) {
|
|
total += static_cast<int>(fileLines.size());
|
|
}
|
|
return total;
|
|
}
|
|
|
|
void VrCameraSimulator::LoadLaserDataFromDirectory()
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(m_laserDataMutex);
|
|
m_laserDataFiles.clear();
|
|
m_laserFileLines.clear();
|
|
m_laserFileReplayIdx = 0;
|
|
m_laserLineReplayIdx = 0;
|
|
|
|
if (m_laserDataDir.isEmpty()) {
|
|
emit LaserDataDirectoryChanged(0, 0);
|
|
emit LogMessage("Laser data directory cleared");
|
|
return;
|
|
}
|
|
|
|
QDir dir(m_laserDataDir);
|
|
if (!dir.exists()) {
|
|
emit LogMessage("Laser data directory not found: " + m_laserDataDir, true);
|
|
emit LaserDataDirectoryChanged(0, 0);
|
|
return;
|
|
}
|
|
|
|
// Find all .txt and .dat files
|
|
QStringList filters = {"*.txt", "*.dat", "*.TXT", "*.DAT"};
|
|
QStringList nameFilters;
|
|
for (const auto& f : filters) nameFilters << f;
|
|
|
|
QDirIterator it(m_laserDataDir, nameFilters, QDir::Files);
|
|
QStringList filePaths;
|
|
while (it.hasNext()) filePaths << it.next();
|
|
filePaths.sort();
|
|
|
|
int totalLines = 0;
|
|
LaserDataLoader loader;
|
|
|
|
for (const QString& filePath : filePaths) {
|
|
std::string path = filePath.toStdString();
|
|
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> laserLines;
|
|
int lineNum = 0;
|
|
float scanSpeed = 0;
|
|
int maxTimeStamp = 0;
|
|
int clockPerSecond = 0;
|
|
|
|
int result = loader.LoadLaserScanData(path, laserLines, lineNum,
|
|
scanSpeed, maxTimeStamp, clockPerSecond);
|
|
if (result != 0 || laserLines.empty()) {
|
|
emit LogMessage(QString("Skipping %1 (no valid laser data)").arg(filePath), true);
|
|
continue;
|
|
}
|
|
|
|
m_laserDataFiles << filePath;
|
|
|
|
std::vector<CachedLaserLine> cachedLines;
|
|
for (const auto& linePair : laserLines) {
|
|
const SVzLaserLineData& lineData = linePair.second;
|
|
CachedLaserLine cached;
|
|
cached.pointCount = lineData.nPointCount;
|
|
cached.timestamp = lineData.llTimeStamp;
|
|
cached.frameIdx = lineData.llFrameIdx;
|
|
|
|
if (lineData.nPointCount > 0 && lineData.p3DPoint) {
|
|
cached.x.resize(lineData.nPointCount);
|
|
cached.y.resize(lineData.nPointCount);
|
|
cached.z.resize(lineData.nPointCount);
|
|
|
|
if (linePair.first == keResultDataType_Position ||
|
|
linePair.first == keResultDataType_PositionF) {
|
|
const SVzNL3DPosition* pts = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
|
|
for (int i = 0; i < lineData.nPointCount; i++) {
|
|
cached.x[i] = static_cast<float>(pts[i].pt3D.x);
|
|
cached.y[i] = static_cast<float>(pts[i].pt3D.y);
|
|
cached.z[i] = static_cast<float>(pts[i].pt3D.z);
|
|
}
|
|
} else if (linePair.first == keResultDataType_PointXYZ) {
|
|
const SVzNLPointXYZ* pts = static_cast<const SVzNLPointXYZ*>(lineData.p3DPoint);
|
|
for (int i = 0; i < lineData.nPointCount; i++) {
|
|
cached.x[i] = pts[i].x;
|
|
cached.y[i] = pts[i].y;
|
|
cached.z[i] = pts[i].z;
|
|
}
|
|
} else if (linePair.first == keResultDataType_PointXYZRGBA) {
|
|
const SVzNLPointXYZRGBA* pts = static_cast<const SVzNLPointXYZRGBA*>(lineData.p3DPoint);
|
|
for (int i = 0; i < lineData.nPointCount; i++) {
|
|
cached.x[i] = pts[i].x;
|
|
cached.y[i] = pts[i].y;
|
|
cached.z[i] = pts[i].z;
|
|
}
|
|
}
|
|
}
|
|
|
|
cachedLines.push_back(cached);
|
|
}
|
|
|
|
m_laserFileLines.push_back(cachedLines);
|
|
totalLines += static_cast<int>(cachedLines.size());
|
|
|
|
// Free VZNLSDK-managed memory
|
|
loader.FreeLaserScanData(laserLines);
|
|
}
|
|
|
|
emit LogMessage(QString("Loaded %1 files / %2 total laser lines from %3")
|
|
.arg(m_laserDataFiles.size()).arg(totalLines).arg(m_laserDataDir));
|
|
emit LaserDataDirectoryChanged(m_laserDataFiles.size(), totalLines);
|
|
}
|
|
|
|
VrCameraSimulator::CachedLaserLine VrCameraSimulator::GetNextLaserLine()
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(m_laserDataMutex);
|
|
CachedLaserLine empty;
|
|
|
|
if (m_laserFileLines.empty()) return empty;
|
|
|
|
// Cycle through files and lines
|
|
if (m_laserFileReplayIdx >= static_cast<int>(m_laserFileLines.size())) {
|
|
m_laserFileReplayIdx = 0;
|
|
m_laserLineReplayIdx = 0;
|
|
}
|
|
|
|
const auto& currentFile = m_laserFileLines[m_laserFileReplayIdx];
|
|
if (currentFile.empty()) {
|
|
m_laserFileReplayIdx++;
|
|
m_laserLineReplayIdx = 0;
|
|
if (m_laserFileReplayIdx >= static_cast<int>(m_laserFileLines.size())) {
|
|
m_laserFileReplayIdx = 0;
|
|
}
|
|
return GetNextLaserLine(); // try next file
|
|
}
|
|
|
|
if (m_laserLineReplayIdx >= static_cast<int>(currentFile.size())) {
|
|
m_laserLineReplayIdx = 0;
|
|
m_laserFileReplayIdx++;
|
|
if (m_laserFileReplayIdx >= static_cast<int>(m_laserFileLines.size())) {
|
|
m_laserFileReplayIdx = 0;
|
|
}
|
|
return GetNextLaserLine(); // try next file
|
|
}
|
|
|
|
CachedLaserLine line = currentFile[m_laserLineReplayIdx];
|
|
m_laserLineReplayIdx++;
|
|
return line;
|
|
}
|
|
|
|
void VrCameraSimulator::ResetLaserReplay()
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(m_laserDataMutex);
|
|
m_laserFileReplayIdx = 0;
|
|
m_laserLineReplayIdx = 0;
|
|
}
|
|
|
|
// ============================================================================
|
|
// Image Generator: Checkerboard Pattern (fallback when no images loaded)
|
|
// ============================================================================
|
|
|
|
QByteArray VrCameraSimulator::GenerateCheckerboardGray(int frameIdx, int offsetX, int offsetY)
|
|
{
|
|
const int sqSize = 64;
|
|
const int size = m_imageWidth * m_imageHeight;
|
|
QByteArray buf(size, '\0');
|
|
uint8_t* pixels = reinterpret_cast<uint8_t*>(buf.data());
|
|
|
|
int shift = frameIdx % sqSize;
|
|
|
|
for (int y = 0; y < m_imageHeight; y++) {
|
|
for (int x = 0; x < m_imageWidth; x++) {
|
|
int cx = (x + offsetX + shift) / sqSize;
|
|
int cy = (y + offsetY + shift) / sqSize;
|
|
pixels[y * m_imageWidth + x] = ((cx + cy) % 2 == 0) ? 220 : 30;
|
|
}
|
|
}
|
|
|
|
return buf;
|
|
}
|
|
|
|
QImage VrCameraSimulator::GenerateCheckerboardQImage(int frameIdx, int offsetX, int offsetY)
|
|
{
|
|
QByteArray raw = GenerateCheckerboardGray(frameIdx, offsetX, offsetY);
|
|
QImage img(reinterpret_cast<const uint8_t*>(raw.constData()),
|
|
m_imageWidth, m_imageHeight, QImage::Format_Grayscale8);
|
|
return img.copy(); // deep copy since raw is temporary
|
|
}
|
|
|