575 lines
18 KiB
C++
575 lines
18 KiB
C++
#include "BinocularMarkReceiver.h"
|
||
#include "json/json.h"
|
||
#include "VrLog.h"
|
||
#include <algorithm>
|
||
#include <chrono>
|
||
#include <cstring>
|
||
|
||
|
||
using namespace VA;
|
||
|
||
// 静态工厂方法实现
|
||
int IBinocularMarkReceiver::CreateInstance(IBinocularMarkReceiver** ppReceiver)
|
||
{
|
||
if (!ppReceiver) {
|
||
return -1;
|
||
}
|
||
|
||
*ppReceiver = new BinocularMarkReceiver();
|
||
return 0;
|
||
}
|
||
|
||
BinocularMarkReceiver::BinocularMarkReceiver()
|
||
: m_pTcpClient(nullptr)
|
||
, m_serverPort(0)
|
||
, m_bConnected(false)
|
||
, m_bSingleDetectionReady(false)
|
||
, m_bImageDataReady(false)
|
||
, m_bHeartbeatRunning(false)
|
||
, m_nHeartbeatInterval(30)
|
||
{
|
||
m_pTcpClient = IVrTCPClient::CreateInstance();
|
||
}
|
||
|
||
BinocularMarkReceiver::~BinocularMarkReceiver()
|
||
{
|
||
Disconnect();
|
||
|
||
if (m_pTcpClient) {
|
||
IVrTCPClient::DestroyInstance(m_pTcpClient);
|
||
m_pTcpClient = nullptr;
|
||
}
|
||
}
|
||
|
||
int BinocularMarkReceiver::Connect(const std::string& serverIp, uint16_t serverPort)
|
||
{
|
||
if (m_bConnected) {
|
||
LOG_DEBUG("Already connected\n");
|
||
return 0;
|
||
}
|
||
|
||
m_serverIp = serverIp;
|
||
m_serverPort = serverPort;
|
||
|
||
LOG_DEBUG("Connecting to %s:%d\n", serverIp.c_str(), serverPort);
|
||
|
||
// 连接到服务器
|
||
int ret = m_pTcpClient->LinkDevice(serverIp, serverPort, false, linkEventCallback, this);
|
||
if (ret != 0) {
|
||
LOG_DEBUG("LinkDevice failed: %d\n", ret);
|
||
return -1;
|
||
}
|
||
|
||
// 启动工作线程
|
||
ret = m_pTcpClient->StartWork(tcpRecvCallback, this);
|
||
if (ret != 0) {
|
||
LOG_DEBUG("StartWork failed: %d\n", ret);
|
||
m_pTcpClient->CloseDevice();
|
||
return -1;
|
||
}
|
||
|
||
LOG_DEBUG("Connect initiated\n");
|
||
return 0;
|
||
}
|
||
|
||
int BinocularMarkReceiver::Disconnect()
|
||
{
|
||
if (!m_bConnected) {
|
||
return 0;
|
||
}
|
||
|
||
// 停止心跳线程
|
||
m_bHeartbeatRunning = false;
|
||
if (m_heartbeatThread.joinable()) {
|
||
m_heartbeatThread.join();
|
||
}
|
||
|
||
// 断开连接
|
||
if (m_pTcpClient) {
|
||
m_pTcpClient->CloseDevice();
|
||
}
|
||
|
||
m_bConnected = false;
|
||
m_dataBuffer.clear();
|
||
|
||
return 0;
|
||
}
|
||
|
||
bool BinocularMarkReceiver::IsConnected() const
|
||
{
|
||
return m_bConnected;
|
||
}
|
||
|
||
int BinocularMarkReceiver::TriggerDetection()
|
||
{
|
||
Json::Value root;
|
||
root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch()).count());
|
||
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
|
||
return sendJsonMessage("cmd_trigger", jsonData);
|
||
}
|
||
|
||
void BinocularMarkReceiver::SetMarkResultCallback(MarkResultCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
m_markResultCallback = callback;
|
||
}
|
||
|
||
void BinocularMarkReceiver::SetEventCallback(EventCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
m_eventCallback = callback;
|
||
}
|
||
|
||
// TCP连接状态回调
|
||
void BinocularMarkReceiver::linkEventCallback(IVrTCPClient* pClient, bool connected, void* pParam)
|
||
{
|
||
BinocularMarkReceiver* pThis = static_cast<BinocularMarkReceiver*>(pParam);
|
||
if (!pThis) return;
|
||
|
||
pThis->m_bConnected = connected;
|
||
|
||
if (connected) {
|
||
LOG_DEBUG("Connected to server\n");
|
||
// 触发连接事件
|
||
if (pThis->m_eventCallback) {
|
||
pThis->m_eventCallback(ReceiverEventType::CONNECTED, "");
|
||
}
|
||
|
||
// 启动心跳线程
|
||
pThis->m_bHeartbeatRunning = true;
|
||
pThis->m_heartbeatThread = std::thread(&BinocularMarkReceiver::heartbeatThreadFunc, pThis);
|
||
} else {
|
||
LOG_DEBUG("Disconnected from server\n");
|
||
// 触发断开连接事件
|
||
if (pThis->m_eventCallback) {
|
||
pThis->m_eventCallback(ReceiverEventType::DISCONNECTED, "");
|
||
}
|
||
}
|
||
}
|
||
|
||
// TCP数据接收回调
|
||
void BinocularMarkReceiver::tcpRecvCallback(IVrTCPClient* pClient, const char* pData, const int nLen, void* pParam)
|
||
{
|
||
BinocularMarkReceiver* pThis = static_cast<BinocularMarkReceiver*>(pParam);
|
||
if (!pThis || !pData || nLen <= 0) return;
|
||
|
||
// 追加到缓冲区
|
||
{
|
||
std::lock_guard<std::mutex> lock(pThis->m_mutex);
|
||
pThis->m_dataBuffer.insert(pThis->m_dataBuffer.end(), pData, pData + nLen);
|
||
}
|
||
|
||
// 解析数据帧
|
||
pThis->parseFrames();
|
||
}
|
||
|
||
// 心跳线程函数
|
||
void BinocularMarkReceiver::heartbeatThreadFunc()
|
||
{
|
||
while (m_bHeartbeatRunning) {
|
||
std::this_thread::sleep_for(std::chrono::seconds(m_nHeartbeatInterval));
|
||
|
||
if (!m_bHeartbeatRunning) break;
|
||
|
||
// 发送心跳消息
|
||
Json::Value root;
|
||
root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch()).count());
|
||
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
|
||
sendJsonMessage("heartbeat", jsonData);
|
||
}
|
||
}
|
||
|
||
void BinocularMarkReceiver::parseFrames()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
|
||
while (true) {
|
||
// 查找帧头
|
||
auto it = std::search(m_dataBuffer.begin(), m_dataBuffer.end(), FRAME_HEADER, FRAME_HEADER + FRAME_HEADER_SIZE);
|
||
if (it == m_dataBuffer.end()) {
|
||
m_dataBuffer.clear();
|
||
break;
|
||
}
|
||
|
||
// 丢弃帧头之前的数据
|
||
if (it != m_dataBuffer.begin()) {
|
||
m_dataBuffer.erase(m_dataBuffer.begin(), it);
|
||
}
|
||
|
||
// 检查是否有足够的数据来读取长度字段
|
||
if (m_dataBuffer.size() < FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE) {
|
||
break;
|
||
}
|
||
|
||
// 读取数据长度（8字节ASCII字符串格式）
|
||
std::string lengthStr(m_dataBuffer.begin() + FRAME_HEADER_SIZE, m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE);
|
||
int64_t dataLength = std::stoll(lengthStr);
|
||
|
||
// 检查数据长度是否合理（最大10MB）
|
||
if (dataLength < 0 || dataLength > 10 * 1024 * 1024) {
|
||
m_dataBuffer.erase(m_dataBuffer.begin(), m_dataBuffer.begin() + FRAME_HEADER_SIZE);
|
||
continue;
|
||
}
|
||
|
||
// 计算完整帧的总长度
|
||
size_t totalFrameLength = FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength + FRAME_TAIL_SIZE;
|
||
|
||
// 检查是否有完整的帧
|
||
if (m_dataBuffer.size() < totalFrameLength) {
|
||
break;
|
||
}
|
||
|
||
// 提取JSON数据
|
||
std::string jsonData(m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE,
|
||
m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength);
|
||
|
||
// 验证帧尾
|
||
std::string tail(m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength,
|
||
m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength + FRAME_TAIL_SIZE);
|
||
if (tail != FRAME_TAIL) {
|
||
m_dataBuffer.erase(m_dataBuffer.begin(), m_dataBuffer.begin() + FRAME_HEADER_SIZE);
|
||
continue;
|
||
}
|
||
|
||
// 处理JSON消息
|
||
handleJsonMessage(jsonData);
|
||
|
||
// 移除已处理的帧
|
||
m_dataBuffer.erase(m_dataBuffer.begin(), m_dataBuffer.begin() + totalFrameLength);
|
||
}
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleJsonMessage(const std::string& jsonData)
|
||
{
|
||
Json::Reader reader;
|
||
Json::Value root;
|
||
|
||
if (!reader.parse(jsonData, root)) {
|
||
LOG_DEBUG("JSON parse failed\n");
|
||
return;
|
||
}
|
||
|
||
if (!root.isObject()) {
|
||
LOG_DEBUG("JSON is not an object\n");
|
||
return;
|
||
}
|
||
|
||
// 获取消息类型
|
||
std::string messageType = root.get("msg_type", "").asString();
|
||
LOG_DEBUG("Received message type: %s\n", messageType.c_str());
|
||
|
||
if (messageType == "mark_result") {
|
||
handleMarkResult(jsonData);
|
||
} else if (messageType == "single_detection_result") {
|
||
handleSingleDetectionResult(jsonData);
|
||
} else if (messageType == "image_data") {
|
||
handleImageData(jsonData);
|
||
} else if (messageType == "heartbeat_ack") {
|
||
handleHeartbeatAck(jsonData);
|
||
} else if (messageType == "cmd_response") {
|
||
handleCommandResponse(jsonData);
|
||
} else {
|
||
LOG_DEBUG("Unknown message type: %s\n", messageType.c_str());
|
||
}
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleMarkResult(const std::string& jsonStr)
|
||
{
|
||
Json::Reader reader;
|
||
Json::Value root;
|
||
if (!reader.parse(jsonStr, root)) return;
|
||
|
||
int64_t timestamp = root.get("timestamp", 0).asInt64();
|
||
int errorCode = root.get("error_code", -1).asInt();
|
||
|
||
std::vector<VrMark3D> marks;
|
||
|
||
if (errorCode == 0 && root.isMember("marks")) {
|
||
const Json::Value& marksArray = root["marks"];
|
||
for (unsigned int i = 0; i < marksArray.size(); i++) {
|
||
VrMark3D mark;
|
||
mark.markID = marksArray[i].get("mark_id", -1).asInt();
|
||
mark.x = marksArray[i].get("x", 0.0).asDouble();
|
||
mark.y = marksArray[i].get("y", 0.0).asDouble();
|
||
mark.z = marksArray[i].get("z", 0.0).asDouble();
|
||
marks.push_back(mark);
|
||
}
|
||
}
|
||
|
||
if (m_markResultCallback) {
|
||
m_markResultCallback(marks, timestamp, errorCode);
|
||
}
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleHeartbeatAck(const std::string& jsonStr)
|
||
{
|
||
// 心跳应答，暂时不处理
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleCommandResponse(const std::string& jsonStr)
|
||
{
|
||
// 命令应答，暂时不处理
|
||
}
|
||
|
||
std::string BinocularMarkReceiver::buildFrame(const std::string& jsonData)
|
||
{
|
||
std::string frame;
|
||
|
||
// 添加帧头
|
||
frame.append(FRAME_HEADER, FRAME_HEADER_SIZE);
|
||
|
||
// 添加数据长度（8字节ASCII字符串格式）
|
||
char lengthStr[9];
|
||
snprintf(lengthStr, sizeof(lengthStr), "%08zu", jsonData.size());
|
||
frame.append(lengthStr, FRAME_LENGTH_SIZE);
|
||
|
||
// 添加JSON数据
|
||
frame.append(jsonData);
|
||
|
||
// 添加帧尾
|
||
frame.append(FRAME_TAIL, FRAME_TAIL_SIZE);
|
||
|
||
return frame;
|
||
}
|
||
|
||
int BinocularMarkReceiver::sendJsonMessage(const std::string& messageType, const std::string& jsonData)
|
||
{
|
||
if (!m_bConnected) {
|
||
return -1;
|
||
}
|
||
|
||
// 解析JSON并添加msg_type字段
|
||
Json::Reader reader;
|
||
Json::Value root;
|
||
if (!reader.parse(jsonData, root)) {
|
||
root = Json::Value(Json::objectValue);
|
||
}
|
||
root["msg_type"] = messageType;
|
||
|
||
// 转换为JSON字符串
|
||
Json::FastWriter writer;
|
||
std::string finalJsonData = writer.write(root);
|
||
|
||
// 构造数据帧
|
||
std::string frame = buildFrame(finalJsonData);
|
||
|
||
// 发送数据
|
||
if (!m_pTcpClient->SendData(frame.c_str(), frame.size())) {
|
||
return -1;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
// ========== 新增接口实现 ==========
|
||
|
||
IBinocularMarkReceiver::SingleDetectionResult BinocularMarkReceiver::RequestSingleDetection(int timeoutMs)
|
||
{
|
||
SingleDetectionResult result;
|
||
result.errorCode = -1;
|
||
|
||
if (!m_bConnected) {
|
||
return result;
|
||
}
|
||
|
||
// 发送请求
|
||
Json::Value root;
|
||
root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch()).count());
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
|
||
m_bSingleDetectionReady = false;
|
||
if (sendJsonMessage("cmd_single_detection", jsonData) != 0) {
|
||
return result;
|
||
}
|
||
|
||
// 使用condition_variable同步等待结果
|
||
std::unique_lock<std::mutex> lock(m_mutex);
|
||
if (m_cvSingleDetection.wait_for(lock, std::chrono::milliseconds(timeoutMs),
|
||
[this] { return m_bSingleDetectionReady; })) {
|
||
return m_pendingSingleDetectionResult;
|
||
}
|
||
|
||
result.errorCode = -2; // 超时
|
||
return result;
|
||
}
|
||
|
||
IBinocularMarkReceiver::ImageData BinocularMarkReceiver::RequestSingleImage(int timeoutMs)
|
||
{
|
||
ImageData result;
|
||
result.timestamp = 0;
|
||
|
||
if (!m_bConnected) {
|
||
return result;
|
||
}
|
||
|
||
// 发送请求
|
||
Json::Value root;
|
||
root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch()).count());
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
|
||
m_bImageDataReady = false;
|
||
if (sendJsonMessage("cmd_single_image", jsonData) != 0) {
|
||
return result;
|
||
}
|
||
|
||
// 使用condition_variable同步等待结果
|
||
std::unique_lock<std::mutex> lock(m_mutex);
|
||
if (m_cvImageData.wait_for(lock, std::chrono::milliseconds(timeoutMs), [this] { return m_bImageDataReady; })) {
|
||
return m_pendingImageData;
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
int BinocularMarkReceiver::StartWork()
|
||
{
|
||
Json::Value root;
|
||
root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch()).count());
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
return sendJsonMessage("cmd_start_work", jsonData);
|
||
}
|
||
|
||
int BinocularMarkReceiver::StopWork()
|
||
{
|
||
Json::Value root;
|
||
root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch()).count());
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
return sendJsonMessage("cmd_stop_work", jsonData);
|
||
}
|
||
|
||
int BinocularMarkReceiver::SetCalibrationMatrix(const std::string& calibrationXml)
|
||
{
|
||
Json::Value root;
|
||
root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch()).count());
|
||
root["calibration_xml"] = calibrationXml;
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
return sendJsonMessage("cmd_set_calibration", jsonData);
|
||
}
|
||
|
||
int BinocularMarkReceiver::SetExposureTime(double exposureTime)
|
||
{
|
||
Json::Value root;
|
||
root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch()).count());
|
||
root["exposure_time"] = exposureTime;
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
return sendJsonMessage("cmd_set_exposure_time", jsonData);
|
||
}
|
||
|
||
int BinocularMarkReceiver::SetGain(double gain)
|
||
{
|
||
Json::Value root;
|
||
root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch()).count());
|
||
root["gain"] = gain;
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
return sendJsonMessage("cmd_set_gain", jsonData);
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleSingleDetectionResult(const std::string& jsonStr)
|
||
{
|
||
LOG_DEBUG("handleSingleDetectionResult start, JSON size: %zu\n", jsonStr.size());
|
||
|
||
Json::Reader reader;
|
||
Json::Value root;
|
||
if (!reader.parse(jsonStr, root)) {
|
||
LOG_DEBUG("handleSingleDetectionResult JSON parse failed\n");
|
||
return;
|
||
}
|
||
|
||
LOG_DEBUG("handleSingleDetectionResult JSON parsed\n");
|
||
|
||
int64_t timestamp = root.get("timestamp", 0).asInt64();
|
||
int errorCode = root.get("error_code", -1).asInt();
|
||
std::string leftImageBase64 = root.get("left_image", "").asString();
|
||
std::string rightImageBase64 = root.get("right_image", "").asString();
|
||
|
||
LOG_DEBUG("handleSingleDetectionResult got data: timestamp=%lld, error=%d, left_size=%zu, right_size=%zu\n",
|
||
timestamp, errorCode, leftImageBase64.size(), rightImageBase64.size());
|
||
|
||
std::vector<VrMark3D> marks;
|
||
if (errorCode == 0 && root.isMember("marks")) {
|
||
const Json::Value& marksArray = root["marks"];
|
||
for (unsigned int i = 0; i < marksArray.size(); i++) {
|
||
VrMark3D mark;
|
||
mark.markID = marksArray[i].get("mark_id", -1).asInt();
|
||
mark.x = marksArray[i].get("x", 0.0).asDouble();
|
||
mark.y = marksArray[i].get("y", 0.0).asDouble();
|
||
mark.z = marksArray[i].get("z", 0.0).asDouble();
|
||
marks.push_back(mark);
|
||
}
|
||
}
|
||
|
||
LOG_DEBUG("handleSingleDetectionResult parsed %zu marks\n", marks.size());
|
||
|
||
// 存储结果并通知等待的线程
|
||
{
|
||
// std::lock_guard<std::mutex> lock(m_mutex);
|
||
LOG_DEBUG("handleSingleDetectionResult storing data\n");
|
||
m_pendingSingleDetectionResult.marks = marks;
|
||
m_pendingSingleDetectionResult.leftImageBase64 = leftImageBase64;
|
||
m_pendingSingleDetectionResult.rightImageBase64 = rightImageBase64;
|
||
m_pendingSingleDetectionResult.timestamp = timestamp;
|
||
m_pendingSingleDetectionResult.errorCode = errorCode;
|
||
m_bSingleDetectionReady = true;
|
||
LOG_DEBUG("handleSingleDetectionResult data stored\n");
|
||
}
|
||
LOG_DEBUG("handleSingleDetectionResult notifying\n");
|
||
m_cvSingleDetection.notify_one();
|
||
LOG_DEBUG("handleSingleDetectionResult complete\n");
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleImageData(const std::string& jsonStr)
|
||
{
|
||
LOG_DEBUG("handleImageData start, JSON size: %zu\n", jsonStr.size());
|
||
|
||
Json::Reader reader;
|
||
Json::Value root;
|
||
if (!reader.parse(jsonStr, root)) {
|
||
LOG_DEBUG("handleImageData JSON parse failed\n");
|
||
return;
|
||
}
|
||
|
||
LOG_DEBUG("handleImageData JSON parsed\n");
|
||
|
||
int64_t timestamp = root.get("timestamp", 0).asInt64();
|
||
std::string leftImageBase64 = root.get("left_image", "").asString();
|
||
std::string rightImageBase64 = root.get("right_image", "").asString();
|
||
|
||
LOG_DEBUG("handleImageData got data: timestamp=%lld, left_size=%zu, right_size=%zu\n", timestamp, leftImageBase64.size(), rightImageBase64.size());
|
||
|
||
// 存储结果并通知等待的线程
|
||
{
|
||
// std::lock_guard<std::mutex> lock(m_mutex);
|
||
LOG_DEBUG("handleImageData storing data\n");
|
||
m_pendingImageData.leftImageBase64 = leftImageBase64;
|
||
m_pendingImageData.rightImageBase64 = rightImageBase64;
|
||
m_pendingImageData.timestamp = timestamp;
|
||
m_bImageDataReady = true;
|
||
LOG_DEBUG("handleImageData data stored\n");
|
||
}
|
||
LOG_DEBUG("handleImageData notifying\n");
|
||
m_cvImageData.notify_one();
|
||
LOG_DEBUG("handleImageData complete\n");
|
||
}
|