826 lines
28 KiB
C++
826 lines
28 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_bLeftCameraInfoReady(false)
|
||
, m_bRightCameraInfoReady(false)
|
||
, m_bCalibrationMatrixReady(false)
|
||
, m_bHeartbeatRunning(false)
|
||
, m_bHeartbeatEnabled(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;
|
||
m_cvHeartbeat.notify_one(); // 唤醒心跳线程
|
||
if (m_heartbeatThread.joinable()) {
|
||
m_heartbeatThread.join();
|
||
}
|
||
|
||
// 断开连接
|
||
if (m_pTcpClient) {
|
||
m_pTcpClient->CloseDevice();
|
||
}
|
||
|
||
m_bConnected = false;
|
||
m_dataBuffer.clear();
|
||
|
||
// 通知所有等待的条件变量，避免其他线程卡住
|
||
m_cvSingleDetection.notify_all();
|
||
m_cvImageData.notify_all();
|
||
m_cvLeftCameraInfo.notify_all();
|
||
m_cvRightCameraInfo.notify_all();
|
||
m_cvCalibrationMatrix.notify_all();
|
||
|
||
return 0;
|
||
}
|
||
|
||
bool BinocularMarkReceiver::IsConnected() const
|
||
{
|
||
return m_bConnected;
|
||
}
|
||
|
||
void BinocularMarkReceiver::SetMarkResultCallback(MarkResultCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
m_markResultCallback = callback;
|
||
}
|
||
|
||
void BinocularMarkReceiver::SetImageCallback(ImageCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
m_imageCallback = 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;
|
||
|
||
// 获取回调函数的拷贝，在锁外调用
|
||
EventCallback callback;
|
||
{
|
||
std::lock_guard<std::mutex> lock(pThis->m_mutex);
|
||
callback = pThis->m_eventCallback;
|
||
}
|
||
|
||
if (connected) {
|
||
LOG_DEBUG("Connected to server\n");
|
||
// 在锁外触发连接事件
|
||
if (callback) {
|
||
callback(ReceiverEventType::CONNECTED, "");
|
||
}
|
||
|
||
// 启动心跳线程
|
||
pThis->m_bHeartbeatRunning = true;
|
||
pThis->m_heartbeatThread = std::thread(&BinocularMarkReceiver::heartbeatThreadFunc, pThis);
|
||
} else {
|
||
LOG_DEBUG("Disconnected from server\n");
|
||
// 在锁外触发断开连接事件
|
||
if (callback) {
|
||
callback(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) {
|
||
// 使用 condition_variable 实现可中断的等待
|
||
std::unique_lock<std::mutex> lock(m_mutexHeartbeat);
|
||
m_cvHeartbeat.wait_for(lock, std::chrono::seconds(m_nHeartbeatInterval),
|
||
[this] { return !m_bHeartbeatRunning; });
|
||
|
||
if (!m_bHeartbeatRunning) break;
|
||
|
||
// 检查是否启用心跳
|
||
if (!m_bHeartbeatEnabled) continue;
|
||
|
||
// 发送心跳消息
|
||
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::vector<std::string> messagesToProcess;
|
||
|
||
{
|
||
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);
|
||
|
||
// 检查数据长度是否合理（最大50MB，支持双目高分辨率图像）
|
||
if (dataLength < 0 || dataLength > 50 * 1024 * 1024) {
|
||
LOG_WARN("Invalid data length: %lld, discarding frame header\n", dataLength);
|
||
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;
|
||
}
|
||
|
||
// 收集消息，稍后在锁外处理
|
||
messagesToProcess.push_back(jsonData);
|
||
|
||
// 移除已处理的帧
|
||
m_dataBuffer.erase(m_dataBuffer.begin(), m_dataBuffer.begin() + totalFrameLength);
|
||
}
|
||
} // 释放锁
|
||
|
||
// 在锁外处理所有消息
|
||
for (const auto& jsonData : messagesToProcess) {
|
||
handleJsonMessage(jsonData);
|
||
}
|
||
}
|
||
|
||
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") {
|
||
handleHeartbeat(jsonData);
|
||
} else if (messageType == "heartbeat_ack") {
|
||
handleHeartbeatAck(jsonData);
|
||
} else if (messageType == "cmd_response") {
|
||
handleCommandResponse(jsonData);
|
||
} else if (messageType == "camera_info_response") {
|
||
handleCameraInfoResponse(jsonData);
|
||
} else if (messageType == "calibration_matrix_response") {
|
||
handleCalibrationMatrixResponse(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);
|
||
}
|
||
}
|
||
|
||
// 提取图像数据
|
||
std::string leftImageBase64 = root.get("left_image", "").asString();
|
||
std::string rightImageBase64 = root.get("right_image", "").asString();
|
||
|
||
// 如果有图像数据且注册了图像回调，触发图像回调
|
||
if (!leftImageBase64.empty() && !rightImageBase64.empty()) {
|
||
ImageCallback imageCallback;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
imageCallback = m_imageCallback;
|
||
}
|
||
if (imageCallback) {
|
||
imageCallback(leftImageBase64, rightImageBase64, timestamp);
|
||
}
|
||
}
|
||
|
||
// 如果有marks数据且注册了mark回调，触发mark回调
|
||
if (!marks.empty()) {
|
||
MarkResultCallback callback;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
callback = m_markResultCallback;
|
||
}
|
||
if (callback) {
|
||
callback(marks, timestamp, errorCode);
|
||
}
|
||
}
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleHeartbeat(const std::string& jsonStr)
|
||
{
|
||
// 收到服务器心跳，回复心跳应答
|
||
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_ack", jsonData);
|
||
}
|
||
|
||
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::StartCapture()
|
||
{
|
||
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_continuous_image", jsonData);
|
||
}
|
||
|
||
int BinocularMarkReceiver::StopCapture()
|
||
{
|
||
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_continuous_image", 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(SVrCameraEnum target, 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;
|
||
if (target == SVrCameraEnum::LEFT) {
|
||
root["camera"] = "left";
|
||
} else if (target == SVrCameraEnum::RIGHT) {
|
||
root["camera"] = "right";
|
||
}
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
return sendJsonMessage("cmd_set_exposure_time", jsonData);
|
||
}
|
||
|
||
int BinocularMarkReceiver::SetGain(SVrCameraEnum target, 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;
|
||
if (target == SVrCameraEnum::LEFT) {
|
||
root["camera"] = "left";
|
||
} else if (target == SVrCameraEnum::RIGHT) {
|
||
root["camera"] = "right";
|
||
}
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
return sendJsonMessage("cmd_set_gain", jsonData);
|
||
}
|
||
|
||
IBinocularMarkReceiver::CameraInfo BinocularMarkReceiver::GetCameraInfo(SVrCameraEnum target, int timeoutMs)
|
||
{
|
||
CameraInfo result;
|
||
result.exposureTime = 0.0;
|
||
result.gain = 0.0;
|
||
|
||
if (!m_bConnected || target == SVrCameraEnum::BOTH) {
|
||
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());
|
||
root["camera"] = (target == SVrCameraEnum::LEFT) ? "left" : "right";
|
||
Json::FastWriter writer;
|
||
std::string jsonData = writer.write(root);
|
||
|
||
if (target == SVrCameraEnum::LEFT) {
|
||
m_bLeftCameraInfoReady = false;
|
||
if (sendJsonMessage("cmd_get_camera_info", jsonData) != 0) {
|
||
return result;
|
||
}
|
||
std::unique_lock<std::mutex> lock(m_mutex);
|
||
if (m_cvLeftCameraInfo.wait_for(lock, std::chrono::milliseconds(timeoutMs),
|
||
[this] { return m_bLeftCameraInfoReady; })) {
|
||
return m_pendingLeftCameraInfo;
|
||
}
|
||
} else {
|
||
m_bRightCameraInfoReady = false;
|
||
if (sendJsonMessage("cmd_get_camera_info", jsonData) != 0) {
|
||
return result;
|
||
}
|
||
std::unique_lock<std::mutex> lock(m_mutex);
|
||
if (m_cvRightCameraInfo.wait_for(lock, std::chrono::milliseconds(timeoutMs),
|
||
[this] { return m_bRightCameraInfoReady; })) {
|
||
return m_pendingRightCameraInfo;
|
||
}
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
std::string BinocularMarkReceiver::GetCalibrationMatrix(int timeoutMs)
|
||
{
|
||
std::string result;
|
||
|
||
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_bCalibrationMatrixReady = false;
|
||
if (sendJsonMessage("cmd_get_calibration", jsonData) != 0) {
|
||
return result;
|
||
}
|
||
|
||
std::unique_lock<std::mutex> lock(m_mutex);
|
||
if (m_cvCalibrationMatrix.wait_for(lock, std::chrono::milliseconds(timeoutMs),
|
||
[this] { return m_bCalibrationMatrixReady; })) {
|
||
return m_pendingCalibrationMatrix;
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
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");
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleCameraInfoResponse(const std::string& jsonStr)
|
||
{
|
||
LOG_DEBUG("handleCameraInfoResponse start, JSON size: %zu\n", jsonStr.size());
|
||
|
||
Json::Reader reader;
|
||
Json::Value root;
|
||
if (!reader.parse(jsonStr, root)) {
|
||
LOG_DEBUG("handleCameraInfoResponse JSON parse failed\n");
|
||
return;
|
||
}
|
||
|
||
LOG_DEBUG("handleCameraInfoResponse JSON parsed\n");
|
||
|
||
std::string camera = root.get("camera", "").asString();
|
||
std::string serialNumber = root.get("serial_number", "").asString();
|
||
std::string modelName = root.get("model_name", "").asString();
|
||
std::string displayName = root.get("display_name", "").asString();
|
||
double exposureTime = root.get("exposure_time", 0.0).asDouble();
|
||
double gain = root.get("gain", 0.0).asDouble();
|
||
|
||
LOG_DEBUG("handleCameraInfoResponse got data: camera=%s, SN=%s, model=%s, exposure=%.2f, gain=%.2f\n",
|
||
camera.c_str(), serialNumber.c_str(), modelName.c_str(), exposureTime, gain);
|
||
|
||
// 存储结果并通知等待的线程
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
LOG_DEBUG("handleCameraInfoResponse storing data\n");
|
||
|
||
if (camera == "left") {
|
||
m_pendingLeftCameraInfo.serialNumber = serialNumber;
|
||
m_pendingLeftCameraInfo.modelName = modelName;
|
||
m_pendingLeftCameraInfo.displayName = displayName;
|
||
m_pendingLeftCameraInfo.exposureTime = exposureTime;
|
||
m_pendingLeftCameraInfo.gain = gain;
|
||
m_bLeftCameraInfoReady = true;
|
||
LOG_DEBUG("handleCameraInfoResponse left camera data stored\n");
|
||
} else if (camera == "right") {
|
||
m_pendingRightCameraInfo.serialNumber = serialNumber;
|
||
m_pendingRightCameraInfo.modelName = modelName;
|
||
m_pendingRightCameraInfo.displayName = displayName;
|
||
m_pendingRightCameraInfo.exposureTime = exposureTime;
|
||
m_pendingRightCameraInfo.gain = gain;
|
||
m_bRightCameraInfoReady = true;
|
||
LOG_DEBUG("handleCameraInfoResponse right camera data stored\n");
|
||
}
|
||
} // 释放锁后再通知
|
||
|
||
LOG_DEBUG("handleCameraInfoResponse notifying\n");
|
||
if (camera == "left") {
|
||
m_cvLeftCameraInfo.notify_one();
|
||
} else if (camera == "right") {
|
||
m_cvRightCameraInfo.notify_one();
|
||
}
|
||
LOG_DEBUG("handleCameraInfoResponse complete\n");
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleCalibrationMatrixResponse(const std::string& jsonStr)
|
||
{
|
||
Json::Reader reader;
|
||
Json::Value root;
|
||
if (!reader.parse(jsonStr, root)) {
|
||
return;
|
||
}
|
||
|
||
std::string calibrationXml = root.get("calibration_xml", "").asString();
|
||
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
m_pendingCalibrationMatrix = calibrationXml;
|
||
m_bCalibrationMatrixReady = true;
|
||
}
|
||
|
||
m_cvCalibrationMatrix.notify_one();
|
||
}
|