276 lines
9.7 KiB
C++
276 lines
9.7 KiB
C++
#ifndef VRCAMERASIMULATOR_H
|
|
#define VRCAMERASIMULATOR_H
|
|
|
|
#include <QObject>
|
|
#include <QImage>
|
|
#include <QString>
|
|
#include <QStringList>
|
|
#include <QByteArray>
|
|
|
|
#include <atomic>
|
|
#include <thread>
|
|
#include <mutex>
|
|
#include <unordered_map>
|
|
#include <vector>
|
|
|
|
#ifdef _WIN32
|
|
#ifndef NOMINMAX
|
|
#define NOMINMAX
|
|
#endif
|
|
#include <winsock2.h>
|
|
#include <ws2tcpip.h>
|
|
using socklen_t = int;
|
|
#else
|
|
#include <sys/socket.h>
|
|
#include <netinet/in.h>
|
|
#include <arpa/inet.h>
|
|
#include <unistd.h>
|
|
#define SOCKET int
|
|
#define INVALID_SOCKET (-1)
|
|
#define SOCKET_ERROR (-1)
|
|
#define closesocket close
|
|
#endif
|
|
|
|
/// VZNLSDK-compatible virtual camera simulator.
|
|
/// Listens on UDP 6789 for device discovery and TCP 6679 for command/control.
|
|
/// Generates synthetic checkerboard images and 3D laser point cloud data.
|
|
class VrCameraSimulator : public QObject
|
|
{
|
|
Q_OBJECT
|
|
|
|
public:
|
|
explicit VrCameraSimulator(QObject* parent = nullptr);
|
|
~VrCameraSimulator() override;
|
|
|
|
// ---- Configuration ----
|
|
void SetImageWidth(int w);
|
|
void SetImageHeight(int h);
|
|
void SetFrameRate(int fps);
|
|
int ImageWidth() const { return m_imageWidth; }
|
|
int ImageHeight() const { return m_imageHeight; }
|
|
int FrameRate() const { return m_frameRate; }
|
|
|
|
// ---- Lifecycle ----
|
|
bool Start();
|
|
void Stop();
|
|
bool IsRunning() const { return m_running; }
|
|
|
|
// ---- Status ----
|
|
bool IsConnected() const { return m_connectedClients > 0; }
|
|
int ConnectedClients() const { return m_connectedClients; }
|
|
QString DeviceIP() const { return m_deviceIP; }
|
|
|
|
// ---- Image source ----
|
|
void SetImageDirectory(const QString& dir);
|
|
QString ImageDirectory() const { return m_imageDir; }
|
|
int ImageCount() const { return static_cast<int>(m_leftImages.size()); }
|
|
|
|
// ---- Laser data source ----
|
|
void SetLaserDataDirectory(const QString& dir);
|
|
QString LaserDataDirectory() const { return m_laserDataDir; }
|
|
int LaserDataFileCount() const { return static_cast<int>(m_laserDataFiles.size()); }
|
|
int LaserDataTotalLines() const;
|
|
|
|
signals:
|
|
void StatusChanged(const QString& status);
|
|
void ClientConnected(const QString& clientIP);
|
|
void ClientDisconnected();
|
|
void ImageGenerated(const QImage& left, const QImage& right);
|
|
void ImageDirectoryChanged(int imageCount);
|
|
void LaserDataDirectoryChanged(int fileCount, int totalLines);
|
|
void LogMessage(const QString& msg, bool isWarning = false);
|
|
|
|
private:
|
|
// ---- Network threads ----
|
|
void UdpListenerLoop();
|
|
void TcpServerLoop();
|
|
void TcpClientLoop(SOCKET clientSock);
|
|
void StreamLoop();
|
|
|
|
// ---- Protocol helpers ----
|
|
QByteArray BuildDiscoveryResponse(const QByteArray& request, const sockaddr_in& senderAddr);
|
|
QByteArray BuildTcpResponse(uint32_t seqNum, uint32_t command,
|
|
const QByteArray& payload, bool success = true);
|
|
|
|
// ---- TCP protocol (VZEB/VZEE framing) ----
|
|
enum class TcpOperaType : uint32_t {
|
|
None = 0,
|
|
Request = 1,
|
|
Respond = 2,
|
|
Post = 3
|
|
};
|
|
|
|
// TCP Commands (from VzXilinxEyeDeviceDefine.h)
|
|
enum TcpCommand : uint32_t {
|
|
Cmd_ReadRegister = 1,
|
|
Cmd_WriteRegister = 2,
|
|
Cmd_ReadDMA = 3,
|
|
Cmd_WriteDMA = 4,
|
|
Cmd_StartStream = 5,
|
|
Cmd_StopStream = 6,
|
|
Cmd_TriggerN = 7,
|
|
Cmd_GetImage = 9,
|
|
Cmd_DeviceOption = 12,
|
|
Cmd_WriteData = 13,
|
|
Cmd_ReadData = 14,
|
|
Cmd_PushLaserResult = 15,
|
|
Cmd_Trigger = 16,
|
|
Cmd_PushDataEx = 17,
|
|
Cmd_ExtDevice = 18,
|
|
Cmd_FileSysOpera = 19,
|
|
Cmd_OpenDevice = 201,
|
|
Cmd_Debug = 200,
|
|
};
|
|
|
|
// TCP protocol frame sizes
|
|
static constexpr int VZEB_HEAD_LEN = 4; // "VZEB"
|
|
static constexpr int VZEE_TAIL_LEN = 4; // "VZEE"
|
|
static constexpr int LENGTH_FIELD_LEN = 4;
|
|
static constexpr int OPERA_FIELD_LEN = 4;
|
|
static constexpr int CMD_FIELD_LEN = 4;
|
|
static constexpr int SEQ_FIELD_LEN = 4;
|
|
static constexpr int TCP_HEADER_SIZE = VZEB_HEAD_LEN + LENGTH_FIELD_LEN
|
|
+ OPERA_FIELD_LEN + CMD_FIELD_LEN
|
|
+ SEQ_FIELD_LEN; // = 20
|
|
|
|
QByteArray PackTcpFrame(TcpOperaType op, uint32_t cmd, uint32_t seq,
|
|
const QByteArray& payload);
|
|
bool UnpackTcpFrame(const QByteArray& frame, TcpOperaType& op, uint32_t& cmd,
|
|
uint32_t& seq, QByteArray& payload);
|
|
bool ParseVzebFrame(const uint8_t* data, int len, int& consumed,
|
|
TcpOperaType& op, uint32_t& cmd, uint32_t& seq,
|
|
QByteArray& payload);
|
|
|
|
// ---- Command handlers ----
|
|
QByteArray HandleCommand(uint32_t cmd, uint32_t seq, const QByteArray& payload);
|
|
QByteArray HandleOpenDevice(uint32_t seq, const QByteArray& payload);
|
|
QByteArray HandleReadRegister(uint32_t seq, const QByteArray& payload);
|
|
QByteArray HandleWriteRegister(uint32_t seq, const QByteArray& payload);
|
|
QByteArray HandleDeviceOption(uint32_t seq, const QByteArray& payload);
|
|
QByteArray HandleReadData(uint32_t seq, const QByteArray& payload);
|
|
QByteArray HandleWriteData(uint32_t seq, const QByteArray& payload);
|
|
QByteArray HandleExtDevice(uint32_t seq, const QByteArray& payload);
|
|
|
|
// ---- Data generators ----
|
|
void LoadImagesFromDirectory();
|
|
void LoadLaserDataFromDirectory();
|
|
QByteArray GenerateCheckerboardGray(int frameIdx, int offsetX, int offsetY);
|
|
QImage GenerateCheckerboardQImage(int frameIdx, int offsetX, int offsetY);
|
|
|
|
// ---- Laser data replay (from loaded files) ----
|
|
struct CachedLaserLine {
|
|
std::vector<float> x, y, z;
|
|
int pointCount = 0;
|
|
unsigned long long timestamp = 0;
|
|
unsigned long long frameIdx = 0;
|
|
};
|
|
CachedLaserLine GetNextLaserLine();
|
|
void ResetLaserReplay();
|
|
|
|
// ---- 3D laser frame builder (SDK-compatible PushLaserResult payload) ----
|
|
QByteArray BuildLaserFrame(const CachedLaserLine& line, int frameIdx,
|
|
const QImage& leftImg, const QImage& rightImg,
|
|
bool lastLine);
|
|
void SendPostCommand(uint32_t cmd, const QByteArray& payload);
|
|
|
|
// ---- Register map ----
|
|
void InitRegisterMap();
|
|
QByteArray ReadRegister(uint32_t addr) const;
|
|
void WriteRegister(uint32_t addr, const QByteArray& data);
|
|
bool HasRegister(uint32_t addr) const;
|
|
|
|
// ---- Winsock boilerplate ----
|
|
static bool InitWinsock();
|
|
static void SetSocketReuseAddr(SOCKET s);
|
|
bool CreateUdpSocket();
|
|
bool CreateTcpListenSocket();
|
|
|
|
// ---- State ----
|
|
std::atomic<bool> m_running{false};
|
|
std::atomic<bool> m_streaming{false};
|
|
std::atomic<int> m_connectedClients{0};
|
|
std::atomic<int> m_frameSeq{0};
|
|
|
|
// ---- Threads ----
|
|
std::thread m_udpThread;
|
|
std::thread m_tcpThread;
|
|
std::thread m_streamThread;
|
|
std::thread m_clientThread;
|
|
|
|
// ---- Sockets ----
|
|
SOCKET m_udpSocket = INVALID_SOCKET;
|
|
SOCKET m_tcpListenSocket = INVALID_SOCKET;
|
|
SOCKET m_tcpClientSocket = INVALID_SOCKET; // current client used for push
|
|
|
|
// ---- Mutexes ----
|
|
std::mutex m_clientMutex;
|
|
mutable std::mutex m_regMutex;
|
|
std::mutex m_sendMutex;
|
|
|
|
// All live client sockets (for shutdown wake-up). Each client runs its own
|
|
// thread with its own recv buffer, so the server never stalls on one client.
|
|
std::vector<SOCKET> m_clientSockets;
|
|
void RemoveClientSocket(SOCKET s);
|
|
|
|
// ---- Config ----
|
|
int m_imageWidth = 1280;
|
|
int m_imageHeight = 960;
|
|
int m_frameRate = 10;
|
|
int m_streamFrameRate = 30;
|
|
int m_udpPort = 6789;
|
|
int m_tcpPort = 6679;
|
|
|
|
// ---- Device identity ----
|
|
QString m_deviceIP;
|
|
QString m_deviceMAC;
|
|
QString m_serialNumber;
|
|
uint8_t m_deviceIPBytes[4] = {192, 168, 1, 200};
|
|
uint8_t m_deviceMACBytes[6] = {0x00, 0x0A, 0x35, 0x00, 0x00, 0x01};
|
|
uint8_t m_deviceSNBytes[8] = {'V', 'R', 'E', 'Y', 'E', '0', '0', '1'};
|
|
|
|
// ---- Virtual registers ----
|
|
// Register address -> variable-length value (QMatrix=128B, SN=8B, ROI=20B, ...)
|
|
std::unordered_map<uint32_t, QByteArray> m_registers;
|
|
|
|
// ---- Stored user data ----
|
|
QByteArray m_userData;
|
|
QByteArray m_calibMatrix; // 4x4 double = 128 bytes
|
|
|
|
// ---- Image storage ----
|
|
QString m_imageDir;
|
|
QStringList m_imageFiles; // base names (without -L/-R suffix)
|
|
std::vector<QImage> m_leftImages;
|
|
std::vector<QImage> m_rightImages;
|
|
std::mutex m_imageMutex;
|
|
|
|
// ---- Laser data storage ----
|
|
QString m_laserDataDir;
|
|
QStringList m_laserDataFiles; // file paths to .txt/.dat
|
|
std::vector<std::vector<CachedLaserLine>> m_laserFileLines; // lines per file
|
|
// recursive because GetNextLaserLine() recurses for empty files
|
|
std::recursive_mutex m_laserDataMutex;
|
|
|
|
// ---- Stream state ----
|
|
int m_imageReplayIdx = 0;
|
|
int m_laserFileReplayIdx = 0;
|
|
int m_laserLineReplayIdx = 0;
|
|
int m_scanLineIdx = 0;
|
|
int m_scanLinesPerCycle = 200;
|
|
|
|
// ---- Receive buffers ----
|
|
static constexpr int UDP_BUF_SIZE = 2048;
|
|
static constexpr int TCP_BUF_SIZE = 65536;
|
|
uint8_t m_udpRecvBuf[UDP_BUF_SIZE] = {};
|
|
uint8_t m_tcpRecvBuf[TCP_BUF_SIZE] = {};
|
|
int m_tcpRecvPos = 0;
|
|
|
|
// ---- Laser swing state ----
|
|
float m_swingAngle = -30.0f;
|
|
float m_swingAngleMin = -30.0f;
|
|
float m_swingAngleMax = 30.0f;
|
|
bool m_swingForward = true;
|
|
int m_swingSpeed = 36;
|
|
};
|
|
|
|
#endif // VRCAMERASIMULATOR_H
|