45 lines
1.3 KiB
C++
45 lines
1.3 KiB
C++
#include "System.h"
|
|
#include "collector/DataCollector.h"
|
|
#include "detector/CollisionDetector.h"
|
|
#include "command/CommandSender.h"
|
|
#include <chrono>
|
|
#include <thread>
|
|
|
|
CollisionAvoidanceSystem::CollisionAvoidanceSystem()
|
|
: dataCollector_(std::make_unique<DataCollector>())
|
|
, collisionDetector_(std::make_unique<CollisionDetector>())
|
|
, commandSender_(std::make_unique<CommandSender>())
|
|
, running_(false) {
|
|
}
|
|
|
|
void CollisionAvoidanceSystem::start() {
|
|
running_ = true;
|
|
processLoop();
|
|
}
|
|
|
|
void CollisionAvoidanceSystem::stop() {
|
|
running_ = false;
|
|
}
|
|
|
|
void CollisionAvoidanceSystem::processLoop() {
|
|
while (running_) {
|
|
// 获取数据
|
|
if (auto data = dataCollector_->getData()) {
|
|
// 检测碰撞
|
|
auto result = collisionDetector_->detect(*data, {});
|
|
|
|
// 如果检测到碰撞风险，发送避障指令
|
|
if (result.hasCollision) {
|
|
VehicleCommand cmd{
|
|
.vehicleId = data->id,
|
|
.type = CommandType::STOP,
|
|
.avoidanceVector = result.avoidanceVector
|
|
};
|
|
commandSender_->sendCommand(cmd, CommandSender::Priority::HIGH);
|
|
}
|
|
}
|
|
|
|
// 控制处理频率
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
}
|
|
}
|