2009 lines
69 KiB
C++
2009 lines
69 KiB
C++
#include "ParkingSpaceGuidePresenter.h"
|
||
|
||
#include <algorithm>
|
||
#include <chrono>
|
||
#include <cstring>
|
||
#include <exception>
|
||
#include <fstream>
|
||
#include <limits>
|
||
|
||
#include <QCollator>
|
||
#include <QDateTime>
|
||
#include <QDir>
|
||
#include <QFile>
|
||
#include <QFileInfo>
|
||
#include <QImageReader>
|
||
#include <QJsonDocument>
|
||
#include <QJsonObject>
|
||
|
||
#include "DetectPresenter.h"
|
||
#include "IYUDPClient.h"
|
||
#include "LaserDataLoader.h"
|
||
#include "ParkingStatusHttpServer.h"
|
||
#include "PathManager.h"
|
||
#include "Version.h"
|
||
#include "VrLog.h"
|
||
|
||
namespace {
|
||
|
||
constexpr int kMvsPixelTypeMono8 = 0x01080001;
|
||
constexpr int kMvsPixelTypeRGB8 = 0x02180014;
|
||
constexpr int kMvsPixelTypeBGR8 = 0x02180015;
|
||
constexpr int kMvsAcqModeContinuous = 2;
|
||
constexpr int kAirplaneProbeIntervalMs = 1000;
|
||
constexpr unsigned int kModelCaptureTimeoutMs = 1000;
|
||
|
||
void SortPathsNaturally(std::vector<QString>& paths)
|
||
{
|
||
QCollator collator;
|
||
collator.setCaseSensitivity(Qt::CaseInsensitive);
|
||
collator.setNumericMode(true);
|
||
std::sort(paths.begin(), paths.end(), [&collator](const QString& lhs,
|
||
const QString& rhs) {
|
||
return collator.compare(QFileInfo(lhs).fileName(),
|
||
QFileInfo(rhs).fileName()) < 0;
|
||
});
|
||
}
|
||
|
||
QString ConfigText(const std::string& value, const QString& fallback)
|
||
{
|
||
const QString text = QString::fromStdString(value).trimmed();
|
||
return text.isEmpty() ? fallback : text;
|
||
}
|
||
|
||
QString ResultText(const QString& value, const QString& fallback)
|
||
{
|
||
const QString text = value.trimmed();
|
||
return text.isEmpty() ? fallback : text;
|
||
}
|
||
|
||
ParkingSpaceGuideInfo FallbackGuideInfo(const UdpBroadcastConfig& config)
|
||
{
|
||
ParkingSpaceGuideInfo info;
|
||
info.targetId = ConfigText(config.targetId, QStringLiteral("T001"));
|
||
info.parkId = ConfigText(config.parkId, QStringLiteral("P01"));
|
||
info.modelType = QStringLiteral("未知");
|
||
return info;
|
||
}
|
||
|
||
bool IsExceptionGuideState(int guideStateCode)
|
||
{
|
||
switch (guideStateCode) {
|
||
case 10:
|
||
case 11:
|
||
case 12:
|
||
case 13:
|
||
case 14:
|
||
case 15:
|
||
case 16:
|
||
case 17:
|
||
case 18:
|
||
case 19:
|
||
case 20:
|
||
case 21:
|
||
case 22:
|
||
case 23:
|
||
case 24:
|
||
return true;
|
||
default:
|
||
return false;
|
||
}
|
||
}
|
||
|
||
QJsonObject BuildGuideInfoJson(const ParkingSpaceGuideInfo& info,
|
||
const UdpBroadcastConfig& config)
|
||
{
|
||
const ParkingSpaceGuideInfo fallback = FallbackGuideInfo(config);
|
||
|
||
QJsonObject object;
|
||
object["targetId"] = ResultText(info.targetId, fallback.targetId);
|
||
object["parkId"] = ResultText(info.parkId, fallback.parkId);
|
||
object["modelType"] = ResultText(info.modelType, fallback.modelType);
|
||
object["guideStateCode"] = info.guideStateCode;
|
||
object["distanceToStop"] = info.distance;
|
||
object["bodyYawAngle"] = info.angle;
|
||
object["lateralOffset"] = info.lateralOffset;
|
||
object["aircraftSpeed"] = info.aircraftSpeed;
|
||
object["hasException"] = info.hasException || IsExceptionGuideState(info.guideStateCode);
|
||
object["guideText"] = info.guideText;
|
||
return object;
|
||
}
|
||
|
||
QJsonObject BuildDetectionResultPayload(const DetectionResult& result,
|
||
const UdpBroadcastConfig& config)
|
||
{
|
||
const bool hasResult = !result.parkingSpaceInfoList.empty();
|
||
const bool success = result.errorCode == 0 && hasResult;
|
||
const ParkingSpaceGuideInfo firstInfo = hasResult
|
||
? result.parkingSpaceInfoList.front()
|
||
: FallbackGuideInfo(config);
|
||
const QJsonObject firstResult = BuildGuideInfoJson(firstInfo, config);
|
||
|
||
QJsonObject payload;
|
||
payload["protocol"] = QStringLiteral("ParkingSpaceGuideResult");
|
||
payload["version"] = QStringLiteral("1.0");
|
||
payload["success"] = success;
|
||
payload["hasResult"] = hasResult;
|
||
payload["targetId"] = firstResult["targetId"];
|
||
payload["parkId"] = firstResult["parkId"];
|
||
payload["modelType"] = firstResult["modelType"];
|
||
payload["guideStateCode"] = firstResult["guideStateCode"];
|
||
payload["distanceToStop"] = firstResult["distanceToStop"];
|
||
payload["bodyYawAngle"] = firstResult["bodyYawAngle"];
|
||
payload["lateralOffset"] = firstResult["lateralOffset"];
|
||
payload["aircraftSpeed"] = firstResult["aircraftSpeed"];
|
||
payload["hasException"] = result.errorCode != 0 || firstResult["hasException"].toBool();
|
||
payload["guideText"] = firstResult["guideText"];
|
||
payload["message"] = result.message;
|
||
payload["errorCode"] = result.errorCode;
|
||
payload["timestamp"] = QDateTime::currentMSecsSinceEpoch();
|
||
return payload;
|
||
}
|
||
|
||
} // namespace
|
||
|
||
ParkingSpaceGuidePresenter::ParkingSpaceGuidePresenter(QObject* parent)
|
||
: ParkingSpaceGuidePresenter(ParkingSpaceGuideSimulationOptions(), parent)
|
||
{
|
||
}
|
||
|
||
qint64 MonotonicMilliseconds()
|
||
{
|
||
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::steady_clock::now().time_since_epoch())
|
||
.count();
|
||
}
|
||
|
||
ParkingSpaceGuidePresenter::ParkingSpaceGuidePresenter(
|
||
const ParkingSpaceGuideSimulationOptions& simulationOptions,
|
||
QObject* parent)
|
||
: QObject(parent)
|
||
, m_simulationOptions(simulationOptions)
|
||
{
|
||
m_simulationOptions.frameIntervalMs =
|
||
(std::max)(1, m_simulationOptions.frameIntervalMs);
|
||
}
|
||
|
||
ParkingSpaceGuidePresenter::~ParkingSpaceGuidePresenter()
|
||
{
|
||
DeinitApp();
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::Init()
|
||
{
|
||
m_reconfiguring.store(false);
|
||
NotifyWorkStatus(WorkStatus::InitIng);
|
||
NotifyStatus("停机引导初始化中");
|
||
|
||
int ret = InitConfig();
|
||
if (ret != 0) {
|
||
NotifyWorkStatus(WorkStatus::Error);
|
||
return ret;
|
||
}
|
||
|
||
const ConfigResult config = m_configManager->GetConfigResult();
|
||
m_cameraList.clear();
|
||
|
||
DetectionResult initialStatus;
|
||
initialStatus.errorCode = -1;
|
||
initialStatus.message = QStringLiteral("暂无检测结果");
|
||
UpdateCurrentStatus(initialStatus);
|
||
|
||
DetectionResult waitingDisplay;
|
||
ParkingSpaceGuideInfo waitingInfo;
|
||
waitingInfo.targetId = ConfigText(
|
||
config.udpBroadcastConfig.targetId,
|
||
QStringLiteral("T001"));
|
||
waitingInfo.parkId = ConfigText(
|
||
config.udpBroadcastConfig.parkId,
|
||
QStringLiteral("P01"));
|
||
waitingInfo.modelType = QStringLiteral("未知");
|
||
waitingInfo.guideStateCode = static_cast<int>(ParkingGuideState::Waiting);
|
||
waitingInfo.guideText = QStringLiteral("WAIT");
|
||
waitingDisplay.parkingSpaceInfoList.push_back(waitingInfo);
|
||
waitingDisplay.message = QStringLiteral("等待飞机进入引导区域");
|
||
|
||
m_detectPresenter = new DetectPresenter();
|
||
|
||
if (m_simulationOptions.enabled) {
|
||
ret = InitSimulationData();
|
||
if (ret != 0) {
|
||
NotifyWorkStatus(WorkStatus::Error);
|
||
return ret;
|
||
}
|
||
} else {
|
||
ret = InitMvsCamera(config.mvsCamera);
|
||
if (ret != 0) {
|
||
NotifyStatus("平面相机初始化失败，继续使用预留算法模式");
|
||
}
|
||
if (m_cameraList.empty()) {
|
||
const std::string cameraName = config.mvsCamera.serialNumber.empty()
|
||
? std::string("平面相机")
|
||
: config.mvsCamera.serialNumber;
|
||
m_cameraList.push_back(std::make_pair(cameraName, static_cast<void*>(m_mvsDevice)));
|
||
}
|
||
|
||
ret = InitRsLidar(config.lidarConfig);
|
||
if (ret != 0) {
|
||
NotifyStatus("雷达初始化失败，继续使用预留算法模式");
|
||
}
|
||
}
|
||
|
||
ret = InitLedDisplay(config.ledDisplayConfig);
|
||
if (ret != 0) {
|
||
NotifyStatus("显示屏初始化失败");
|
||
}
|
||
|
||
ret = InitUdpBroadcast(config.udpBroadcastConfig);
|
||
if (ret != 0) {
|
||
NotifyStatus("组播初始化失败");
|
||
}
|
||
|
||
ret = InitHttpServer(config.httpServerConfig);
|
||
if (ret != 0) {
|
||
NotifyStatus("HTTP状态服务初始化失败");
|
||
}
|
||
|
||
if (StatusCallback()) {
|
||
StatusCallback()->OnCameraCountChanged(1);
|
||
}
|
||
|
||
if (StatusCallback()) {
|
||
StatusCallback()->OnDetectionResult(waitingDisplay);
|
||
}
|
||
|
||
m_initialized = true;
|
||
NotifyWorkStatus(WorkStatus::Ready);
|
||
NotifyStatus("停机引导初始化完成");
|
||
return 0;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::DeinitApp()
|
||
{
|
||
m_initialized.store(false);
|
||
{
|
||
std::lock_guard<std::mutex> reconfigurationLock(m_reconfigurationMutex);
|
||
m_reconfiguring.store(true);
|
||
StopDetection();
|
||
CloseDevices();
|
||
|
||
delete m_detectPresenter;
|
||
m_detectPresenter = nullptr;
|
||
}
|
||
|
||
if (m_configManager) {
|
||
m_configManager->Shutdown();
|
||
delete m_configManager;
|
||
m_configManager = nullptr;
|
||
}
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::StartDetection(int cameraIndex)
|
||
{
|
||
std::lock_guard<std::mutex> threadLock(m_detectionThreadMutex);
|
||
if (!m_initialized || m_reconfiguring.load() || !m_detectPresenter || !m_configManager) {
|
||
NotifyStatus("系统未初始化");
|
||
return false;
|
||
}
|
||
if (m_simulationOptions.enabled &&
|
||
(m_simulationRadarFrames.size() < 2 ||
|
||
m_simulationCameraFrames.empty())) {
|
||
NotifyStatus("多帧仿真点云或图像数据未就绪");
|
||
return false;
|
||
}
|
||
if (!m_simulationOptions.enabled &&
|
||
(!m_lidarDevice || !m_lidarDevice->IsRunning())) {
|
||
NotifyStatus("雷达未连接，无法启动实时检测");
|
||
return false;
|
||
}
|
||
|
||
if (m_detectionRunning.load()) {
|
||
NotifyStatus(m_simulationOptions.enabled
|
||
? "停机引导仿真回放已在运行"
|
||
: "停机引导实时检测已在运行");
|
||
return true;
|
||
}
|
||
|
||
if (m_simulationThread.joinable()) {
|
||
m_simulationThread.join();
|
||
}
|
||
if (m_detectionThread.joinable()) {
|
||
m_detectionThread.join();
|
||
}
|
||
|
||
if (cameraIndex > 0) {
|
||
m_detectIndex = cameraIndex;
|
||
} else {
|
||
m_detectIndex = m_currentCameraIndex;
|
||
}
|
||
|
||
{
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
m_consumedCloudSequence = m_cloudSequence;
|
||
m_processedCloudSequence = m_cloudSequence;
|
||
m_lidarErrorPending = false;
|
||
m_lidarErrorMessage.clear();
|
||
}
|
||
m_trackingMode.store(false);
|
||
m_simulationRound.store(0);
|
||
m_lastProbeAcceptedMs.store(0);
|
||
m_stopDetectionRequested.store(false);
|
||
m_detectionRunning.store(true);
|
||
NotifyWorkStatus(WorkStatus::Working);
|
||
NotifyStatus(m_simulationOptions.enabled
|
||
? "停机引导多帧仿真回放已开始"
|
||
: "停机引导实时检测已开始");
|
||
try {
|
||
m_detectionThread = std::thread(&ParkingSpaceGuidePresenter::DetectionLoop, this);
|
||
if (m_simulationOptions.enabled) {
|
||
m_simulationThread = std::thread(
|
||
&ParkingSpaceGuidePresenter::SimulationPlaybackLoop, this);
|
||
}
|
||
} catch (...) {
|
||
m_stopDetectionRequested.store(true);
|
||
m_detectionCondition.notify_all();
|
||
if (m_simulationThread.joinable()) {
|
||
m_simulationThread.join();
|
||
}
|
||
if (m_detectionThread.joinable()) {
|
||
m_detectionThread.join();
|
||
}
|
||
m_detectionRunning.store(false);
|
||
NotifyWorkStatus(WorkStatus::Error);
|
||
NotifyStatus(m_simulationOptions.enabled
|
||
? "启动停机引导仿真线程失败"
|
||
: "启动停机引导实时检测线程失败");
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::TriggerDetection(int cameraIndex)
|
||
{
|
||
std::lock_guard<std::mutex> threadLock(m_detectionThreadMutex);
|
||
if (!m_initialized || m_reconfiguring.load() || !m_detectPresenter || !m_configManager) {
|
||
NotifyStatus("系统未初始化");
|
||
return false;
|
||
}
|
||
|
||
if (m_detectionRunning.load()) {
|
||
NotifyStatus("实时检测运行中，无法执行单次检测");
|
||
return false;
|
||
}
|
||
|
||
if (cameraIndex > 0) {
|
||
m_detectIndex = cameraIndex;
|
||
} else {
|
||
m_detectIndex = m_currentCameraIndex;
|
||
}
|
||
|
||
std::shared_ptr<const OwnedCloudFrame> cloudFrame;
|
||
{
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
cloudFrame = m_latestCloudFrame;
|
||
}
|
||
if (!cloudFrame) {
|
||
NotifyStatus("暂无可用雷达点云");
|
||
return false;
|
||
}
|
||
|
||
NotifyWorkStatus(WorkStatus::Detecting);
|
||
NotifyStatus("停机引导单次检测已开始");
|
||
bool success = false;
|
||
ParkingGuideAlgorithmState algorithmState;
|
||
try {
|
||
success = DetectOnce(cloudFrame->cloud,
|
||
cloudFrame->timestampMs,
|
||
algorithmState);
|
||
} catch (const std::exception& e) {
|
||
NotifyStatus(std::string("停机引导单次检测异常：") + e.what());
|
||
} catch (...) {
|
||
NotifyStatus("停机引导单次检测发生未知异常");
|
||
}
|
||
NotifyWorkStatus(success ? WorkStatus::Completed : WorkStatus::Error);
|
||
NotifyStatus(success ? "停机引导单次检测完成"
|
||
: "停机引导单次检测失败");
|
||
return success;
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::DetectOnce(const RsCloudData& cloud,
|
||
qint64 frameTimestampMs,
|
||
ParkingGuideAlgorithmState& algorithmState)
|
||
{
|
||
std::lock_guard<std::mutex> executionLock(m_detectionExecutionMutex);
|
||
if (!m_detectPresenter || !m_configManager) {
|
||
return false;
|
||
}
|
||
|
||
DetectionResult result;
|
||
ParkingGuideAlgorithmControl control;
|
||
result.cameraIndex = m_detectIndex;
|
||
const int ret = m_detectPresenter->DetectParkingSpaceGuide(cloud,
|
||
m_configManager->GetAlgorithmParams(),
|
||
frameTimestampMs,
|
||
algorithmState,
|
||
result,
|
||
control);
|
||
result.cameraIndex = m_detectIndex;
|
||
|
||
if (ret != 0) {
|
||
result.errorCode = ret;
|
||
if (result.message.isEmpty()) {
|
||
result.message = QStringLiteral("停机引导检测失败");
|
||
}
|
||
} else {
|
||
if (result.parkingSpaceInfoList.empty() && result.message.isEmpty()) {
|
||
result.message = QStringLiteral("无停机引导结果");
|
||
}
|
||
|
||
if (control.needModelRecognition) {
|
||
const auto appendRecognitionFailure = [this, &result, &control](const QString& reason) {
|
||
const QString text = reason.trimmed().isEmpty()
|
||
? QStringLiteral("机型识别失败")
|
||
: reason.trimmed();
|
||
NotifyStatus(text.toUtf8().toStdString());
|
||
result.message = result.message.trimmed().isEmpty()
|
||
? text
|
||
: result.message + QStringLiteral("；") + text;
|
||
const int maxAttempts = (std::max)(
|
||
1,
|
||
m_configManager->GetAlgorithmParams()
|
||
.modelRecognitionParam.maxRecognitionAttempts);
|
||
if (control.nextState.modelRecognitionAttempts >= maxAttempts) {
|
||
for (ParkingSpaceGuideInfo& info : result.parkingSpaceInfoList) {
|
||
info.hasException = true;
|
||
info.guideStateCode = static_cast<int>(
|
||
ParkingGuideState::AircraftVerificationFailed);
|
||
info.guideText = QStringLiteral("STOP+IDFAIL");
|
||
}
|
||
}
|
||
};
|
||
|
||
try {
|
||
QImage modelImage;
|
||
QString captureError;
|
||
if (!CaptureModelRecognitionImage(modelImage, captureError)) {
|
||
appendRecognitionFailure(captureError);
|
||
} else {
|
||
result.image = modelImage;
|
||
ModelRecognitionResult modelResult;
|
||
const int modelRet = m_detectPresenter->RecognizeModel2D(
|
||
modelImage,
|
||
control.recognitionContext,
|
||
modelResult);
|
||
const double minimumConfidence =
|
||
m_configManager->GetAlgorithmParams()
|
||
.modelRecognitionParam.confidenceThreshold;
|
||
|
||
if (modelRet == 0 && modelResult.IsVerified(minimumConfidence)) {
|
||
control.nextState.modelType = modelResult.modelType.trimmed();
|
||
control.nextState.modelConfidence = modelResult.confidence;
|
||
control.nextState.modelVerified = true;
|
||
for (ParkingSpaceGuideInfo& info : result.parkingSpaceInfoList) {
|
||
info.modelType = control.nextState.modelType;
|
||
info.confidence = control.nextState.modelConfidence;
|
||
}
|
||
} else {
|
||
QString reason = modelResult.message.trimmed();
|
||
if (reason.isEmpty() && modelRet == 0) {
|
||
reason = QStringLiteral(
|
||
"机型识别置信度不足：%1，实际%2，阈值%3")
|
||
.arg(modelResult.modelType)
|
||
.arg(modelResult.confidence, 0, 'f', 3)
|
||
.arg(minimumConfidence, 0, 'f', 3);
|
||
}
|
||
if (reason.isEmpty()) {
|
||
reason = QStringLiteral("机型识别结果未通过验证");
|
||
}
|
||
appendRecognitionFailure(reason);
|
||
}
|
||
}
|
||
} catch (const std::exception& e) {
|
||
appendRecognitionFailure(
|
||
QStringLiteral("机型识别异常：%1").arg(QString::fromUtf8(e.what())));
|
||
} catch (...) {
|
||
appendRecognitionFailure(QStringLiteral("机型识别发生未知异常"));
|
||
}
|
||
}
|
||
}
|
||
|
||
if (ret == 0) {
|
||
algorithmState = control.nextState;
|
||
}
|
||
PublishDetectionResult(result);
|
||
return ret == 0;
|
||
}
|
||
|
||
std::shared_ptr<ParkingSpaceGuidePresenter::OwnedCloudFrame>
|
||
ParkingSpaceGuidePresenter::AcquireCloudFrameBuffer()
|
||
{
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
for (size_t i = 0; i < m_cloudFrameSlots.size(); ++i) {
|
||
const size_t slotIndex = (m_nextCloudFrameSlot + i) % m_cloudFrameSlots.size();
|
||
std::shared_ptr<OwnedCloudFrame>& slot = m_cloudFrameSlots[slotIndex];
|
||
if (!slot) {
|
||
slot = std::make_shared<OwnedCloudFrame>();
|
||
}
|
||
if (slot.use_count() == 1) {
|
||
m_nextCloudFrameSlot = (slotIndex + 1) % m_cloudFrameSlots.size();
|
||
return slot;
|
||
}
|
||
}
|
||
return std::shared_ptr<OwnedCloudFrame>();
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::CopyCloudFrame(const RsCloudData& cloud,
|
||
const RsFrameInfo& info,
|
||
OwnedCloudFrame& destination)
|
||
{
|
||
size_t totalPointCount = 0;
|
||
for (const auto& item : cloud) {
|
||
const SVzLaserLineData& line = item.second;
|
||
if (item.first != keResultDataType_PointXYZI ||
|
||
line.nPointCount < 0 ||
|
||
(line.nPointCount > 0 && !line.p3DPoint)) {
|
||
return false;
|
||
}
|
||
totalPointCount += static_cast<size_t>(line.nPointCount);
|
||
}
|
||
|
||
destination.points.resize(totalPointCount);
|
||
destination.cloud.clear();
|
||
destination.cloud.reserve(cloud.size());
|
||
destination.info = info;
|
||
destination.timestampMs = MonotonicMilliseconds();
|
||
|
||
size_t pointOffset = 0;
|
||
for (const auto& item : cloud) {
|
||
SVzLaserLineData line = item.second;
|
||
line.p2DPoint = nullptr;
|
||
if (line.nPointCount > 0) {
|
||
SVzNLPointXYZI* target = destination.points.data() + pointOffset;
|
||
std::memcpy(target,
|
||
line.p3DPoint,
|
||
sizeof(SVzNLPointXYZI) * static_cast<size_t>(line.nPointCount));
|
||
line.p3DPoint = target;
|
||
pointOffset += static_cast<size_t>(line.nPointCount);
|
||
} else {
|
||
line.p3DPoint = nullptr;
|
||
}
|
||
destination.cloud.emplace_back(item.first, line);
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::SubmitCloudFrame(const RsCloudData& cloud,
|
||
const RsFrameInfo& info)
|
||
{
|
||
try {
|
||
std::shared_ptr<OwnedCloudFrame> frame = AcquireCloudFrameBuffer();
|
||
if (!frame) {
|
||
return false;
|
||
}
|
||
if (!CopyCloudFrame(cloud, info, *frame)) {
|
||
if (!m_cloudCopyErrorReported.exchange(true)) {
|
||
NotifyStatus("雷达点云格式非 PointXYZI，已丢弃当前帧");
|
||
}
|
||
return false;
|
||
}
|
||
|
||
m_cloudCopyErrorReported.store(false);
|
||
return SubmitOwnedCloudFrame(frame) != 0;
|
||
} catch (const std::exception& e) {
|
||
if (!m_cloudCopyErrorReported.exchange(true)) {
|
||
NotifyStatus(std::string("复制雷达点云失败：") + e.what());
|
||
}
|
||
} catch (...) {
|
||
if (!m_cloudCopyErrorReported.exchange(true)) {
|
||
NotifyStatus("复制雷达点云发生未知异常");
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
unsigned long long ParkingSpaceGuidePresenter::SubmitOwnedCloudFrame(
|
||
const std::shared_ptr<OwnedCloudFrame>& frame)
|
||
{
|
||
if (!frame ||
|
||
(m_simulationOptions.enabled && m_stopDetectionRequested.load())) {
|
||
return 0;
|
||
}
|
||
|
||
unsigned long long sequence = 0;
|
||
{
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
if (m_simulationOptions.enabled && m_stopDetectionRequested.load()) {
|
||
return 0;
|
||
}
|
||
m_latestCloudFrame = frame;
|
||
sequence = ++m_cloudSequence;
|
||
}
|
||
m_detectionCondition.notify_all();
|
||
return sequence;
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::WaitForSimulationFrameProcessed(
|
||
unsigned long long sequence)
|
||
{
|
||
std::unique_lock<std::mutex> dataLock(m_dataMutex);
|
||
m_detectionCondition.wait(dataLock, [this, sequence]() {
|
||
return m_stopDetectionRequested.load() ||
|
||
m_processedCloudSequence >= sequence;
|
||
});
|
||
return !m_stopDetectionRequested.load() &&
|
||
m_processedCloudSequence >= sequence;
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::WaitForSimulationInterval(int intervalMs)
|
||
{
|
||
if (intervalMs <= 0) {
|
||
return !m_stopDetectionRequested.load();
|
||
}
|
||
std::unique_lock<std::mutex> dataLock(m_dataMutex);
|
||
const bool stopped = m_detectionCondition.wait_for(
|
||
dataLock,
|
||
std::chrono::milliseconds(intervalMs),
|
||
[this]() { return m_stopDetectionRequested.load(); });
|
||
return !stopped;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::SimulationPlaybackLoop()
|
||
{
|
||
const auto failPlayback = [this](const QString& message) {
|
||
const QString text = message.trimmed().isEmpty()
|
||
? QStringLiteral("仿真回放失败")
|
||
: message.trimmed();
|
||
NotifyStatus(text.toUtf8().toStdString());
|
||
PublishErrorResult(-1, text);
|
||
m_stopDetectionRequested.store(true);
|
||
m_detectionRunning.store(false);
|
||
m_trackingMode.store(false);
|
||
NotifyWorkStatus(WorkStatus::Error);
|
||
m_detectionCondition.notify_all();
|
||
};
|
||
|
||
bool playbackCompleted = false;
|
||
size_t playbackRound = 0;
|
||
while (!m_stopDetectionRequested.load()) {
|
||
++playbackRound;
|
||
m_simulationRound.store(
|
||
static_cast<unsigned long long>(playbackRound));
|
||
if (playbackRound > 1) {
|
||
NotifyStatus(QStringLiteral("开始第 %1 轮仿真回放")
|
||
.arg(playbackRound)
|
||
.toUtf8()
|
||
.toStdString());
|
||
}
|
||
|
||
for (size_t frameIndex = 0;
|
||
frameIndex < m_simulationRadarFrames.size();
|
||
++frameIndex) {
|
||
if (m_stopDetectionRequested.load()) {
|
||
break;
|
||
}
|
||
|
||
const auto frameStartedAt = std::chrono::steady_clock::now();
|
||
const std::shared_ptr<OwnedCloudFrame>& radarFrame =
|
||
m_simulationRadarFrames[frameIndex];
|
||
radarFrame->timestampMs = static_cast<qint64>(frameIndex) *
|
||
static_cast<qint64>(m_simulationOptions.frameIntervalMs);
|
||
const size_t imageIndex =
|
||
frameIndex % m_simulationCameraFrames.size();
|
||
const QImage& cameraFrame = m_simulationCameraFrames[imageIndex];
|
||
CacheSimulationCameraFrame(frameIndex, cameraFrame);
|
||
if (m_stopDetectionRequested.load()) {
|
||
return;
|
||
}
|
||
|
||
const unsigned long long sequence =
|
||
SubmitOwnedCloudFrame(radarFrame);
|
||
if (sequence == 0) {
|
||
if (m_stopDetectionRequested.load()) {
|
||
return;
|
||
}
|
||
failPlayback(QStringLiteral("提交仿真雷达帧失败：%1")
|
||
.arg(QFileInfo(
|
||
m_simulationRadarFiles[frameIndex])
|
||
.fileName()));
|
||
return;
|
||
}
|
||
|
||
if (frameIndex == 0 ||
|
||
frameIndex + 1 == m_simulationRadarFrames.size() ||
|
||
(frameIndex + 1) % 10 == 0) {
|
||
NotifyStatus(QStringLiteral("仿真帧 %1/%2：%3 + %4")
|
||
.arg(frameIndex + 1)
|
||
.arg(m_simulationRadarFrames.size())
|
||
.arg(QFileInfo(
|
||
m_simulationRadarFiles[frameIndex])
|
||
.fileName())
|
||
.arg(QFileInfo(
|
||
m_simulationCameraFiles[imageIndex])
|
||
.fileName())
|
||
.toUtf8()
|
||
.toStdString());
|
||
}
|
||
if (!WaitForSimulationFrameProcessed(sequence)) {
|
||
return;
|
||
}
|
||
|
||
const bool hasNextFrame =
|
||
frameIndex + 1 < m_simulationRadarFrames.size();
|
||
if (hasNextFrame || m_simulationOptions.loop) {
|
||
const long long elapsedMs =
|
||
std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::steady_clock::now() - frameStartedAt)
|
||
.count();
|
||
const int remainingMs = elapsedMs >=
|
||
static_cast<long long>(m_simulationOptions.frameIntervalMs)
|
||
? 0
|
||
: m_simulationOptions.frameIntervalMs -
|
||
static_cast<int>(elapsedMs);
|
||
if (!WaitForSimulationInterval(remainingMs)) {
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (m_stopDetectionRequested.load()) {
|
||
break;
|
||
}
|
||
if (!m_simulationOptions.loop) {
|
||
playbackCompleted = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (playbackCompleted) {
|
||
m_stopDetectionRequested.store(true);
|
||
m_detectionRunning.store(false);
|
||
m_trackingMode.store(false);
|
||
m_detectionCondition.notify_all();
|
||
NotifyWorkStatus(WorkStatus::Completed);
|
||
NotifyStatus(QStringLiteral("停机引导仿真回放完成，共处理 %1 帧")
|
||
.arg(m_simulationRadarFrames.size())
|
||
.toUtf8()
|
||
.toStdString());
|
||
}
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::ProcessAirplanePresence(const RsCloudData& cloud)
|
||
{
|
||
AirplanePresenceResult presence;
|
||
const int ret = m_detectPresenter->DetectAirplanePresence(
|
||
cloud,
|
||
m_configManager->GetAlgorithmParams(),
|
||
presence);
|
||
if (ret != 0) {
|
||
const QString message = presence.message.trimmed().isEmpty()
|
||
? QStringLiteral("飞机存在检测失败")
|
||
: presence.message.trimmed();
|
||
PublishErrorResult(ret, message);
|
||
return false;
|
||
}
|
||
|
||
if (presence.state == AirplanePresenceState::Detected) {
|
||
NotifyStatus("检测到飞机，进入引导检测");
|
||
return true;
|
||
}
|
||
|
||
ParkingGuideState guideState = ParkingGuideState::Unknown;
|
||
QString guideText;
|
||
if (presence.state == AirplanePresenceState::GateBlocked) {
|
||
guideState = ParkingGuideState::GateBlocked;
|
||
guideText = QStringLiteral("GATEBLOCK");
|
||
} else if (presence.state == AirplanePresenceState::ViewBlocked) {
|
||
guideState = ParkingGuideState::ViewBlocked;
|
||
guideText = QStringLiteral("VIEWBLOCKED");
|
||
} else {
|
||
return false;
|
||
}
|
||
|
||
DetectionResult result;
|
||
ParkingSpaceGuideInfo info;
|
||
info.hasException = true;
|
||
info.guideStateCode = static_cast<int>(guideState);
|
||
info.guideText = guideText;
|
||
result.parkingSpaceInfoList.push_back(info);
|
||
result.message = presence.message.trimmed().isEmpty() ? guideText : presence.message.trimmed();
|
||
PublishDetectionResult(result);
|
||
return false;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::PublishDetectionResult(DetectionResult& result)
|
||
{
|
||
if (m_simulationOptions.enabled && result.image.isNull()) {
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
result.image = m_simulationCameraImage;
|
||
}
|
||
EnrichDetectionResult(result);
|
||
if (StatusCallback()) {
|
||
StatusCallback()->OnDetectionResult(result);
|
||
}
|
||
|
||
UpdateCurrentStatus(result);
|
||
if (m_ledDisplay) {
|
||
m_ledDisplay->SendResult(ConvertLedResult(result));
|
||
}
|
||
BroadcastDetectionResult(result);
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::PublishErrorResult(int errorCode, const QString& message)
|
||
{
|
||
DetectionResult result;
|
||
result.errorCode = errorCode == 0 ? -1 : errorCode;
|
||
result.message = message;
|
||
PublishDetectionResult(result);
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::CaptureModelRecognitionImage(QImage& image,
|
||
QString& errorMessage)
|
||
{
|
||
image = QImage();
|
||
errorMessage.clear();
|
||
if (m_simulationOptions.enabled) {
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
image = m_simulationCameraImage;
|
||
if (image.isNull()) {
|
||
errorMessage = QStringLiteral("仿真平面相机 BMP 图像不可用");
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
if (!m_mvsDevice || !m_mvsDevice->IsDeviceOpen() || !m_mvsDevice->IsAcquisitioning()) {
|
||
errorMessage = QStringLiteral("机型识别相机不可用，雷达引导继续");
|
||
return false;
|
||
}
|
||
|
||
int ret = m_mvsDevice->SendSoftTrigger();
|
||
if (ret != 0) {
|
||
errorMessage = QStringLiteral("机型识别相机软触发失败：%1").arg(ret);
|
||
return false;
|
||
}
|
||
|
||
MvsImageData captured;
|
||
ret = m_mvsDevice->CaptureImage(captured, kModelCaptureTimeoutMs);
|
||
std::unique_ptr<unsigned char[]> capturedData(captured.pData);
|
||
if (ret != 0 || !captured.pData) {
|
||
errorMessage = QStringLiteral("获取机型识别图像失败：%1").arg(ret);
|
||
return false;
|
||
}
|
||
|
||
if (!CacheMvsFrame(captured, image)) {
|
||
errorMessage = QStringLiteral("机型识别图像格式无效");
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::DetectionLoop()
|
||
{
|
||
ParkingGuideAlgorithmState algorithmState;
|
||
unsigned long long activeSimulationRound = 0;
|
||
while (!m_stopDetectionRequested.load()) {
|
||
std::shared_ptr<const OwnedCloudFrame> cloudFrame;
|
||
unsigned long long frameSequence = 0;
|
||
QString lidarError;
|
||
{
|
||
std::unique_lock<std::mutex> dataLock(m_dataMutex);
|
||
m_detectionCondition.wait(dataLock, [this]() {
|
||
return m_stopDetectionRequested.load() ||
|
||
m_lidarErrorPending ||
|
||
m_cloudSequence != m_consumedCloudSequence;
|
||
});
|
||
if (m_stopDetectionRequested.load()) {
|
||
break;
|
||
}
|
||
|
||
if (m_lidarErrorPending) {
|
||
lidarError = m_lidarErrorMessage;
|
||
m_lidarErrorPending = false;
|
||
} else {
|
||
cloudFrame = m_latestCloudFrame;
|
||
m_consumedCloudSequence = m_cloudSequence;
|
||
frameSequence = m_consumedCloudSequence;
|
||
}
|
||
}
|
||
|
||
if (!lidarError.isEmpty()) {
|
||
PublishErrorResult(-1, lidarError);
|
||
m_stopDetectionRequested.store(true);
|
||
m_detectionRunning.store(false);
|
||
m_trackingMode.store(false);
|
||
NotifyWorkStatus(WorkStatus::Error);
|
||
break;
|
||
}
|
||
if (!cloudFrame) {
|
||
continue;
|
||
}
|
||
|
||
if (m_simulationOptions.enabled) {
|
||
const unsigned long long currentRound = m_simulationRound.load();
|
||
if (currentRound != activeSimulationRound) {
|
||
algorithmState = ParkingGuideAlgorithmState();
|
||
m_trackingMode.store(false);
|
||
activeSimulationRound = currentRound;
|
||
}
|
||
}
|
||
|
||
try {
|
||
m_trackingMode.store(true);
|
||
DetectOnce(cloudFrame->cloud,
|
||
cloudFrame->timestampMs,
|
||
algorithmState);
|
||
} catch (const std::exception& e) {
|
||
PublishErrorResult(-1,
|
||
QStringLiteral("停机引导检测异常：%1")
|
||
.arg(QString::fromUtf8(e.what())));
|
||
} catch (...) {
|
||
PublishErrorResult(-1, QStringLiteral("停机引导检测发生未知异常"));
|
||
}
|
||
|
||
{
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
m_processedCloudSequence =
|
||
(std::max)(m_processedCloudSequence, frameSequence);
|
||
}
|
||
m_detectionCondition.notify_all();
|
||
}
|
||
|
||
m_detectionRunning.store(false);
|
||
m_trackingMode.store(false);
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::StopDetection()
|
||
{
|
||
std::lock_guard<std::mutex> threadLock(m_detectionThreadMutex);
|
||
const bool wasRunning = m_detectionRunning.load();
|
||
{
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
m_stopDetectionRequested.store(true);
|
||
}
|
||
m_detectionCondition.notify_all();
|
||
|
||
if (m_simulationThread.joinable() &&
|
||
m_simulationThread.get_id() != std::this_thread::get_id()) {
|
||
m_simulationThread.join();
|
||
}
|
||
if (m_detectionThread.joinable() &&
|
||
m_detectionThread.get_id() != std::this_thread::get_id()) {
|
||
m_detectionThread.join();
|
||
}
|
||
m_detectionRunning.store(false);
|
||
m_trackingMode.store(false);
|
||
m_lastProbeAcceptedMs.store(0);
|
||
|
||
if (wasRunning) {
|
||
NotifyWorkStatus(WorkStatus::Ready);
|
||
NotifyStatus(m_simulationOptions.enabled
|
||
? "停机引导仿真回放已停止"
|
||
: "停机引导实时检测已停止");
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::IsDetectionRunning() const
|
||
{
|
||
return m_detectionRunning.load();
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::LoadAndDetect(const QString& fileName)
|
||
{
|
||
Q_UNUSED(fileName);
|
||
return TriggerDetection(m_currentCameraIndex) ? 0 : -1;
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::SaveDetectionDataToFile(const std::string& filePath)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
|
||
std::ofstream file(filePath.c_str(), std::ios::out | std::ios::trunc);
|
||
if (!file.is_open()) {
|
||
return -1;
|
||
}
|
||
|
||
file << "ParkingSpaceGuide cached data\n";
|
||
file << "hasFrame=" << (m_hasFrame ? 1 : 0) << "\n";
|
||
file << "frameWidth=" << m_latestFrameInfo.width << "\n";
|
||
file << "frameHeight=" << m_latestFrameInfo.height << "\n";
|
||
file << "hasCloud=" << (m_latestCloudFrame ? 1 : 0) << "\n";
|
||
file << "cloudLines=" << (m_latestCloudFrame ? m_latestCloudFrame->cloud.size() : 0) << "\n";
|
||
return 0;
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::GetDetectionDataCacheSize() const
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
return (m_hasFrame ? 1 : 0) + (m_latestCloudFrame ? 1 : 0);
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::SetDefaultCameraIndex(int cameraIndex)
|
||
{
|
||
m_currentCameraIndex = (std::max)(1, cameraIndex);
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::GetDetectIndex() const
|
||
{
|
||
return m_detectIndex;
|
||
}
|
||
|
||
QString ParkingSpaceGuidePresenter::GetAlgoVersion() const
|
||
{
|
||
return DetectPresenter::GetAlgoVersion();
|
||
}
|
||
|
||
ConfigManager* ParkingSpaceGuidePresenter::GetConfigManager()
|
||
{
|
||
return m_configManager;
|
||
}
|
||
|
||
std::vector<std::pair<std::string, void*>> ParkingSpaceGuidePresenter::GetCameraList() const
|
||
{
|
||
return m_cameraList;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::OnConfigChanged(const ConfigResult& configResult)
|
||
{
|
||
std::lock_guard<std::mutex> reconfigurationLock(m_reconfigurationMutex);
|
||
if (!m_initialized) {
|
||
return;
|
||
}
|
||
|
||
m_reconfiguring.store(true);
|
||
const bool restartDetection = m_detectionRunning.load();
|
||
const int detectIndex = m_detectIndex;
|
||
StopDetection();
|
||
|
||
NotifyStatus(m_simulationOptions.enabled
|
||
? "配置已变更，正在重新初始化输出协议"
|
||
: "配置已变更，正在重新连接设备");
|
||
if (!m_simulationOptions.enabled) {
|
||
InitMvsCamera(configResult.mvsCamera);
|
||
InitRsLidar(configResult.lidarConfig);
|
||
}
|
||
InitLedDisplay(configResult.ledDisplayConfig);
|
||
InitUdpBroadcast(configResult.udpBroadcastConfig);
|
||
InitHttpServer(configResult.httpServerConfig);
|
||
|
||
m_reconfiguring.store(false);
|
||
if (restartDetection) {
|
||
StartDetection(detectIndex);
|
||
}
|
||
}
|
||
|
||
IYParkingSpaceGuideStatus* ParkingSpaceGuidePresenter::StatusCallback() const
|
||
{
|
||
return static_cast<IYParkingSpaceGuideStatus*>(m_statusCallback);
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::InitConfig()
|
||
{
|
||
if (!m_configManager) {
|
||
m_configManager = new ConfigManager();
|
||
}
|
||
|
||
if (!m_configManager->Initialize(PathManager::GetInstance().GetConfigFilePath().toStdString())) {
|
||
NotifyStatus("初始化配置管理器失败");
|
||
return -1;
|
||
}
|
||
|
||
ConfigResult config = m_configManager->GetConfigResult();
|
||
config.Normalize();
|
||
SystemConfig systemConfig = m_configManager->GetConfig();
|
||
systemConfig.configResult = config;
|
||
m_configManager->UpdateFullConfig(systemConfig);
|
||
return 0;
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::InitSimulationData()
|
||
{
|
||
ClearSimulationData();
|
||
|
||
const QFileInfo directoryInfo(m_simulationOptions.dataDirectory);
|
||
if (!directoryInfo.exists() || !directoryInfo.isDir()) {
|
||
NotifyStatus(QStringLiteral("仿真数据目录不存在或不是目录：%1")
|
||
.arg(directoryInfo.absoluteFilePath())
|
||
.toUtf8()
|
||
.toStdString());
|
||
return -1;
|
||
}
|
||
|
||
const QDir directory(directoryInfo.absoluteFilePath());
|
||
const QFileInfoList entries = directory.entryInfoList(
|
||
QDir::Files | QDir::Readable | QDir::NoDotAndDotDot,
|
||
QDir::Name);
|
||
for (const QFileInfo& entry : entries) {
|
||
const QString suffix = entry.suffix();
|
||
if (suffix.compare(QStringLiteral("txt"), Qt::CaseInsensitive) == 0) {
|
||
m_simulationRadarFiles.push_back(entry.absoluteFilePath());
|
||
} else if (suffix.compare(QStringLiteral("bmp"), Qt::CaseInsensitive) == 0) {
|
||
m_simulationCameraFiles.push_back(entry.absoluteFilePath());
|
||
}
|
||
}
|
||
|
||
SortPathsNaturally(m_simulationRadarFiles);
|
||
SortPathsNaturally(m_simulationCameraFiles);
|
||
if (m_simulationRadarFiles.size() < 2) {
|
||
NotifyStatus("仿真数据目录至少需要两个 TXT 雷达帧");
|
||
ClearSimulationData();
|
||
return -1;
|
||
}
|
||
if (m_simulationCameraFiles.empty()) {
|
||
NotifyStatus("仿真数据目录至少需要一个 BMP 平面相机帧");
|
||
ClearSimulationData();
|
||
return -1;
|
||
}
|
||
|
||
NotifyStatus(QStringLiteral("开始将仿真数据加载到内存：%1 个点云，%2 个图像")
|
||
.arg(m_simulationRadarFiles.size())
|
||
.arg(m_simulationCameraFiles.size())
|
||
.toUtf8()
|
||
.toStdString());
|
||
|
||
QString errorMessage;
|
||
try {
|
||
m_simulationRadarFrames.reserve(m_simulationRadarFiles.size());
|
||
for (size_t frameIndex = 0;
|
||
frameIndex < m_simulationRadarFiles.size();
|
||
++frameIndex) {
|
||
std::shared_ptr<OwnedCloudFrame> radarFrame;
|
||
if (!LoadSimulationRadarFrame(frameIndex,
|
||
radarFrame,
|
||
errorMessage)) {
|
||
NotifyStatus(QStringLiteral("加载仿真点云失败：%1")
|
||
.arg(errorMessage)
|
||
.toUtf8()
|
||
.toStdString());
|
||
ClearSimulationData();
|
||
return -1;
|
||
}
|
||
m_simulationRadarFrames.push_back(radarFrame);
|
||
|
||
if (frameIndex == 0 ||
|
||
frameIndex + 1 == m_simulationRadarFiles.size() ||
|
||
(frameIndex + 1) % 10 == 0) {
|
||
NotifyStatus(QStringLiteral("点云加载进度：%1/%2")
|
||
.arg(frameIndex + 1)
|
||
.arg(m_simulationRadarFiles.size())
|
||
.toUtf8()
|
||
.toStdString());
|
||
}
|
||
}
|
||
|
||
m_simulationCameraFrames.reserve(m_simulationCameraFiles.size());
|
||
for (size_t imageIndex = 0;
|
||
imageIndex < m_simulationCameraFiles.size();
|
||
++imageIndex) {
|
||
QImage cameraFrame;
|
||
if (!LoadSimulationCameraFile(imageIndex,
|
||
cameraFrame,
|
||
errorMessage)) {
|
||
NotifyStatus(QStringLiteral("加载仿真图像失败：%1")
|
||
.arg(errorMessage)
|
||
.toUtf8()
|
||
.toStdString());
|
||
ClearSimulationData();
|
||
return -1;
|
||
}
|
||
m_simulationCameraFrames.push_back(cameraFrame);
|
||
if (imageIndex == 0 ||
|
||
imageIndex + 1 == m_simulationCameraFiles.size() ||
|
||
(imageIndex + 1) % 10 == 0) {
|
||
NotifyStatus(QStringLiteral("图像加载进度：%1/%2")
|
||
.arg(imageIndex + 1)
|
||
.arg(m_simulationCameraFiles.size())
|
||
.toUtf8()
|
||
.toStdString());
|
||
}
|
||
}
|
||
} catch (const std::exception& e) {
|
||
NotifyStatus(QStringLiteral("仿真数据加载到内存失败：%1")
|
||
.arg(QString::fromUtf8(e.what()))
|
||
.toUtf8()
|
||
.toStdString());
|
||
ClearSimulationData();
|
||
return -1;
|
||
} catch (...) {
|
||
NotifyStatus("仿真数据加载到内存时发生未知异常");
|
||
ClearSimulationData();
|
||
return -1;
|
||
}
|
||
|
||
{
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
m_latestCloudFrame = m_simulationRadarFrames.front();
|
||
}
|
||
CacheSimulationCameraFrame(0, m_simulationCameraFrames.front());
|
||
|
||
m_cameraList.clear();
|
||
m_cameraList.push_back(std::make_pair(
|
||
std::string("仿真平面相机"), static_cast<void*>(nullptr)));
|
||
NotifyCameraStatus(true);
|
||
NotifyLidarStatus(true);
|
||
NotifyStatus(QStringLiteral("仿真数据已全部加载到内存：%1 个点云帧，%2 个图像帧，图像循环配对，周期 %3 ms%4")
|
||
.arg(m_simulationRadarFrames.size())
|
||
.arg(m_simulationCameraFrames.size())
|
||
.arg(m_simulationOptions.frameIntervalMs)
|
||
.arg(m_simulationOptions.loop
|
||
? QStringLiteral("，循环回放")
|
||
: QStringLiteral("，单次回放"))
|
||
.toUtf8()
|
||
.toStdString());
|
||
return 0;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::ClearSimulationData()
|
||
{
|
||
m_simulationRadarFiles.clear();
|
||
m_simulationCameraFiles.clear();
|
||
m_simulationRadarFrames.clear();
|
||
m_simulationCameraFrames.clear();
|
||
m_cameraList.clear();
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
m_simulationCameraImage = QImage();
|
||
m_latestFrame.clear();
|
||
m_latestFrameInfo = CameraFrameInfo();
|
||
m_hasFrame = false;
|
||
m_latestCloudFrame.reset();
|
||
for (std::shared_ptr<OwnedCloudFrame>& slot : m_cloudFrameSlots) {
|
||
slot.reset();
|
||
}
|
||
m_nextCloudFrameSlot = 0;
|
||
m_cloudSequence = 0;
|
||
m_consumedCloudSequence = 0;
|
||
m_processedCloudSequence = 0;
|
||
m_lidarErrorPending = false;
|
||
m_lidarErrorMessage.clear();
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::LoadSimulationRadarFrame(
|
||
size_t frameIndex,
|
||
std::shared_ptr<OwnedCloudFrame>& frame,
|
||
QString& errorMessage)
|
||
{
|
||
frame.reset();
|
||
errorMessage.clear();
|
||
if (frameIndex >= m_simulationRadarFiles.size()) {
|
||
errorMessage = QStringLiteral("雷达帧序号越界：%1").arg(frameIndex);
|
||
return false;
|
||
}
|
||
|
||
const QString filePath = m_simulationRadarFiles[frameIndex];
|
||
LaserDataLoader loader;
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> loadedData;
|
||
int lineCount = 0;
|
||
float scanSpeed = 0.0f;
|
||
int maxTimestamp = 0;
|
||
int clocksPerSecond = 0;
|
||
int ret = -1;
|
||
try {
|
||
ret = loader.LoadLaserScanData(QFile::encodeName(filePath).toStdString(),
|
||
loadedData,
|
||
lineCount,
|
||
scanSpeed,
|
||
maxTimestamp,
|
||
clocksPerSecond);
|
||
} catch (const std::exception& e) {
|
||
errorMessage = QStringLiteral("加载 %1 异常：%2")
|
||
.arg(QFileInfo(filePath).fileName(),
|
||
QString::fromUtf8(e.what()));
|
||
loader.FreeLaserScanData(loadedData);
|
||
return false;
|
||
} catch (...) {
|
||
errorMessage = QStringLiteral("加载 %1 时发生未知异常")
|
||
.arg(QFileInfo(filePath).fileName());
|
||
loader.FreeLaserScanData(loadedData);
|
||
return false;
|
||
}
|
||
if (ret != 0) {
|
||
errorMessage = QStringLiteral("%1：%2")
|
||
.arg(QFileInfo(filePath).fileName(),
|
||
QString::fromStdString(loader.GetLastError()));
|
||
loader.FreeLaserScanData(loadedData);
|
||
return false;
|
||
}
|
||
if (loadedData.empty()) {
|
||
errorMessage = QStringLiteral("%1 未包含扫描线")
|
||
.arg(QFileInfo(filePath).fileName());
|
||
loader.FreeLaserScanData(loadedData);
|
||
return false;
|
||
}
|
||
|
||
size_t totalPointCount = 0;
|
||
int maximumLinePointCount = 0;
|
||
for (const auto& item : loadedData) {
|
||
const SVzLaserLineData& line = item.second;
|
||
if (line.nPointCount < 0 ||
|
||
(line.nPointCount > 0 && !line.p3DPoint)) {
|
||
errorMessage = QStringLiteral("%1 包含无效扫描线")
|
||
.arg(QFileInfo(filePath).fileName());
|
||
loader.FreeLaserScanData(loadedData);
|
||
return false;
|
||
}
|
||
if (item.first != keResultDataType_Position &&
|
||
item.first != keResultDataType_PointXYZI &&
|
||
item.first != keResultDataType_PointXYZRGBA) {
|
||
errorMessage = QStringLiteral("%1 的点类型不支持：%2")
|
||
.arg(QFileInfo(filePath).fileName())
|
||
.arg(static_cast<int>(item.first));
|
||
loader.FreeLaserScanData(loadedData);
|
||
return false;
|
||
}
|
||
|
||
const size_t pointCount = static_cast<size_t>(line.nPointCount);
|
||
if (pointCount > (std::numeric_limits<size_t>::max)() - totalPointCount) {
|
||
errorMessage = QStringLiteral("%1 的点数量溢出")
|
||
.arg(QFileInfo(filePath).fileName());
|
||
loader.FreeLaserScanData(loadedData);
|
||
return false;
|
||
}
|
||
totalPointCount += pointCount;
|
||
maximumLinePointCount = (std::max)(maximumLinePointCount,
|
||
line.nPointCount);
|
||
}
|
||
if (totalPointCount == 0) {
|
||
errorMessage = QStringLiteral("%1 不包含有效点")
|
||
.arg(QFileInfo(filePath).fileName());
|
||
loader.FreeLaserScanData(loadedData);
|
||
return false;
|
||
}
|
||
|
||
try {
|
||
std::shared_ptr<OwnedCloudFrame> converted =
|
||
std::make_shared<OwnedCloudFrame>();
|
||
converted->points.resize(totalPointCount);
|
||
std::memset(converted->points.data(),
|
||
0,
|
||
sizeof(SVzNLPointXYZI) * totalPointCount);
|
||
converted->cloud.reserve(loadedData.size());
|
||
|
||
size_t pointOffset = 0;
|
||
for (const auto& item : loadedData) {
|
||
const SVzLaserLineData& sourceLine = item.second;
|
||
SVzLaserLineData targetLine = sourceLine;
|
||
targetLine.p2DPoint = nullptr;
|
||
targetLine.p3DPoint = sourceLine.nPointCount > 0
|
||
? converted->points.data() + pointOffset
|
||
: nullptr;
|
||
|
||
for (int pointIndex = 0;
|
||
pointIndex < sourceLine.nPointCount;
|
||
++pointIndex) {
|
||
SVzNLPointXYZI& targetPoint =
|
||
converted->points[pointOffset + static_cast<size_t>(pointIndex)];
|
||
if (item.first == keResultDataType_Position) {
|
||
const auto* sourcePoints =
|
||
static_cast<const SVzNL3DPosition*>(sourceLine.p3DPoint);
|
||
targetPoint.x = static_cast<float>(sourcePoints[pointIndex].pt3D.x);
|
||
targetPoint.y = static_cast<float>(sourcePoints[pointIndex].pt3D.y);
|
||
targetPoint.z = static_cast<float>(sourcePoints[pointIndex].pt3D.z);
|
||
targetPoint.intensity = 0.0f;
|
||
} else if (item.first == keResultDataType_PointXYZRGBA) {
|
||
const auto* sourcePoints =
|
||
static_cast<const SVzNLPointXYZRGBA*>(sourceLine.p3DPoint);
|
||
targetPoint.x = sourcePoints[pointIndex].x;
|
||
targetPoint.y = sourcePoints[pointIndex].y;
|
||
targetPoint.z = sourcePoints[pointIndex].z;
|
||
targetPoint.intensity = 0.0f;
|
||
} else {
|
||
const auto* sourcePoints =
|
||
static_cast<const SVzNLPointXYZI*>(sourceLine.p3DPoint);
|
||
targetPoint = sourcePoints[pointIndex];
|
||
}
|
||
}
|
||
|
||
converted->cloud.emplace_back(keResultDataType_PointXYZI,
|
||
targetLine);
|
||
pointOffset += static_cast<size_t>(sourceLine.nPointCount);
|
||
}
|
||
|
||
converted->info.height = static_cast<uint32_t>(loadedData.size());
|
||
converted->info.width = static_cast<uint32_t>(maximumLinePointCount);
|
||
converted->info.isDense = false;
|
||
frame = converted;
|
||
} catch (const std::exception& e) {
|
||
errorMessage = QStringLiteral("转换 %1 失败：%2")
|
||
.arg(QFileInfo(filePath).fileName(),
|
||
QString::fromUtf8(e.what()));
|
||
loader.FreeLaserScanData(loadedData);
|
||
return false;
|
||
} catch (...) {
|
||
errorMessage = QStringLiteral("转换 %1 时发生未知异常")
|
||
.arg(QFileInfo(filePath).fileName());
|
||
loader.FreeLaserScanData(loadedData);
|
||
return false;
|
||
}
|
||
|
||
loader.FreeLaserScanData(loadedData);
|
||
return true;
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::LoadSimulationCameraFile(
|
||
size_t imageIndex,
|
||
QImage& image,
|
||
QString& errorMessage)
|
||
{
|
||
image = QImage();
|
||
errorMessage.clear();
|
||
if (imageIndex >= m_simulationCameraFiles.size()) {
|
||
errorMessage = QStringLiteral("图像帧序号越界：%1").arg(imageIndex);
|
||
return false;
|
||
}
|
||
|
||
const QString filePath = m_simulationCameraFiles[imageIndex];
|
||
QImageReader reader(filePath, "bmp");
|
||
reader.setAutoTransform(true);
|
||
image = reader.read();
|
||
if (image.isNull()) {
|
||
errorMessage = QStringLiteral("%1：%2")
|
||
.arg(QFileInfo(filePath).fileName(),
|
||
reader.errorString());
|
||
return false;
|
||
}
|
||
if (image.format() != QImage::Format_RGB888) {
|
||
image = image.convertToFormat(QImage::Format_RGB888);
|
||
}
|
||
if (image.isNull()) {
|
||
errorMessage = QStringLiteral("%1 转换为 RGB888 失败")
|
||
.arg(QFileInfo(filePath).fileName());
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::CacheSimulationCameraFrame(
|
||
size_t frameIndex,
|
||
const QImage& image)
|
||
{
|
||
const QImage rgbImage = image.format() == QImage::Format_RGB888
|
||
? image
|
||
: image.convertToFormat(QImage::Format_RGB888);
|
||
|
||
CameraFrameInfo frameInfo;
|
||
frameInfo.width = rgbImage.width();
|
||
frameInfo.height = rgbImage.height();
|
||
frameInfo.pixelFormat = kMvsPixelTypeRGB8;
|
||
frameInfo.frameId = static_cast<unsigned long long>(frameIndex + 1);
|
||
frameInfo.timestamp = static_cast<unsigned long long>(
|
||
QDateTime::currentMSecsSinceEpoch());
|
||
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
m_simulationCameraImage = rgbImage;
|
||
m_latestFrame.clear();
|
||
m_latestFrameInfo = frameInfo;
|
||
m_hasFrame = !rgbImage.isNull();
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::InitMvsCamera(const MvsCameraConfig& config)
|
||
{
|
||
auto closeMvsDevice = [this]() {
|
||
if (!m_mvsDevice) {
|
||
return;
|
||
}
|
||
|
||
m_mvsDevice->StopAcquisition();
|
||
m_mvsDevice->UnregisterImageCallback();
|
||
m_mvsDevice->CloseDevice();
|
||
m_mvsDevice->UninitSDK();
|
||
delete m_mvsDevice;
|
||
m_mvsDevice = nullptr;
|
||
};
|
||
|
||
closeMvsDevice();
|
||
m_cameraList.clear();
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
m_latestFrame.clear();
|
||
m_latestFrameInfo = CameraFrameInfo();
|
||
m_hasFrame = false;
|
||
}
|
||
|
||
if (!config.enabled) {
|
||
NotifyCameraStatus(false);
|
||
NotifyStatus("平面相机未启用");
|
||
m_cameraList.push_back(std::make_pair(std::string("平面相机"), static_cast<void*>(nullptr)));
|
||
return 0;
|
||
}
|
||
|
||
if (IMvsDevice::CreateObject(&m_mvsDevice) != 0 || !m_mvsDevice) {
|
||
NotifyCameraStatus(false);
|
||
NotifyStatus("创建平面相机设备失败");
|
||
return -1;
|
||
}
|
||
|
||
int ret = m_mvsDevice->InitSDK();
|
||
if (ret != 0) {
|
||
NotifyCameraStatus(false);
|
||
NotifyStatus("平面相机SDK初始化失败：" + std::to_string(ret));
|
||
closeMvsDevice();
|
||
return ret;
|
||
}
|
||
|
||
std::vector<MvsDeviceInfo> devices;
|
||
ret = m_mvsDevice->EnumerateDevices(devices);
|
||
if (ret != 0 || devices.empty()) {
|
||
NotifyCameraStatus(false);
|
||
NotifyStatus(ret != 0
|
||
? "搜索平面相机失败：" + std::to_string(ret)
|
||
: "未搜索到平面相机");
|
||
closeMvsDevice();
|
||
return ret != 0 ? ret : -1;
|
||
}
|
||
|
||
NotifyStatus("搜索到平面相机数量：" + std::to_string(devices.size()));
|
||
|
||
if (!config.serialNumber.empty()) {
|
||
ret = m_mvsDevice->OpenDevice(config.serialNumber);
|
||
} else {
|
||
unsigned int deviceIndex = static_cast<unsigned int>((std::max)(0, config.deviceIndex));
|
||
if (deviceIndex >= devices.size()) {
|
||
NotifyStatus("相机设备序号超出范围，使用第0台设备");
|
||
deviceIndex = 0;
|
||
}
|
||
ret = m_mvsDevice->OpenDeviceByIndex(deviceIndex);
|
||
}
|
||
|
||
if (ret != 0) {
|
||
NotifyCameraStatus(false);
|
||
NotifyStatus("打开平面相机失败：" + std::to_string(ret));
|
||
closeMvsDevice();
|
||
return ret;
|
||
}
|
||
|
||
MvsDeviceInfo openedInfo;
|
||
std::string cameraName = config.serialNumber.empty() ? std::string("平面相机") : config.serialNumber;
|
||
if (m_mvsDevice->GetDeviceInfo(openedInfo) == 0) {
|
||
if (!openedInfo.displayName.empty()) {
|
||
cameraName = openedInfo.displayName;
|
||
} else if (!openedInfo.serialNumber.empty()) {
|
||
cameraName = openedInfo.serialNumber;
|
||
}
|
||
}
|
||
ret = m_mvsDevice->SetEnumFeature("AcquisitionMode", kMvsAcqModeContinuous);
|
||
if (ret != 0) {
|
||
NotifyStatus("设置平面相机连续采集模式失败，继续使用当前模式：" + std::to_string(ret));
|
||
}
|
||
|
||
ret = m_mvsDevice->SetEnumFeature("PixelFormat", kMvsPixelTypeMono8);
|
||
if (ret != 0) {
|
||
NotifyStatus("设置平面相机灰度格式失败，继续使用当前格式：" + std::to_string(ret));
|
||
}
|
||
|
||
ret = m_mvsDevice->SetTriggerMode(true);
|
||
if (ret != 0) {
|
||
NotifyCameraStatus(false);
|
||
NotifyStatus("启用机型识别相机触发模式失败：" + std::to_string(ret));
|
||
closeMvsDevice();
|
||
return ret;
|
||
}
|
||
|
||
ret = m_mvsDevice->SetEnumFeatureByString("TriggerSource", "Software");
|
||
if (ret != 0) {
|
||
NotifyCameraStatus(false);
|
||
NotifyStatus("设置机型识别相机软触发源失败：" + std::to_string(ret));
|
||
closeMvsDevice();
|
||
return ret;
|
||
}
|
||
|
||
ret = m_mvsDevice->StartAcquisition();
|
||
if (ret != 0) {
|
||
NotifyCameraStatus(false);
|
||
NotifyStatus("启动平面相机采集失败：" + std::to_string(ret));
|
||
closeMvsDevice();
|
||
return ret;
|
||
}
|
||
|
||
NotifyCameraStatus(true);
|
||
m_cameraList.push_back(std::make_pair(cameraName, static_cast<void*>(m_mvsDevice)));
|
||
NotifyStatus("平面相机已连接：" + cameraName);
|
||
return 0;
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::InitRsLidar(const RsLidarConfig& config)
|
||
{
|
||
if (m_lidarDevice) {
|
||
m_lidarDevice->Stop();
|
||
m_lidarDevice->CloseDevice();
|
||
delete m_lidarDevice;
|
||
m_lidarDevice = nullptr;
|
||
}
|
||
|
||
{
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
m_latestCloudFrame.reset();
|
||
for (std::shared_ptr<OwnedCloudFrame>& slot : m_cloudFrameSlots) {
|
||
slot.reset();
|
||
}
|
||
m_nextCloudFrameSlot = 0;
|
||
m_cloudSequence = 0;
|
||
m_consumedCloudSequence = 0;
|
||
m_processedCloudSequence = 0;
|
||
m_lidarErrorPending = false;
|
||
m_lidarErrorMessage.clear();
|
||
}
|
||
m_trackingMode.store(false);
|
||
m_lastProbeAcceptedMs.store(0);
|
||
m_cloudCopyErrorReported.store(false);
|
||
|
||
if (IRsLidarDevice::CreateObject(&m_lidarDevice) != 0 || !m_lidarDevice) {
|
||
NotifyLidarStatus(false);
|
||
return -1;
|
||
}
|
||
|
||
m_lidarDevice->SetPointCloudCallback([this](const RsCloudData& cloud, const RsFrameInfo& info) {
|
||
if (!m_trackingMode.load()) {
|
||
const long long nowMs = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::steady_clock::now().time_since_epoch()).count();
|
||
const long long lastMs = m_lastProbeAcceptedMs.load();
|
||
if (lastMs > 0 && nowMs - lastMs < kAirplaneProbeIntervalMs) {
|
||
return;
|
||
}
|
||
m_lastProbeAcceptedMs.store(nowMs);
|
||
}
|
||
|
||
SubmitCloudFrame(cloud, info);
|
||
});
|
||
|
||
m_lidarDevice->SetExceptionCallback([this](const RsExceptionInfo& info) {
|
||
const QString message = info.message.empty()
|
||
? QStringLiteral("雷达异常")
|
||
: QStringLiteral("雷达异常：%1").arg(QString::fromStdString(info.message));
|
||
const bool fatal = info.code == 0x40 || info.code >= 0x80;
|
||
if (fatal && m_detectionRunning.load()) {
|
||
std::lock_guard<std::mutex> dataLock(m_dataMutex);
|
||
m_lidarErrorPending = true;
|
||
m_lidarErrorMessage = message;
|
||
}
|
||
if (fatal) {
|
||
m_detectionCondition.notify_all();
|
||
}
|
||
NotifyStatus(message.toUtf8().toStdString());
|
||
if (fatal) {
|
||
NotifyLidarStatus(false);
|
||
}
|
||
});
|
||
|
||
int ret = m_lidarDevice->InitDevice();
|
||
if (ret != 0) {
|
||
NotifyLidarStatus(false);
|
||
return ret;
|
||
}
|
||
|
||
ret = m_lidarDevice->OpenDevice(config);
|
||
if (ret != 0) {
|
||
NotifyLidarStatus(false);
|
||
return ret;
|
||
}
|
||
|
||
ret = m_lidarDevice->Start();
|
||
NotifyLidarStatus(ret == 0);
|
||
return ret;
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::InitLedDisplay(const LedDisplayConfig& config)
|
||
{
|
||
if (m_ledDisplay) {
|
||
m_ledDisplay->Close();
|
||
delete m_ledDisplay;
|
||
m_ledDisplay = nullptr;
|
||
}
|
||
|
||
if (ILedDisplayDevice::CreateObject(&m_ledDisplay) != 0 || !m_ledDisplay) {
|
||
NotifyLedStatus(false);
|
||
return -1;
|
||
}
|
||
|
||
m_ledDisplay->SetStatusCallback([](ELedDisplayStatus status,
|
||
const std::string& message,
|
||
void* user) {
|
||
auto* self = static_cast<ParkingSpaceGuidePresenter*>(user);
|
||
if (!self) {
|
||
return;
|
||
}
|
||
self->NotifyLedStatus(status == ELedDisplayStatus::Connected);
|
||
if (!message.empty()) {
|
||
self->NotifyStatus(std::string("显示屏：") + message);
|
||
}
|
||
}, this);
|
||
|
||
const int ret = m_ledDisplay->Open(config);
|
||
NotifyLedStatus(config.enabled && ret == 0 && m_ledDisplay->IsConnected());
|
||
return ret;
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::InitUdpBroadcast(const UdpBroadcastConfig& config)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_udpMutex);
|
||
|
||
if (m_udpClient) {
|
||
m_udpClient->StopUDPClient();
|
||
delete m_udpClient;
|
||
m_udpClient = nullptr;
|
||
}
|
||
|
||
m_udpBroadcastConfig = config;
|
||
if (!config.enabled) {
|
||
NotifyStatus("组播未启用");
|
||
return 0;
|
||
}
|
||
|
||
if (config.address.empty() || config.port <= 0 || config.port > 65535) {
|
||
NotifyStatus("组播配置无效");
|
||
return -1;
|
||
}
|
||
|
||
if (!IYUDPClient::CreateUDPClient(&m_udpClient) || !m_udpClient) {
|
||
NotifyStatus("创建组播客户端失败");
|
||
return -1;
|
||
}
|
||
|
||
const int ret = m_udpClient->Init(false);
|
||
if (ret != 0) {
|
||
NotifyStatus("初始化组播客户端失败：" + std::to_string(ret));
|
||
m_udpClient->StopUDPClient();
|
||
delete m_udpClient;
|
||
m_udpClient = nullptr;
|
||
return ret;
|
||
}
|
||
|
||
NotifyStatus("组播客户端已初始化");
|
||
return 0;
|
||
}
|
||
|
||
int ParkingSpaceGuidePresenter::InitHttpServer(const HttpServerConfig& config)
|
||
{
|
||
CloseHttpServer();
|
||
|
||
m_httpServer = new ParkingStatusHttpServer();
|
||
const int ret = m_httpServer->Start(
|
||
config,
|
||
[this]() {
|
||
return CurrentStatusJson();
|
||
});
|
||
if (ret != 0) {
|
||
const std::string error = m_httpServer->LastError();
|
||
delete m_httpServer;
|
||
m_httpServer = nullptr;
|
||
NotifyStatus(error.empty() ? "HTTP状态服务启动失败" : "HTTP状态服务启动失败：" + error);
|
||
return ret;
|
||
}
|
||
|
||
if (config.enabled) {
|
||
NotifyStatus("HTTP状态服务已启动：" + config.address + ":" + std::to_string(config.port));
|
||
} else {
|
||
NotifyStatus("HTTP状态服务未启用");
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::CloseUdpBroadcast()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_udpMutex);
|
||
if (!m_udpClient) {
|
||
return;
|
||
}
|
||
|
||
m_udpClient->StopUDPClient();
|
||
delete m_udpClient;
|
||
m_udpClient = nullptr;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::CloseHttpServer()
|
||
{
|
||
if (!m_httpServer) {
|
||
return;
|
||
}
|
||
|
||
m_httpServer->Stop();
|
||
delete m_httpServer;
|
||
m_httpServer = nullptr;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::CloseDevices()
|
||
{
|
||
CloseHttpServer();
|
||
CloseUdpBroadcast();
|
||
|
||
if (m_mvsDevice) {
|
||
m_mvsDevice->StopAcquisition();
|
||
m_mvsDevice->UnregisterImageCallback();
|
||
m_mvsDevice->CloseDevice();
|
||
m_mvsDevice->UninitSDK();
|
||
delete m_mvsDevice;
|
||
m_mvsDevice = nullptr;
|
||
}
|
||
|
||
if (m_lidarDevice) {
|
||
m_lidarDevice->Stop();
|
||
m_lidarDevice->CloseDevice();
|
||
delete m_lidarDevice;
|
||
m_lidarDevice = nullptr;
|
||
}
|
||
|
||
if (m_ledDisplay) {
|
||
m_ledDisplay->Close();
|
||
delete m_ledDisplay;
|
||
m_ledDisplay = nullptr;
|
||
}
|
||
|
||
if (m_simulationOptions.enabled) {
|
||
ClearSimulationData();
|
||
}
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::EnrichDetectionResult(DetectionResult& result) const
|
||
{
|
||
const UdpBroadcastConfig config = m_configManager
|
||
? m_configManager->GetConfigResult().udpBroadcastConfig
|
||
: m_udpBroadcastConfig;
|
||
|
||
for (ParkingSpaceGuideInfo& info : result.parkingSpaceInfoList) {
|
||
if (info.targetId.trimmed().isEmpty()) {
|
||
info.targetId = ConfigText(config.targetId, QStringLiteral("T001"));
|
||
}
|
||
if (info.parkId.trimmed().isEmpty()) {
|
||
info.parkId = ConfigText(config.parkId, QStringLiteral("P01"));
|
||
}
|
||
if (info.modelType.trimmed().isEmpty()) {
|
||
info.modelType = QStringLiteral("未知");
|
||
}
|
||
}
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::UpdateCurrentStatus(const DetectionResult& result)
|
||
{
|
||
const UdpBroadcastConfig config = m_configManager
|
||
? m_configManager->GetConfigResult().udpBroadcastConfig
|
||
: m_udpBroadcastConfig;
|
||
const QByteArray data = QJsonDocument(BuildDetectionResultPayload(result, config))
|
||
.toJson(QJsonDocument::Compact);
|
||
|
||
std::lock_guard<std::mutex> lock(m_statusMutex);
|
||
m_currentStatusJson.assign(data.constData(), static_cast<size_t>(data.size()));
|
||
}
|
||
|
||
std::string ParkingSpaceGuidePresenter::CurrentStatusJson() const
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_statusMutex);
|
||
if (!m_currentStatusJson.empty()) {
|
||
return m_currentStatusJson;
|
||
}
|
||
return "{\"success\":false,\"hasResult\":false,\"message\":\"暂无检测结果\",\"errorCode\":-1}";
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::BroadcastDetectionResult(const DetectionResult& result)
|
||
{
|
||
Q_UNUSED(result);
|
||
|
||
std::lock_guard<std::mutex> lock(m_udpMutex);
|
||
if (!m_udpClient || !m_udpBroadcastConfig.enabled) {
|
||
return false;
|
||
}
|
||
|
||
const std::string currentStatus = CurrentStatusJson();
|
||
const QByteArray data(currentStatus.data(), static_cast<int>(currentStatus.size()));
|
||
const int ret = m_udpClient->SendData(m_udpBroadcastConfig.port,
|
||
m_udpBroadcastConfig.address.c_str(),
|
||
data.constData(),
|
||
data.size());
|
||
if (ret != 0) {
|
||
NotifyStatus("组播发送失败：" + std::to_string(ret));
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::NotifyStatus(const std::string& message)
|
||
{
|
||
LOG_INFO("%s\n", message.c_str());
|
||
if (StatusCallback()) {
|
||
StatusCallback()->OnStatusUpdate(message);
|
||
}
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::NotifyWorkStatus(WorkStatus status)
|
||
{
|
||
if (StatusCallback()) {
|
||
StatusCallback()->OnWorkStatusChanged(status);
|
||
}
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::NotifyCameraStatus(bool connected)
|
||
{
|
||
if (StatusCallback()) {
|
||
StatusCallback()->OnCamera1StatusChanged(connected);
|
||
}
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::NotifyLidarStatus(bool connected)
|
||
{
|
||
if (StatusCallback()) {
|
||
StatusCallback()->OnCamera2StatusChanged(connected);
|
||
}
|
||
}
|
||
|
||
void ParkingSpaceGuidePresenter::NotifyLedStatus(bool connected)
|
||
{
|
||
if (StatusCallback()) {
|
||
StatusCallback()->OnSerialConnectionChanged(connected);
|
||
}
|
||
}
|
||
|
||
LedDisplayResult ParkingSpaceGuidePresenter::ConvertLedResult(const DetectionResult& result) const
|
||
{
|
||
LedDisplayResult output;
|
||
if (result.errorCode != 0) {
|
||
output.valid = true;
|
||
output.guideCode = 2;
|
||
return output;
|
||
}
|
||
|
||
output.valid = !result.parkingSpaceInfoList.empty();
|
||
if (!output.valid) {
|
||
return output;
|
||
}
|
||
|
||
const ParkingSpaceGuideInfo& info = result.parkingSpaceInfoList.front();
|
||
output.guideCode = (info.hasException || IsExceptionGuideState(info.guideStateCode)) ? 2 : 1;
|
||
output.distance = static_cast<float>(info.distance);
|
||
output.lateralOffset = static_cast<float>(info.lateralOffset);
|
||
output.angle = static_cast<float>(info.angle);
|
||
return output;
|
||
}
|
||
|
||
bool ParkingSpaceGuidePresenter::CacheMvsFrame(const MvsImageData& image, QImage& frame)
|
||
{
|
||
frame = QImage();
|
||
if (!image.pData || image.width == 0 || image.height == 0 || image.dataSize == 0) {
|
||
return false;
|
||
}
|
||
|
||
const int width = static_cast<int>(image.width);
|
||
const int height = static_cast<int>(image.height);
|
||
const size_t pixelCount = static_cast<size_t>(width) * static_cast<size_t>(height);
|
||
if (pixelCount == 0) {
|
||
return false;
|
||
}
|
||
|
||
QImage rgbImage;
|
||
if (image.pixelFormat == kMvsPixelTypeRGB8 && image.dataSize >= pixelCount * 3) {
|
||
QImage src(image.pData, width, height, width * 3, QImage::Format_RGB888);
|
||
rgbImage = src.copy();
|
||
} else if (image.pixelFormat == kMvsPixelTypeBGR8 && image.dataSize >= pixelCount * 3) {
|
||
QImage src(image.pData, width, height, width * 3, QImage::Format_RGB888);
|
||
rgbImage = src.rgbSwapped();
|
||
} else if (image.pixelFormat == kMvsPixelTypeMono8 && image.dataSize >= pixelCount) {
|
||
QImage src(image.pData, width, height, width, QImage::Format_Grayscale8);
|
||
rgbImage = src.convertToFormat(QImage::Format_RGB888);
|
||
} else {
|
||
return false;
|
||
}
|
||
|
||
if (rgbImage.isNull()) {
|
||
return false;
|
||
}
|
||
|
||
std::vector<unsigned char> compact(pixelCount * 3);
|
||
for (int row = 0; row < height; ++row) {
|
||
std::memcpy(compact.data() + static_cast<size_t>(row) * static_cast<size_t>(width) * 3,
|
||
rgbImage.constScanLine(row),
|
||
static_cast<size_t>(width) * 3);
|
||
}
|
||
|
||
CameraFrameInfo frameInfo;
|
||
frameInfo.width = width;
|
||
frameInfo.height = height;
|
||
frameInfo.pixelFormat = image.pixelFormat;
|
||
frameInfo.frameId = image.frameID;
|
||
frameInfo.timestamp = image.timestamp;
|
||
|
||
std::lock_guard<std::mutex> lock(m_dataMutex);
|
||
m_latestFrame.swap(compact);
|
||
m_latestFrameInfo = frameInfo;
|
||
m_hasFrame = true;
|
||
frame = rgbImage;
|
||
return true;
|
||
}
|