856 lines
32 KiB
C++
856 lines
32 KiB
C++
#include <csignal>
|
||
#include <typeinfo>
|
||
#include <chrono>
|
||
#include <cmath>
|
||
#include "System.h"
|
||
#include "utils/Logger.h"
|
||
#include "collector/DataCollector.h"
|
||
#include "spatial/CoordinateConverter.h"
|
||
#include "network/TrafficLightHttpServer.h"
|
||
#include "config/SystemConfig.h"
|
||
#include "types/TrafficLightTypes.h"
|
||
#include "network/WebSocketServer.h"
|
||
#include <nlohmann/json.hpp>
|
||
|
||
System* System::instance_ = nullptr;
|
||
|
||
System::System()
|
||
: controllableVehicles_(ControllableVehicles::getInstance()) {
|
||
instance_ = this;
|
||
std::signal(SIGINT, signalHandler);
|
||
std::signal(SIGTERM, signalHandler);
|
||
|
||
// 使用单例模式获取配置实例
|
||
auto& config = SystemConfig::instance();
|
||
}
|
||
|
||
System::~System() {
|
||
stop();
|
||
instance_ = nullptr;
|
||
}
|
||
|
||
void System::signalHandler(int signal) {
|
||
Logger::info("Received signal: ", signal);
|
||
if (instance_) {
|
||
instance_->stop();
|
||
}
|
||
std::exit(0);
|
||
}
|
||
|
||
bool System::initialize() {
|
||
try {
|
||
// 配置已在 main 中加载，这里直接使用
|
||
const auto& system_config = SystemConfig::instance();
|
||
|
||
// 初始化全局坐标转换器
|
||
CoordinateConverter::instance().setReferencePoint(
|
||
system_config.airport.reference_point.latitude,
|
||
system_config.airport.reference_point.longitude);
|
||
|
||
// 加载路口配置
|
||
intersection_config_ = IntersectionConfig::load("config/intersections.json");
|
||
Logger::info("Loaded {} intersections", intersection_config_.getIntersections().size());
|
||
|
||
// 初始化 WebSocket 服务器
|
||
ws_server_ = std::make_unique<network::WebSocketServer>(system_config.websocket.port);
|
||
ws_thread_ = std::thread([this]() { ws_server_->start(); });
|
||
Logger::info("WebSocket server initialized on port ", system_config.websocket.port);
|
||
|
||
// 新增: 初始化并启动红绿灯 HTTP 服务器
|
||
try {
|
||
traffic_light_http_server_ = std::make_unique<network::TrafficLightHttpServer>(
|
||
system_config.traffic_light_server.port,
|
||
system_config.traffic_light_server.max_connections,
|
||
".",
|
||
*this
|
||
);
|
||
// Start server in its own thread (start itself handles creating internal threads for io_context)
|
||
traffic_light_http_server_thread_ = std::thread([this]() {
|
||
try {
|
||
traffic_light_http_server_->start(2); // Use 2 threads for IO context
|
||
} catch (const std::exception& e) {
|
||
Logger::error("TrafficLightHttpServer thread failed to start: ", e.what());
|
||
}
|
||
});
|
||
Logger::info("Traffic light HTTP server initialized on port ", system_config.traffic_light_server.port);
|
||
} catch (const std::exception& e) {
|
||
Logger::error("Failed to initialize Traffic Light HTTP Server: ", e.what());
|
||
// Decide if this failure is critical
|
||
return false; // Example: treat as critical
|
||
}
|
||
|
||
// 加载机场区域配置
|
||
airportBounds_ = std::make_unique<AirportBounds>("config/airport_bounds.json");
|
||
|
||
// 初始化冲突检测器
|
||
collisionDetector_ = std::make_unique<CollisionDetector>(*airportBounds_, controllableVehicles_);
|
||
|
||
// 初始化红绿灯检测器
|
||
trafficLightDetector_ = std::make_unique<TrafficLightDetector>(intersection_config_, controllableVehicles_, *this);
|
||
|
||
// 初始化安全区
|
||
initializeSafetyZones();
|
||
|
||
// 加载告警配置
|
||
WarnConfig warnConfig{
|
||
system_config.warning.warning_interval_ms,
|
||
system_config.warning.log_interval_ms };
|
||
|
||
// 创建数据采集器并初始化
|
||
dataCollector_ = std::make_unique<DataCollector>(*airportBounds_);
|
||
return dataCollector_->initialize(system_config.data_source, warnConfig);
|
||
} catch (const std::exception& e) {
|
||
Logger::error("Failed to initialize system: ", e.what());
|
||
return false;
|
||
}
|
||
}
|
||
|
||
void System::initializeSafetyZones() {
|
||
// 清空现有的安全区
|
||
safetyZones_.clear();
|
||
|
||
// 获取所有路口配置
|
||
const auto& intersections = intersection_config_.getIntersections();
|
||
|
||
// 为每个路口创建安全区
|
||
for (const auto& intersection : intersections) {
|
||
// 获取路口中心点的地理坐标
|
||
Vector2D center = CoordinateConverter::instance().toLocalXY(
|
||
intersection.position.latitude,
|
||
intersection.position.longitude);
|
||
|
||
// 创建安全区
|
||
auto safetyZone = std::make_unique<SafetyZone>(
|
||
center,
|
||
intersection.safetyZone.aircraftRadius, // 飞机安全区半径
|
||
intersection.safetyZone.vehicleRadius // 特勤车安全区半径
|
||
);
|
||
|
||
Logger::debug("创建路口安全区: id=", intersection.id,
|
||
", center=(", center.x, ",", center.y, ")",
|
||
", aircraftRadius=", intersection.safetyZone.aircraftRadius,
|
||
", vehicleRadius=", intersection.safetyZone.vehicleRadius);
|
||
|
||
// 保存安全区
|
||
safetyZones_[intersection.id] = std::move(safetyZone);
|
||
}
|
||
}
|
||
|
||
void System::start() {
|
||
if (running_) {
|
||
return;
|
||
}
|
||
|
||
running_ = true;
|
||
dataCollector_->start();
|
||
processThread_ = std::thread(&System::processLoop, this);
|
||
Logger::info("System processing loop started");
|
||
}
|
||
|
||
void System::stop() {
|
||
Logger::info("Stopping system...");
|
||
running_ = false;
|
||
|
||
// Stop data collector first
|
||
if (dataCollector_) {
|
||
dataCollector_->stop();
|
||
}
|
||
|
||
// Stop WebSocket server
|
||
if (ws_server_) {
|
||
ws_server_->stop();
|
||
}
|
||
if (ws_thread_.joinable()) {
|
||
ws_thread_.join();
|
||
}
|
||
|
||
// 新增: Stop Traffic Light HTTP Server
|
||
if (traffic_light_http_server_) {
|
||
traffic_light_http_server_->stop();
|
||
}
|
||
if (traffic_light_http_server_thread_.joinable()) {
|
||
traffic_light_http_server_thread_.join();
|
||
}
|
||
|
||
// Stop processing loop
|
||
if (processThread_.joinable()) {
|
||
processThread_.join();
|
||
}
|
||
|
||
Logger::info("System fully stopped.");
|
||
}
|
||
|
||
void System::processLoop() {
|
||
while (running_) {
|
||
try {
|
||
auto now = std::chrono::steady_clock::now();
|
||
|
||
// 获取最新数据和时间戳
|
||
std::tie(latest_aircraft_, latest_vehicles_, latest_traffic_lights_) = dataCollector_->getLatestData();
|
||
auto [aircraft_ts, vehicle_ts, traffic_light_ts] = dataCollector_->getLastUpdateTimestamps();
|
||
|
||
// 检查数据更新
|
||
bool has_new_data = false;
|
||
|
||
if (aircraft_ts > last_aircraft_timestamp) {
|
||
has_new_data = true;
|
||
last_aircraft_timestamp = aircraft_ts;
|
||
Logger::debug("航空器数据已更新，新时间戳: ", aircraft_ts);
|
||
}
|
||
|
||
if (vehicle_ts > last_vehicle_timestamp) {
|
||
has_new_data = true;
|
||
last_vehicle_timestamp = vehicle_ts;
|
||
Logger::debug("车辆数据已更新，新时间戳: ", vehicle_ts);
|
||
}
|
||
|
||
if (traffic_light_ts > last_traffic_light_timestamp) {
|
||
has_new_data = true;
|
||
last_traffic_light_timestamp = traffic_light_ts;
|
||
Logger::debug("红绿灯数据已更新，新时间戳: ", traffic_light_ts);
|
||
}
|
||
|
||
if (has_new_data) {
|
||
// 使用成员变量的引用构建 objects 向量
|
||
std::vector<MovingObject*> objects;
|
||
for (auto& ac : latest_aircraft_) {
|
||
objects.push_back(&ac);
|
||
}
|
||
for (auto& veh : latest_vehicles_) {
|
||
const auto* config = controllableVehicles_.findVehicle(veh.vehicleNo);
|
||
if (config) {
|
||
veh.type = config->type == "UNMANNED" ? MovingObjectType::UNMANNED : MovingObjectType::SPECIAL;
|
||
veh.isControllable = config->type == "UNMANNED";
|
||
}
|
||
objects.push_back(&veh);
|
||
}
|
||
|
||
// 创建当前周期的风险管理器
|
||
std::unordered_map<std::string, RiskLevel> vehicleMaxRiskLevels; // 统一记录所有车辆的最高风险等级
|
||
std::vector<CollisionRisk> detectedRisks; // 统一收集所有检测到的风险
|
||
|
||
// 检查安全区更新
|
||
auto safety_zone_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
now - last_safety_zone_update)
|
||
.count();
|
||
|
||
if (safety_zone_elapsed >= SystemConfig::instance().collision_detection.update_interval_ms) {
|
||
updateSafetyZoneStates(objects);
|
||
Logger::debug("开始检查安全区冲突...");
|
||
|
||
// 记录安全区风险检测前的风险数量
|
||
size_t beforeSafetyZone = detectedRisks.size();
|
||
checkUnmannedVehicleSafetyZones(latest_vehicles_, objects, vehicleMaxRiskLevels, detectedRisks);
|
||
Logger::debug("安全区检测新增风险数量: ", detectedRisks.size() - beforeSafetyZone);
|
||
|
||
last_safety_zone_update = now;
|
||
}
|
||
|
||
// 检查和处理常规碰撞风险
|
||
collisionDetector_->updateTraffic(latest_aircraft_, latest_vehicles_);
|
||
auto collisionRisks = collisionDetector_->detectCollisions();
|
||
|
||
Logger::debug("碰撞检测器检测到风险数量: ", collisionRisks.size());
|
||
for (const auto& risk : collisionRisks) {
|
||
Logger::debug("碰撞风险: id1=", risk.id1,
|
||
", id2=", risk.id2,
|
||
", level=", static_cast<int>(risk.level),
|
||
", distance=", risk.distance);
|
||
}
|
||
|
||
// 合并风险
|
||
size_t beforeMerge = detectedRisks.size();
|
||
detectedRisks.insert(detectedRisks.end(), collisionRisks.begin(), collisionRisks.end());
|
||
Logger::debug("合并后新增风险数量: ", detectedRisks.size() - beforeMerge);
|
||
|
||
// 统一处理所有风险
|
||
processCollisions(detectedRisks, vehicleMaxRiskLevels);
|
||
|
||
// 广播位置更新
|
||
for (const auto& ac : latest_aircraft_) {
|
||
broadcastPositionUpdate(ac);
|
||
}
|
||
for (const auto& veh : latest_vehicles_) {
|
||
broadcastPositionUpdate(veh);
|
||
}
|
||
|
||
// 处理红绿灯信号
|
||
for (const auto& signal : latest_traffic_lights_) {
|
||
broadcastTrafficLightStatus(signal);
|
||
trafficLightDetector_->processSignal(signal, latest_vehicles_);
|
||
}
|
||
}
|
||
|
||
// 等待下一次处理
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(
|
||
SystemConfig::instance().collision_detection.update_interval_ms));
|
||
} catch (const std::exception& e) {
|
||
Logger::error("处理循环发生错误: ", e.what());
|
||
}
|
||
}
|
||
}
|
||
|
||
void System::checkUnmannedVehicleSafetyZones(
|
||
const std::vector<Vehicle>& vehicles,
|
||
const std::vector<MovingObject*>& objects,
|
||
std::unordered_map<std::string, RiskLevel>& vehicleMaxRiskLevels,
|
||
std::vector<CollisionRisk>& detectedRisks) {
|
||
|
||
// 遍历所有无人车
|
||
for (const auto& vehicle : vehicles) {
|
||
// 遍历所有路口安全区
|
||
for (const auto& [intersectionId, zone] : safetyZones_) {
|
||
if (zone->getState() == SafetyZoneState::INACTIVE) {
|
||
continue;
|
||
}
|
||
|
||
// 计算无人车到安全区中心的距离
|
||
double dx = vehicle.position.x - zone->getCenter().x;
|
||
double dy = vehicle.position.y - zone->getCenter().y;
|
||
double distance = std::sqrt(dx * dx + dy * dy);
|
||
double zoneRadius = zone->getCurrentRadius();
|
||
|
||
RiskLevel riskLevel = RiskLevel::NONE;
|
||
if (distance <= zoneRadius) {
|
||
riskLevel = RiskLevel::CRITICAL;
|
||
} else if (distance <= 2 * zoneRadius) {
|
||
riskLevel = RiskLevel::WARNING;
|
||
}
|
||
|
||
if (riskLevel != RiskLevel::NONE) {
|
||
// 查找触发安全区的目标物体
|
||
for (const auto* obj : objects) {
|
||
// 添加判断：跳过与自己的碰撞检测
|
||
if (obj->id == vehicle.id) {
|
||
continue;
|
||
}
|
||
|
||
if ((obj->isAircraft() || obj->isSpecialVehicle()) &&
|
||
zone->isObjectInZone(*obj)) {
|
||
// 创建风险对象
|
||
CollisionRisk risk;
|
||
risk.id1 = vehicle.id;
|
||
risk.id2 = obj->id;
|
||
risk.level = riskLevel;
|
||
risk.distance = distance;
|
||
risk.minDistance = distance;
|
||
risk.relativeSpeed = std::sqrt(
|
||
std::pow(obj->speed * std::cos(obj->heading) -
|
||
vehicle.speed * std::cos(vehicle.heading),
|
||
2) +
|
||
std::pow(obj->speed * std::sin(obj->heading) -
|
||
vehicle.speed * std::sin(vehicle.heading),
|
||
2));
|
||
risk.relativeMotion = {
|
||
obj->position.x - vehicle.position.x,
|
||
obj->position.y - vehicle.position.y };
|
||
|
||
detectedRisks.push_back(risk);
|
||
|
||
// 更新风险级别
|
||
auto& currentRisk = vehicleMaxRiskLevels[vehicle.id];
|
||
currentRisk = std::max(currentRisk, riskLevel);
|
||
|
||
Logger::debug("安全区风险: id1=", risk.id1,
|
||
", id2=", risk.id2,
|
||
", level=", static_cast<int>(risk.level),
|
||
", distance=", risk.distance);
|
||
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void System::processCollisions(
|
||
const std::vector<CollisionRisk>& detectedRisks,
|
||
std::unordered_map<std::string, RiskLevel>& vehicleMaxRiskLevels) {
|
||
|
||
// 记录当前有风险的可控车辆
|
||
std::unordered_set<std::string> currentVehiclesWithRisk;
|
||
|
||
// 修改日志输出，显示更多信息
|
||
Logger::debug("开始处理碰撞风险，风险数量: ", detectedRisks.size());
|
||
for (const auto& risk : detectedRisks) {
|
||
Logger::debug("待处理风险: id1=", risk.id1,
|
||
", id2=", risk.id2,
|
||
", level=", static_cast<int>(risk.level),
|
||
", distance=", risk.distance,
|
||
", minDistance=", risk.minDistance);
|
||
}
|
||
|
||
// 处理当前的碰撞风险
|
||
for (const auto& risk : detectedRisks) {
|
||
// 处理 id1 和 id2 中的可控车辆
|
||
bool id1_controllable = controllableVehicles_.isControllable(risk.id1);
|
||
bool id2_controllable = controllableVehicles_.isControllable(risk.id2);
|
||
|
||
auto processVehicle = [&](const std::string& vehicleId, const std::string& otherId) {
|
||
// 发送指令
|
||
VehicleCommand cmd;
|
||
cmd.vehicleId = vehicleId;
|
||
cmd.type = risk.level == RiskLevel::CRITICAL ? CommandType::ALERT : CommandType::WARNING;
|
||
cmd.reason = otherId.substr(0, 2) == "AC" ? CommandReason::AIRCRAFT_CROSSING : CommandReason::SPECIAL_VEHICLE;
|
||
cmd.timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch())
|
||
.count();
|
||
|
||
// 设置目标位置
|
||
const MovingObject* target = findVehicle(otherId);
|
||
if (target) {
|
||
cmd.latitude = target->geo.latitude;
|
||
cmd.longitude = target->geo.longitude;
|
||
cmd.relativeSpeed = risk.relativeSpeed;
|
||
cmd.relativeMotionX = risk.relativeMotion.x;
|
||
cmd.relativeMotionY = risk.relativeMotion.y;
|
||
cmd.minDistance = risk.minDistance;
|
||
}
|
||
|
||
Logger::debug("准备发送指令: 车辆=", vehicleId,
|
||
", 类型=", cmd.type == CommandType::ALERT ? "ALERT" : "WARNING",
|
||
", 原因=", cmd.reason == CommandReason::AIRCRAFT_CROSSING ? "AIRCRAFT_CROSSING" : "SPECIAL_VEHICLE",
|
||
", 距离=", risk.distance,
|
||
", 风险等级=", static_cast<int>(risk.level));
|
||
|
||
broadcastVehicleCommand(cmd);
|
||
controllableVehicles_.sendCommand(vehicleId, cmd);
|
||
|
||
// 更新风险记录
|
||
currentVehiclesWithRisk.insert(vehicleId);
|
||
vehicleMaxRiskLevels[vehicleId] = risk.level;
|
||
|
||
Logger::info("碰撞风险: 车辆=", vehicleId,
|
||
", 目标=", otherId,
|
||
", 距离=", risk.distance, "m",
|
||
", 最小距离=", risk.minDistance, "m",
|
||
", 相对速度=", risk.relativeSpeed, "m/s");
|
||
};
|
||
|
||
// 为每个可控车辆处理风险
|
||
if (id1_controllable) {
|
||
processVehicle(risk.id1, risk.id2);
|
||
}
|
||
if (id2_controllable) {
|
||
processVehicle(risk.id2, risk.id1);
|
||
}
|
||
|
||
// 广播碰撞预警消息
|
||
broadcastCollisionWarning(risk);
|
||
}
|
||
|
||
// 处理恢复指令
|
||
for (const auto& vehicleId : lastVehiclesWithRisk_) {
|
||
if (currentVehiclesWithRisk.find(vehicleId) == currentVehiclesWithRisk.end()) {
|
||
auto riskIt = vehicleMaxRiskLevels.find(vehicleId);
|
||
if (riskIt == vehicleMaxRiskLevels.end() || riskIt->second == RiskLevel::NONE) {
|
||
VehicleCommand cmd;
|
||
cmd.vehicleId = vehicleId;
|
||
cmd.type = CommandType::RESUME;
|
||
cmd.reason = CommandReason::RESUME_TRAFFIC;
|
||
cmd.timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch())
|
||
.count();
|
||
|
||
broadcastVehicleCommand(cmd);
|
||
controllableVehicles_.sendCommand(vehicleId, cmd);
|
||
Logger::info("发送恢复指令到车辆: ", vehicleId);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 更新风险车辆列表
|
||
lastVehiclesWithRisk_ = std::move(currentVehiclesWithRisk);
|
||
}
|
||
|
||
void System::broadcastPositionUpdate(const MovingObject& obj) {
|
||
if (!ws_server_) {
|
||
return;
|
||
}
|
||
|
||
network::PositionUpdateMessage msg;
|
||
msg.objectId = obj.id;
|
||
msg.objectType = (typeid(obj) == typeid(Aircraft)) ? "aircraft" : "vehicle";
|
||
msg.timestamp = obj.timestamp;
|
||
msg.longitude = obj.geo.longitude;
|
||
msg.latitude = obj.geo.latitude;
|
||
msg.heading = obj.heading;
|
||
msg.speed = obj.speed;
|
||
|
||
nlohmann::json j = {
|
||
{"type", "position_update"},
|
||
{"object_id", msg.objectId},
|
||
{"object_type", msg.objectType},
|
||
{"timestamp", msg.timestamp},
|
||
{"position", {{"longitude", msg.longitude}, {"latitude", msg.latitude}}},
|
||
{"heading", msg.heading},
|
||
{"speed", msg.speed} };
|
||
|
||
ws_server_->broadcast(j.dump());
|
||
Logger::debug("广播位置更新: id=", msg.objectId,
|
||
" type=", msg.objectType,
|
||
" pos=(", msg.longitude, ",", msg.latitude, ")",
|
||
" heading=", msg.heading,
|
||
" speed=", msg.speed,
|
||
" timestamp=", msg.timestamp);
|
||
}
|
||
|
||
// 新增辅助函数：将 SignalStatus 转换为字符串
|
||
std::string signalStatusToString(SignalStatus status) {
|
||
switch (status) {
|
||
case SignalStatus::RED: return "RED";
|
||
case SignalStatus::GREEN: return "GREEN";
|
||
case SignalStatus::YELLOW: return "YELLOW";
|
||
default: return "UNKNOWN";
|
||
}
|
||
}
|
||
|
||
void System::broadcastTrafficLightStatus(const TrafficLightSignal& signal) {
|
||
if (!ws_server_) {
|
||
return;
|
||
}
|
||
|
||
// 查找路口信息
|
||
const Intersection* intersection = intersection_config_.findByTrafficLightId(signal.trafficLightId);
|
||
|
||
nlohmann::json j = {
|
||
{"type", "traffic_light_status"},
|
||
{"id", signal.trafficLightId},
|
||
// 修改: 将 status 改为一个包含 ns 和 ew 状态的对象
|
||
{"status", {
|
||
{"ns", signalStatusToString(signal.ns_status)},
|
||
{"ew", signalStatusToString(signal.ew_status)}
|
||
}},
|
||
{"timestamp", signal.timestamp}
|
||
};
|
||
|
||
// 添加路口信息
|
||
if (intersection) {
|
||
j["intersection"] = intersection->id;
|
||
j["position"] = {
|
||
{"longitude", intersection->position.longitude},
|
||
{"latitude", intersection->position.latitude} };
|
||
}
|
||
|
||
ws_server_->broadcast(j.dump());
|
||
|
||
// 修改日志记录
|
||
Logger::debug("广播红绿灯状态: id=", signal.trafficLightId,
|
||
", NS_Status=", signalStatusToString(signal.ns_status),
|
||
", EW_Status=", signalStatusToString(signal.ew_status),
|
||
", intersection=", intersection ? intersection->id : "",
|
||
", timestamp=", signal.timestamp);
|
||
}
|
||
|
||
std::string getRiskLevelString(RiskLevel level) {
|
||
switch (level) {
|
||
case RiskLevel::CRITICAL:
|
||
return "critical";
|
||
case RiskLevel::WARNING:
|
||
return "warning";
|
||
default:
|
||
return "none";
|
||
}
|
||
}
|
||
|
||
void System::broadcastCollisionWarning(const CollisionRisk& risk) {
|
||
if (!ws_server_) {
|
||
return;
|
||
}
|
||
|
||
// 构造冲突预警消息
|
||
nlohmann::json j = {
|
||
{"type", "collision_warning"},
|
||
{"id1", risk.id1},
|
||
{"id2", risk.id2},
|
||
{"distance", risk.distance},
|
||
{"relativeSpeed", risk.relativeSpeed},
|
||
{"warningLevel", getRiskLevelString(risk.level)},
|
||
{"timestamp", std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch())
|
||
.count()} };
|
||
|
||
// 广播冲突预警消息
|
||
ws_server_->broadcast(j.dump());
|
||
Logger::debug("广播冲突预警: id1=", risk.id1, " id2=", risk.id2,
|
||
" distance=", risk.distance, "m",
|
||
" relativeSpeed=", risk.relativeSpeed, "m/s",
|
||
" level=", getRiskLevelString(risk.level));
|
||
}
|
||
|
||
void System::broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning) {
|
||
if (ws_server_) {
|
||
ws_server_->broadcast(warning.toJson().dump());
|
||
|
||
// 根据超时时间记录不同级的日志
|
||
if (warning.elapsed_ms > 30000) { // 30秒以上
|
||
Logger::error("Severe timeout: ", warning.source, " - ",
|
||
warning.elapsed_ms, "ms without response");
|
||
} else {
|
||
Logger::warning("Connection timeout: ", warning.source, " - ",
|
||
warning.elapsed_ms, "ms without response");
|
||
}
|
||
}
|
||
}
|
||
|
||
void System::broadcastVehicleCommand(const VehicleCommand& cmd) {
|
||
if (!ws_server_) {
|
||
return;
|
||
}
|
||
|
||
nlohmann::json j = {
|
||
{"type", "vehicle_command"},
|
||
{"vehicleId", cmd.vehicleId},
|
||
{"commandType", [&]() {
|
||
switch (cmd.type) {
|
||
case CommandType::SIGNAL:
|
||
return "SIGNAL";
|
||
case CommandType::ALERT:
|
||
return "ALERT";
|
||
case CommandType::WARNING:
|
||
return "WARNING";
|
||
case CommandType::RESUME:
|
||
return "RESUME";
|
||
case CommandType::PARKING:
|
||
return "PARKING";
|
||
default:
|
||
return "UNKNOWN";
|
||
}
|
||
}()},
|
||
{"reason", [&]() {
|
||
switch (cmd.reason) {
|
||
case CommandReason::TRAFFIC_LIGHT:
|
||
return "TRAFFIC_LIGHT";
|
||
case CommandReason::AIRCRAFT_CROSSING:
|
||
return "AIRCRAFT_CROSSING";
|
||
case CommandReason::SPECIAL_VEHICLE:
|
||
return "SPECIAL_VEHICLE";
|
||
case CommandReason::AIRCRAFT_PUSH:
|
||
return "AIRCRAFT_PUSH";
|
||
case CommandReason::RESUME_TRAFFIC:
|
||
return "RESUME_TRAFFIC";
|
||
case CommandReason::PARKING_SIDE:
|
||
return "PARKING_SIDE";
|
||
default:
|
||
return "UNKNOWN";
|
||
}
|
||
}()},
|
||
{"timestamp", cmd.timestamp} };
|
||
|
||
// 添加可选字段
|
||
if (cmd.type == CommandType::SIGNAL) {
|
||
j["signalState"] = cmd.signalState == SignalState::RED ? "RED" : "GREEN";
|
||
j["intersectionId"] = cmd.intersectionId;
|
||
}
|
||
|
||
// 添目标位置（对于所有非 RESUME 类型的指令）
|
||
if (cmd.type != CommandType::RESUME) {
|
||
j["targetLatitude"] = cmd.latitude;
|
||
j["targetLongitude"] = cmd.longitude;
|
||
}
|
||
|
||
ws_server_->broadcast(j.dump());
|
||
}
|
||
|
||
const MovingObject* System::findVehicle(const std::string& vehicleId) const {
|
||
if (!dataCollector_) {
|
||
return nullptr;
|
||
}
|
||
|
||
// 获取数据引用（注意：DataCollector 内部会加锁）
|
||
const auto& aircrafts = dataCollector_->getAircraftRef();
|
||
const auto& vehicles = dataCollector_->getVehicleRef();
|
||
|
||
// 在航空器中查找
|
||
for (const auto& aircraft : aircrafts) {
|
||
if (aircraft.flightNo == vehicleId) {
|
||
return &aircraft;
|
||
}
|
||
}
|
||
|
||
// 在车辆中查找
|
||
for (const auto& vehicle : vehicles) {
|
||
if (vehicle.vehicleNo == vehicleId) {
|
||
return &vehicle;
|
||
}
|
||
}
|
||
|
||
return nullptr;
|
||
}
|
||
|
||
void System::updateSafetyZoneStates(const std::vector<MovingObject*>& objects) {
|
||
// 重置所有安全区状态为未激活，同时重置类型
|
||
for (auto& [id, zone] : safetyZones_) {
|
||
zone->setState(SafetyZoneState::INACTIVE);
|
||
zone->resetType(); // 重置安全区类型
|
||
}
|
||
|
||
// 检查所有移动物体
|
||
for (const auto& obj : objects) {
|
||
Logger::debug("检查移动物体: id=", obj->id,
|
||
", type=", obj->type == MovingObjectType::UNMANNED ? "无人车" : (obj->type == MovingObjectType::SPECIAL ? "特勤车" : "飞机"),
|
||
", isAircraft=", obj->isAircraft(),
|
||
", isSpecialVehicle=", obj->isSpecialVehicle(),
|
||
", isUnmannedVehicle=", obj->isUnmannedVehicle());
|
||
|
||
// 只检查飞机和特勤车
|
||
if (obj->isAircraft() || obj->isSpecialVehicle()) {
|
||
checkSafetyZoneIntrusion(*obj);
|
||
}
|
||
}
|
||
}
|
||
|
||
void System::checkSafetyZoneIntrusion(const MovingObject& obj) {
|
||
Logger::debug("检查安全区入侵: id=", obj.id,
|
||
", type=", obj.isAircraft() ? "飞机" : (obj.isSpecialVehicle() ? "特勤车" : "其他"),
|
||
", position=(", obj.position.x, ",", obj.position.y, ")");
|
||
|
||
// 检查每个安全区
|
||
for (auto& [id, zone] : safetyZones_) {
|
||
// 先检查是否在安全区内
|
||
if (zone->isObjectInZone(obj)) {
|
||
// 如果在区内，尝试激活安全区
|
||
if (zone->tryActivate(obj)) {
|
||
Logger::debug("目标 ", obj.id, " 进入路口 ", id, " 安全区, 类型: ",
|
||
zone->getType() == SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车",
|
||
", 半径: ", zone->getCurrentRadius(),
|
||
", 中心点=(", zone->getCenter().x, ",", zone->getCenter().y, ")");
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
bool System::handleSafetyZoneRisk(const Vehicle& vehicle,
|
||
const SafetyZone* zone,
|
||
const std::vector<MovingObject*>& objects,
|
||
double distance,
|
||
const std::string& intersectionId,
|
||
CommandType cmdType,
|
||
const std::string& riskLevel) {
|
||
// 检查是否为无人车
|
||
if (!controllableVehicles_.isControllable(vehicle.id)) {
|
||
return false;
|
||
}
|
||
|
||
VehicleCommand cmd;
|
||
cmd.vehicleId = vehicle.id;
|
||
cmd.type = cmdType;
|
||
cmd.reason = zone->getType() == SafetyZoneType::AIRCRAFT ? CommandReason::AIRCRAFT_CROSSING : CommandReason::SPECIAL_VEHICLE;
|
||
cmd.timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch())
|
||
.count();
|
||
|
||
// 查找触发安全区的目标物体
|
||
const MovingObject* target = nullptr;
|
||
for (const auto& obj : objects) {
|
||
if ((obj->isAircraft() || obj->isSpecialVehicle()) &&
|
||
zone->isObjectInZone(*obj)) {
|
||
target = obj;
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 如果找到目标物体，添加相关信息
|
||
if (target) {
|
||
cmd.latitude = target->geo.latitude;
|
||
cmd.longitude = target->geo.longitude;
|
||
|
||
// 计算相对距离
|
||
double rel_dx = target->position.x - vehicle.position.x;
|
||
double rel_dy = target->position.y - vehicle.position.y;
|
||
|
||
// 计算相对速度
|
||
cmd.relativeSpeed = std::sqrt(
|
||
(target->position.x - vehicle.position.x) * (target->position.x - vehicle.position.x) +
|
||
(target->position.y - vehicle.position.y) * (target->position.y - vehicle.position.y));
|
||
cmd.relativeMotionX = rel_dx;
|
||
cmd.relativeMotionY = rel_dy;
|
||
cmd.minDistance = distance;
|
||
|
||
Logger::debug("安全区冲突目标信息: id=", target->id,
|
||
", 相对距离=(", rel_dx, ",", rel_dy, ")",
|
||
", 相对速度=", cmd.relativeSpeed,
|
||
", 最小距离=", distance);
|
||
}
|
||
|
||
broadcastVehicleCommand(cmd);
|
||
controllableVehicles_.sendCommand(vehicle.id, cmd);
|
||
|
||
CollisionRisk risk;
|
||
risk.id1 = vehicle.id;
|
||
risk.id2 = target ? target->id : "";
|
||
risk.level = cmdType == CommandType::ALERT ? RiskLevel::CRITICAL : RiskLevel::WARNING;
|
||
risk.distance = distance;
|
||
risk.relativeSpeed = cmd.relativeSpeed;
|
||
risk.relativeMotion = { cmd.relativeMotionX, cmd.relativeMotionY };
|
||
risk.zoneType = WarningZoneType::WARNING;
|
||
|
||
broadcastCollisionWarning(risk);
|
||
|
||
return true;
|
||
}
|
||
|
||
void System::processPushedTrafficLightData(const nlohmann::json& di_data) {
|
||
try {
|
||
const auto& config = SystemConfig::instance();
|
||
const std::string targetIntersectionId = config.simulated_mobile_light_target_intersection_id;
|
||
const Intersection* targetIntersection = intersection_config_.findById(targetIntersectionId);
|
||
|
||
if (!targetIntersection) {
|
||
Logger::warning("Configured target intersection ID not found in intersection config: ", targetIntersectionId);
|
||
return;
|
||
}
|
||
|
||
SignalStatus ns_status = SignalStatus::UNKNOWN;
|
||
SignalStatus ew_status = SignalStatus::UNKNOWN;
|
||
|
||
if (di_data.contains("DI-13") && di_data.at("DI-13").get<int>() == 1) {
|
||
ns_status = SignalStatus::GREEN;
|
||
} else if (di_data.contains("DI-12") && di_data.at("DI-12").get<int>() == 1) {
|
||
ns_status = SignalStatus::YELLOW;
|
||
} else if (di_data.contains("DI-11") && di_data.at("DI-11").get<int>() == 1) {
|
||
ns_status = SignalStatus::RED;
|
||
} else {
|
||
Logger::warning("Received DI data has unknown or invalid N/S state: ", di_data.dump());
|
||
}
|
||
|
||
if (di_data.contains("DI-16") && di_data.at("DI-16").get<int>() == 1) {
|
||
ew_status = SignalStatus::GREEN;
|
||
} else if (di_data.contains("DI-15") && di_data.at("DI-15").get<int>() == 1) {
|
||
ew_status = SignalStatus::YELLOW;
|
||
} else if (di_data.contains("DI-14") && di_data.at("DI-14").get<int>() == 1) {
|
||
ew_status = SignalStatus::RED;
|
||
} else {
|
||
Logger::warning("Received DI data has unknown or invalid E/W state: ", di_data.dump());
|
||
}
|
||
|
||
TrafficLightSignal signal;
|
||
signal.trafficLightId = "DI_PUSHED_" + targetIntersectionId;
|
||
signal.ns_status = ns_status;
|
||
signal.ew_status = ew_status;
|
||
signal.timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||
|
||
if (ws_server_) {
|
||
nlohmann::json messageJson;
|
||
messageJson["type"] = "intersection_traffic_light_status";
|
||
messageJson["intersection_id"] = targetIntersection->id;
|
||
messageJson["position"] = {
|
||
{"latitude", targetIntersection->position.latitude},
|
||
{"longitude", targetIntersection->position.longitude}
|
||
};
|
||
messageJson["ns_status"] = static_cast<int>(signal.ns_status);
|
||
messageJson["ew_status"] = static_cast<int>(signal.ew_status);
|
||
messageJson["timestamp"] = signal.timestamp;
|
||
|
||
ws_server_->broadcast(messageJson.dump());
|
||
Logger::debug("广播路口交通灯状态: ", messageJson.dump());
|
||
} else {
|
||
Logger::warning("WebSocket server is not available, cannot broadcast intersection status.");
|
||
}
|
||
|
||
} catch (const std::exception& e) {
|
||
Logger::error("处理推送的交通信号灯数据时出错: ", e.what());
|
||
}
|
||
} |