修改碰撞检测逻辑
This commit is contained in:
parent
573964b16f
commit
dce6f88c77
@ -49,14 +49,15 @@ set(LIB_SOURCES
|
||||
src/collector/DataCollector.cpp
|
||||
src/core/System.cpp
|
||||
src/detector/CollisionDetector.cpp
|
||||
src/detector/SimpleCollisionDetector.cpp
|
||||
src/network/HTTPDataSource.cpp
|
||||
src/network/WebSocketServer.cpp
|
||||
src/network/HTTPClient.cpp
|
||||
src/spatial/AirportBounds.cpp
|
||||
src/spatial/CoordinateConverter.cpp
|
||||
src/types/BasicTypes.cpp
|
||||
src/types/VehicleData.cpp
|
||||
src/vehicle/ControllableVehicles.cpp
|
||||
src/config/AirportBounds.cpp
|
||||
src/config/SystemConfig.cpp
|
||||
src/config/IntersectionConfig.cpp
|
||||
src/detector/TrafficLightDetector.cpp
|
||||
|
||||
@ -17,7 +17,7 @@
|
||||
},
|
||||
"config": {
|
||||
"vehicle_collision_radius": 50.0,
|
||||
"aircraft_ground_radius": 100.0,
|
||||
"aircraft_collision_radius": 100.0,
|
||||
"height_threshold": 15.0,
|
||||
"warning_zone_radius": 100.0,
|
||||
"alert_zone_radius": 50.0
|
||||
@ -32,7 +32,7 @@
|
||||
},
|
||||
"config": {
|
||||
"vehicle_collision_radius": 30.0,
|
||||
"aircraft_ground_radius": 50.0,
|
||||
"aircraft_collision_radius": 50.0,
|
||||
"height_threshold": 10.0,
|
||||
"warning_zone_radius": 50.0,
|
||||
"alert_zone_radius": 25.0
|
||||
@ -47,7 +47,7 @@
|
||||
},
|
||||
"config": {
|
||||
"vehicle_collision_radius": 20.0,
|
||||
"aircraft_ground_radius": 40.0,
|
||||
"aircraft_collision_radius": 40.0,
|
||||
"height_threshold": 5.0,
|
||||
"warning_zone_radius": 40.0,
|
||||
"alert_zone_radius": 20.0
|
||||
@ -62,7 +62,7 @@
|
||||
},
|
||||
"config": {
|
||||
"vehicle_collision_radius": 15.0,
|
||||
"aircraft_ground_radius": 30.0,
|
||||
"aircraft_collision_radius": 30.0,
|
||||
"height_threshold": 5.0,
|
||||
"warning_zone_radius": 30.0,
|
||||
"alert_zone_radius": 15.0
|
||||
@ -77,7 +77,7 @@
|
||||
},
|
||||
"config": {
|
||||
"vehicle_collision_radius": 25.0,
|
||||
"aircraft_ground_radius": 50.0,
|
||||
"aircraft_collision_radius": 50.0,
|
||||
"height_threshold": 10.0,
|
||||
"warning_zone_radius": 100.0,
|
||||
"alert_zone_radius": 50.0
|
||||
|
||||
@ -35,27 +35,9 @@
|
||||
"collision_detection": {
|
||||
"update_interval_ms": 200,
|
||||
"prediction": {
|
||||
"time_window": 30.0,
|
||||
"vehicle_size": 10.0,
|
||||
"aircraft_size": 50.0
|
||||
},
|
||||
"thresholds": {
|
||||
"runway": {
|
||||
"aircraft_ground": 100.0,
|
||||
"vehicle": 50.0
|
||||
},
|
||||
"taxiway": {
|
||||
"aircraft_ground": 50.0,
|
||||
"vehicle": 30.0
|
||||
},
|
||||
"apron": {
|
||||
"aircraft_ground": 40.0,
|
||||
"vehicle": 20.0
|
||||
},
|
||||
"service": {
|
||||
"aircraft_ground": 30.0,
|
||||
"vehicle": 15.0
|
||||
}
|
||||
"time_window": 20.0,
|
||||
"vehicle_size": 5.0,
|
||||
"aircraft_size": 25.0
|
||||
}
|
||||
},
|
||||
"logging": {
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include "spatial/AirportBounds.h"
|
||||
#include "AirportBounds.h"
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
@ -77,7 +77,7 @@ void AirportBounds::loadConfig(const std::string& configFile) {
|
||||
auto& config = value["config"];
|
||||
areaConfigs_[type] = {
|
||||
config["vehicle_collision_radius"].get<double>(),
|
||||
config["aircraft_ground_radius"].get<double>(),
|
||||
config["aircraft_collision_radius"].get<double>(),
|
||||
config["height_threshold"].get<double>(),
|
||||
config["warning_zone_radius"].get<double>(),
|
||||
config["alert_zone_radius"].get<double>()
|
||||
@ -17,7 +17,7 @@ enum class AreaType {
|
||||
// 区域配置
|
||||
struct AreaConfig {
|
||||
double vehicleCollisionRadius = 0.0; // 车辆间冲突检测半径
|
||||
double aircraftGroundRadius = 0.0; // 航空器与车辆冲突检测半径
|
||||
double aircraftCollisionRadius = 0.0; // 航空器与车辆冲突检测半径
|
||||
double heightThreshold = 0.0; // 高度阈值
|
||||
double warning_zone_radius = 0.0; // 预警区域半径
|
||||
double alert_zone_radius = 0.0; // 警报区域半径
|
||||
@ -32,9 +32,9 @@ struct AreaConfig {
|
||||
CollisionThresholds getThresholds(bool isAircraft1, bool isAircraft2) const {
|
||||
// 如果没有配置预警和警报距离，使用碰撞半径作为默认值
|
||||
double warningRadius = warning_zone_radius > 0 ? warning_zone_radius :
|
||||
(isAircraft1 || isAircraft2 ? aircraftGroundRadius : vehicleCollisionRadius);
|
||||
(isAircraft1 || isAircraft2 ? aircraftCollisionRadius : vehicleCollisionRadius);
|
||||
double alertRadius = alert_zone_radius > 0 ? alert_zone_radius :
|
||||
(isAircraft1 || isAircraft2 ? aircraftGroundRadius * 0.5 : vehicleCollisionRadius * 0.5);
|
||||
(isAircraft1 || isAircraft2 ? aircraftCollisionRadius * 0.5 : vehicleCollisionRadius * 0.5);
|
||||
|
||||
return {
|
||||
warningRadius, // 预警距离
|
||||
@ -39,16 +39,10 @@ void SystemConfig::load(const std::string& filename) {
|
||||
// 加载碰撞检测配置
|
||||
collision_detection.update_interval_ms = j["collision_detection"]["update_interval_ms"];
|
||||
|
||||
// 加载阈值配置
|
||||
auto& thresholds = j["collision_detection"]["thresholds"];
|
||||
collision_detection.thresholds.runway.aircraft_ground = thresholds["runway"]["aircraft_ground"];
|
||||
collision_detection.thresholds.runway.vehicle = thresholds["runway"]["vehicle"];
|
||||
collision_detection.thresholds.taxiway.aircraft_ground = thresholds["taxiway"]["aircraft_ground"];
|
||||
collision_detection.thresholds.taxiway.vehicle = thresholds["taxiway"]["vehicle"];
|
||||
collision_detection.thresholds.apron.aircraft_ground = thresholds["apron"]["aircraft_ground"];
|
||||
collision_detection.thresholds.apron.vehicle = thresholds["apron"]["vehicle"];
|
||||
collision_detection.thresholds.service.aircraft_ground = thresholds["service"]["aircraft_ground"];
|
||||
collision_detection.thresholds.service.vehicle = thresholds["service"]["vehicle"];
|
||||
// 加载碰撞预测配置
|
||||
collision_detection.prediction.time_window = j["collision_detection"]["prediction"]["time_window"];
|
||||
collision_detection.prediction.vehicle_size = j["collision_detection"]["prediction"]["vehicle_size"];
|
||||
collision_detection.prediction.aircraft_size = j["collision_detection"]["prediction"]["aircraft_size"];
|
||||
|
||||
// 加载日志配置
|
||||
logging.level = j["logging"]["level"];
|
||||
|
||||
@ -59,16 +59,6 @@ public:
|
||||
double vehicle_size;
|
||||
double aircraft_size;
|
||||
} prediction;
|
||||
struct Thresholds {
|
||||
struct Area {
|
||||
double aircraft_ground;
|
||||
double vehicle;
|
||||
};
|
||||
Area runway;
|
||||
Area taxiway;
|
||||
Area apron;
|
||||
Area service;
|
||||
} thresholds;
|
||||
} collision_detection;
|
||||
|
||||
struct Logging {
|
||||
|
||||
@ -74,6 +74,11 @@ bool System::initialize() {
|
||||
system_config.warning.log_interval_ms
|
||||
};
|
||||
|
||||
// 初始化简单冲突检测器
|
||||
// simpleCollisionDetector_ = std::make_unique<SimpleCollisionDetector>(
|
||||
// intersection_config_, *controllableVehicles_);
|
||||
// Logger::info("简单冲突检测器初始化完成");
|
||||
|
||||
return dataCollector_->initialize(dataSourceConfig, warnConfig);
|
||||
}
|
||||
catch (const std::exception& e) {
|
||||
@ -207,11 +212,11 @@ void System::processLoop() {
|
||||
} else {
|
||||
// 正常的更新检查
|
||||
for (const auto& veh : vehicles) {
|
||||
Logger::debug("处理车辆: ", veh.vehicleNo);
|
||||
Logger::debug(" - 位置: (", veh.geo.longitude, ", ", veh.geo.latitude, ")");
|
||||
Logger::debug(" - 速度: ", veh.speed, "m/s");
|
||||
Logger::debug(" - 时间戳: ", veh.timestamp);
|
||||
Logger::debug(" - 时间差: ", veh.timestamp - last_vehicle_timestamp);
|
||||
// Logger::debug("处理车辆: ", veh.vehicleNo);
|
||||
// Logger::debug(" - 位置: (", veh.geo.longitude, ", ", veh.geo.latitude, ")");
|
||||
// Logger::debug(" - 速度: ", veh.speed, "m/s");
|
||||
// Logger::debug(" - 时间戳: ", veh.timestamp);
|
||||
// Logger::debug(" - 时间差: ", veh.timestamp - last_vehicle_timestamp);
|
||||
|
||||
if (veh.timestamp > last_vehicle_timestamp) {
|
||||
has_new_vehicles = true;
|
||||
@ -248,6 +253,7 @@ void System::processLoop() {
|
||||
// 只有当有新的数据时才更新冲突检测
|
||||
if (last_aircraft_timestamp > last_collision_timestamp ||
|
||||
last_vehicle_timestamp > last_collision_timestamp) {
|
||||
|
||||
// 更新冲突检测器
|
||||
collisionDetector_->updateTraffic(aircraft, vehicles);
|
||||
auto collisions = collisionDetector_->detectCollisions();
|
||||
@ -261,8 +267,47 @@ void System::processLoop() {
|
||||
", 区域类型=", static_cast<int>(risk.zoneType));
|
||||
}
|
||||
processCollisions(collisions);
|
||||
|
||||
// 简单冲突检测
|
||||
// simpleCollisionDetector_->updateTraffic(aircraft, vehicles);
|
||||
// auto simpleCollisions = simpleCollisionDetector_->detectCollisions();
|
||||
|
||||
// if(!simpleCollisions.empty()){
|
||||
// Logger::debug("检测到 ", simpleCollisions.size(), "个碰撞风险");
|
||||
// for (const auto& risk : simpleCollisions) {
|
||||
// Logger::debug("碰撞风险详情: id1=", risk.id1,
|
||||
// ", id2=", risk.id2,
|
||||
// ", 距离=", risk.distance, "m, 风险等级=", static_cast<int>(risk.level),
|
||||
// ", 区域类型=", static_cast<int>(risk.isIntersection));
|
||||
// }
|
||||
// processCollisions(simpleCollisions);
|
||||
} else if (!lastVehiclesWithRisk_.empty()) {
|
||||
// 当前没有任何风险，但上次有风险车辆，需要处理恢复指令
|
||||
Logger::debug("当前无碰撞风险，检查是否需要发送恢复指令");
|
||||
for (const auto& vehicleId : lastVehiclesWithRisk_) {
|
||||
Logger::debug("车辆 ", vehicleId, " 当前没有风险，准备发送恢复指令");
|
||||
VehicleCommand cmd;
|
||||
cmd.vehicleId = vehicleId;
|
||||
cmd.type = CommandType::RESUME;
|
||||
cmd.reason = CommandReason::RESUME_TRAFFIC;
|
||||
cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count();
|
||||
|
||||
broadcastVehicleCommand(cmd);
|
||||
controllableVehicles_->sendCommand(vehicleId, cmd);
|
||||
Logger::info("发送恢复指令到车辆: ", vehicleId);
|
||||
}
|
||||
// 清空风险车辆列表
|
||||
Logger::debug("清空风险车辆列表: 原数量=", lastVehiclesWithRisk_.size(),
|
||||
", 原列表={", [&]() {
|
||||
std::string s;
|
||||
for (const auto& id : lastVehiclesWithRisk_) {
|
||||
s += id + ",";
|
||||
}
|
||||
return s;
|
||||
}(), "}");
|
||||
lastVehiclesWithRisk_.clear();
|
||||
}
|
||||
|
||||
|
||||
last_collision_timestamp = std::max(last_aircraft_timestamp, last_vehicle_timestamp);
|
||||
}
|
||||
last_collision_update = now;
|
||||
@ -316,6 +361,141 @@ void System::processLoop() {
|
||||
}
|
||||
}
|
||||
|
||||
void System::processCollisions(const std::vector<SimpleCollisionRisk>& risks) {
|
||||
// 记录当前有风险的可控车辆
|
||||
std::unordered_set<std::string> currentVehiclesWithRisk;
|
||||
const auto& config = SystemConfig::instance().collision_detection;
|
||||
|
||||
Logger::debug("开始处理碰撞风险，当前风险数量: ", risks.size());
|
||||
|
||||
// 处理当前的碰撞风险
|
||||
for (const auto& risk : risks) {
|
||||
// 处理 id1 和 id2 中的可控车辆
|
||||
bool id1_controllable = controllableVehicles_->isControllable(risk.id1);
|
||||
bool id2_controllable = controllableVehicles_->isControllable(risk.id2);
|
||||
|
||||
// 如果两个都不是可控车辆，跳过
|
||||
if (!id1_controllable && !id2_controllable) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 根据风险等级处理
|
||||
auto processVehicle = [&](const std::string& vehicleId, const std::string& otherId) {
|
||||
switch (risk.level) {
|
||||
case SimpleRiskLevel::CRITICAL: {
|
||||
// 危险区域：立即发送告警指令
|
||||
VehicleCommand cmd;
|
||||
cmd.vehicleId = vehicleId;
|
||||
cmd.type = CommandType::ALERT;
|
||||
cmd.reason = otherId.substr(0, 2) == "AC" ?
|
||||
CommandReason::AIRCRAFT_CROSSING :
|
||||
CommandReason::SPECIAL_VEHICLE;
|
||||
cmd.timestamp = 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;
|
||||
}
|
||||
|
||||
broadcastVehicleCommand(cmd);
|
||||
controllableVehicles_->sendCommand(vehicleId, cmd);
|
||||
Logger::warning("发送告警指令到车辆: ", vehicleId,
|
||||
" 当前距离: ", risk.distance, "m",
|
||||
" 相对速度: ", risk.relativeSpeed, "m/s");
|
||||
break;
|
||||
}
|
||||
case SimpleRiskLevel::WARNING: {
|
||||
// 预警区域：发送预警指令
|
||||
VehicleCommand cmd;
|
||||
cmd.vehicleId = vehicleId;
|
||||
cmd.type = CommandType::WARNING;
|
||||
cmd.reason = otherId.substr(0, 2) == "AC" ?
|
||||
CommandReason::AIRCRAFT_CROSSING :
|
||||
CommandReason::SPECIAL_VEHICLE;
|
||||
cmd.timestamp = 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;
|
||||
}
|
||||
|
||||
broadcastVehicleCommand(cmd);
|
||||
controllableVehicles_->sendCommand(vehicleId, cmd);
|
||||
Logger::info("发送预警指令到车辆: ", vehicleId,
|
||||
" 当前距离: ", risk.distance, "m",
|
||||
" 相对速度: ", risk.relativeSpeed, "m/s");
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
};
|
||||
|
||||
// 为每个可控车辆处理风险
|
||||
if (id1_controllable) {
|
||||
currentVehiclesWithRisk.insert(risk.id1);
|
||||
Logger::debug("添加当前有风险的可控车辆: ", risk.id1,
|
||||
", 当前风险车辆数量: ", currentVehiclesWithRisk.size());
|
||||
processVehicle(risk.id1, risk.id2);
|
||||
}
|
||||
if (id2_controllable) {
|
||||
currentVehiclesWithRisk.insert(risk.id2);
|
||||
Logger::debug("添加当前有风险的可控车辆: ", risk.id2,
|
||||
", 当前风险车辆数量: ", currentVehiclesWithRisk.size());
|
||||
processVehicle(risk.id2, risk.id1);
|
||||
}
|
||||
|
||||
// 广播碰撞预警消息
|
||||
broadcastCollisionWarning(risk);
|
||||
}
|
||||
|
||||
Logger::debug("当前有风险的可控车辆数量: ", currentVehiclesWithRisk.size());
|
||||
Logger::debug("上一次有风险的可控车辆数量: ", lastVehiclesWithRisk_.size());
|
||||
|
||||
// 对之前有风险但现在没有风险的车辆发送恢复指令
|
||||
for (const auto& vehicleId : lastVehiclesWithRisk_) {
|
||||
Logger::debug("检查车辆是否需要恢复: ", vehicleId);
|
||||
if (currentVehiclesWithRisk.find(vehicleId) == currentVehiclesWithRisk.end()) {
|
||||
Logger::debug("车辆 ", vehicleId, " 当前没有风险，准备发送恢复指令");
|
||||
VehicleCommand cmd;
|
||||
cmd.vehicleId = vehicleId;
|
||||
cmd.type = CommandType::RESUME;
|
||||
cmd.reason = CommandReason::RESUME_TRAFFIC;
|
||||
cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count();
|
||||
|
||||
broadcastVehicleCommand(cmd);
|
||||
controllableVehicles_->sendCommand(vehicleId, cmd);
|
||||
Logger::info("发送恢复指令到车辆: ", vehicleId);
|
||||
} else {
|
||||
Logger::debug("车辆 ", vehicleId, " 仍然有风险，不发送恢复指令");
|
||||
}
|
||||
}
|
||||
|
||||
// 更新上一次有风险的车辆列表
|
||||
Logger::debug("更新风险车辆列表: 原数量=", lastVehiclesWithRisk_.size(),
|
||||
", 新数量=", currentVehiclesWithRisk.size(),
|
||||
", 原列表={", [&]() {
|
||||
std::string s;
|
||||
for (const auto& id : lastVehiclesWithRisk_) {
|
||||
s += id + ",";
|
||||
}
|
||||
return s;
|
||||
}(), "}, 新列表={", [&]() {
|
||||
std::string s;
|
||||
for (const auto& id : currentVehiclesWithRisk) {
|
||||
s += id + ",";
|
||||
}
|
||||
return s;
|
||||
}(), "}");
|
||||
lastVehiclesWithRisk_ = std::move(currentVehiclesWithRisk);
|
||||
}
|
||||
|
||||
void System::processCollisions(const std::vector<CollisionRisk>& risks) {
|
||||
// 记录当前有风险的可控车辆
|
||||
std::unordered_set<std::string> currentVehiclesWithRisk;
|
||||
@ -403,12 +583,14 @@ void System::processCollisions(const std::vector<CollisionRisk>& risks) {
|
||||
// 为每个可控车辆处理风险
|
||||
if (id1_controllable) {
|
||||
currentVehiclesWithRisk.insert(risk.id1);
|
||||
Logger::debug("添加当前有风险的可控车辆: ", risk.id1);
|
||||
Logger::debug("添加当前有风险的可控车辆: ", risk.id1,
|
||||
", 当前风险车辆数量: ", currentVehiclesWithRisk.size());
|
||||
processVehicle(risk.id1, risk.id2);
|
||||
}
|
||||
if (id2_controllable) {
|
||||
currentVehiclesWithRisk.insert(risk.id2);
|
||||
Logger::debug("添加当前有风险的可控车辆: ", risk.id2);
|
||||
Logger::debug("添加当前有风险的可控车辆: ", risk.id2,
|
||||
", 当前风险车辆数量: ", currentVehiclesWithRisk.size());
|
||||
processVehicle(risk.id2, risk.id1);
|
||||
}
|
||||
|
||||
@ -438,8 +620,22 @@ void System::processCollisions(const std::vector<CollisionRisk>& risks) {
|
||||
}
|
||||
}
|
||||
|
||||
// 更新上一次有风险的可控车辆列表
|
||||
Logger::debug("更新风险车辆列表: ", lastVehiclesWithRisk_.size(), " -> ", currentVehiclesWithRisk.size());
|
||||
// 更新上一次有风险的车辆列表
|
||||
Logger::debug("更新风险车辆列表: 原数量=", lastVehiclesWithRisk_.size(),
|
||||
", 新数量=", currentVehiclesWithRisk.size(),
|
||||
", 原列表={", [&]() {
|
||||
std::string s;
|
||||
for (const auto& id : lastVehiclesWithRisk_) {
|
||||
s += id + ",";
|
||||
}
|
||||
return s;
|
||||
}(), "}, 新列表={", [&]() {
|
||||
std::string s;
|
||||
for (const auto& id : currentVehiclesWithRisk) {
|
||||
s += id + ",";
|
||||
}
|
||||
return s;
|
||||
}(), "}");
|
||||
lastVehiclesWithRisk_ = std::move(currentVehiclesWithRisk);
|
||||
}
|
||||
|
||||
@ -520,6 +716,17 @@ std::string getRiskLevelString(RiskLevel level) {
|
||||
}
|
||||
}
|
||||
|
||||
std::string getRiskLevelString(SimpleRiskLevel level) {
|
||||
switch (level) {
|
||||
case SimpleRiskLevel::CRITICAL:
|
||||
return "CRITICAL";
|
||||
case SimpleRiskLevel::WARNING:
|
||||
return "WARNING";
|
||||
default:
|
||||
return "NONE";
|
||||
}
|
||||
}
|
||||
|
||||
void System::broadcastCollisionWarning(const CollisionRisk& risk) {
|
||||
if (!ws_server_) {
|
||||
return;
|
||||
@ -544,6 +751,30 @@ void System::broadcastCollisionWarning(const CollisionRisk& risk) {
|
||||
" level=", getRiskLevelString(risk.level));
|
||||
}
|
||||
|
||||
void System::broadcastCollisionWarning(const SimpleCollisionRisk& 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::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());
|
||||
|
||||
@ -8,12 +8,13 @@
|
||||
#include "types/BasicTypes.h"
|
||||
#include "detector/CollisionDetector.h"
|
||||
#include "detector/TrafficLightDetector.h"
|
||||
#include "spatial/AirportBounds.h"
|
||||
#include "config/AirportBounds.h"
|
||||
#include "vehicle/ControllableVehicles.h"
|
||||
#include "network/WebSocketServer.h"
|
||||
#include "network/MessageTypes.h"
|
||||
#include "config/SystemConfig.h"
|
||||
#include "config/IntersectionConfig.h"
|
||||
#include "detector/SimpleCollisionDetector.h"
|
||||
|
||||
// 前向声明
|
||||
class DataCollector;
|
||||
@ -34,6 +35,7 @@ public:
|
||||
virtual void broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning);
|
||||
void broadcastPositionUpdate(const MovingObject& obj);
|
||||
void broadcastCollisionWarning(const CollisionRisk& risk);
|
||||
void broadcastCollisionWarning(const SimpleCollisionRisk& risk);
|
||||
void broadcastVehicleCommand(const VehicleCommand& cmd);
|
||||
void broadcastTrafficLightStatus(const TrafficLightSignal& signal);
|
||||
|
||||
@ -43,6 +45,7 @@ public:
|
||||
private:
|
||||
void processLoop();
|
||||
void processCollisions(const std::vector<CollisionRisk>& collisions);
|
||||
void processCollisions(const std::vector<SimpleCollisionRisk>& risks);
|
||||
|
||||
std::atomic<bool> running_{false};
|
||||
std::thread processThread_;
|
||||
@ -70,4 +73,6 @@ private:
|
||||
|
||||
// 辅助函数
|
||||
const MovingObject* findVehicle(const std::string& vehicleId) const;
|
||||
|
||||
std::unique_ptr<SimpleCollisionDetector> simpleCollisionDetector_;
|
||||
};
|
||||
@ -11,11 +11,6 @@ CollisionDetector::CollisionDetector(const AirportBounds& bounds, const Controll
|
||||
|
||||
// 记录初始化信息
|
||||
const auto& airportBounds = bounds.getAirportBounds();
|
||||
Logger::debug(
|
||||
"碰撞检测器初始化: 机场边界=(",
|
||||
airportBounds.x, ",", airportBounds.y, ") - (",
|
||||
airportBounds.x + airportBounds.width, ",", airportBounds.y + airportBounds.height, ")"
|
||||
);
|
||||
}
|
||||
|
||||
void CollisionDetector::updateTraffic(const std::vector<Aircraft>& aircraft,
|
||||
@ -85,6 +80,8 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
|
||||
// 检测可控车辆与航空器的碰撞
|
||||
for (const auto& aircraft : aircraftData_) {
|
||||
for (const auto& vehicle : controlVehicles) {
|
||||
|
||||
Logger::debug("检查车辆与航空器碰撞: ", vehicle.vehicleNo, " 与 ", aircraft.flightNo);
|
||||
// 计算当前距离
|
||||
double dx = aircraft.position.x - vehicle.position.x;
|
||||
double dy = aircraft.position.y - vehicle.position.y;
|
||||
@ -93,8 +90,8 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
|
||||
// 计算预测阈值
|
||||
double maxSpeed = std::max(aircraft.speed, vehicle.speed);
|
||||
double predictionDistance = maxSpeed * predictionConfig.time_window;
|
||||
double baseThreshold = getCollisionParams(aircraft.position).aircraftGroundRadius;
|
||||
double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0);
|
||||
double baseThreshold = getCollisionParams(aircraft.position).alert_zone_radius;
|
||||
double predictionThreshold = std::max(predictionDistance, baseThreshold * 4.0);
|
||||
|
||||
// 如果在预测距离内，进行轨迹预测
|
||||
if (currentDistance <= predictionThreshold) {
|
||||
@ -152,7 +149,7 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
|
||||
}
|
||||
}
|
||||
|
||||
// 检测可控车辆之间的碰撞
|
||||
// 检测可控车辆与所有车辆的碰撞
|
||||
for (size_t i = 0; i < controlVehicles.size(); ++i) {
|
||||
const auto& vehicle1 = controlVehicles[i];
|
||||
|
||||
@ -174,8 +171,8 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
|
||||
// 计算预测阈值
|
||||
double maxSpeed = std::max(vehicle1.speed, vehicle2.speed);
|
||||
double predictionDistance = maxSpeed * predictionConfig.time_window;
|
||||
double baseThreshold = getCollisionParams(vehicle1.position).vehicleCollisionRadius;
|
||||
double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0);
|
||||
double baseThreshold = getCollisionParams(vehicle1.position).alert_zone_radius;
|
||||
double predictionThreshold = std::max(predictionDistance, baseThreshold * 4.0);
|
||||
|
||||
// 如果在预测距离内，进行轨迹预测
|
||||
if (currentDistance <= predictionThreshold) {
|
||||
@ -246,54 +243,6 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
|
||||
return risks;
|
||||
}
|
||||
|
||||
RiskLevel CollisionDetector::calculateRiskLevel(double distance, const Vector2D& position,
|
||||
bool isAircraft1, bool isAircraft2) const {
|
||||
const auto& areaConfig = getCollisionParams(position);
|
||||
|
||||
// 获取合适的阈值配置
|
||||
auto thresholds = areaConfig.getThresholds(isAircraft1, isAircraft2);
|
||||
double warningDistance = thresholds.warning;
|
||||
double alertDistance = thresholds.critical;
|
||||
|
||||
// 记录调试信息
|
||||
Logger::debug(
|
||||
"风险等级计算: 距离=", distance,
|
||||
"m, 预警阈值=", warningDistance,
|
||||
"m, 警报阈值=", alertDistance,
|
||||
"m, 物体类型=",
|
||||
isAircraft1 ? (isAircraft2 ? "航空器-航空器" : "航空器-车辆") : "车辆-车辆"
|
||||
);
|
||||
|
||||
// 根据距离判断风险等级
|
||||
if (distance <= alertDistance) {
|
||||
return RiskLevel::CRITICAL;
|
||||
} else if (distance <= warningDistance) {
|
||||
return RiskLevel::WARNING;
|
||||
}
|
||||
return RiskLevel::NONE;
|
||||
}
|
||||
|
||||
WarningZoneType CollisionDetector::determineWarningZone(double distance, double threshold) const {
|
||||
// 获取警报和预警距离
|
||||
double alertDistance = threshold * 0.5; // 警报距离为阈值的50%
|
||||
double warningDistance = threshold; // 预警距离为阈值的100%
|
||||
|
||||
// 记录调试信息
|
||||
Logger::debug(
|
||||
"预警区域判断: 当前距离=", distance,
|
||||
"m, 警报距离=", alertDistance,
|
||||
"m, 预警距离=", warningDistance, "m"
|
||||
);
|
||||
|
||||
// 根据距离判断区域类型
|
||||
if (distance <= alertDistance) {
|
||||
return WarningZoneType::DANGER;
|
||||
} else if (distance <= warningDistance) {
|
||||
return WarningZoneType::WARNING;
|
||||
}
|
||||
return WarningZoneType::NONE;
|
||||
}
|
||||
|
||||
// 添加一个新的辅助函数来统一处理风险等级和预警区域的判断
|
||||
std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
|
||||
double currentDistance,
|
||||
@ -309,13 +258,13 @@ std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
|
||||
RiskLevel level = RiskLevel::NONE;
|
||||
WarningZoneType zoneType = WarningZoneType::NONE;
|
||||
|
||||
// 如果预测到碰撞或距离小于警报距离，设置为危险等级
|
||||
if (willCollide || currentDistance <= thresholds.critical) {
|
||||
// 如果预测到碰撞并且当前距离小于警报距离，设置为危险等级
|
||||
if (willCollide && currentDistance <= thresholds.critical) {
|
||||
level = RiskLevel::CRITICAL;
|
||||
zoneType = WarningZoneType::DANGER;
|
||||
}
|
||||
// 如果距离在预警范围内，设置为预警等级
|
||||
else if (currentDistance <= thresholds.warning) {
|
||||
// 如果预测到碰撞并且当前距离在预警范围内，设置为预警等级
|
||||
else if (willCollide && currentDistance <= thresholds.warning) {
|
||||
level = RiskLevel::WARNING;
|
||||
zoneType = WarningZoneType::WARNING;
|
||||
}
|
||||
@ -352,7 +301,7 @@ bool CollisionDetector::checkCollisionRisk(
|
||||
double predictionDistance = maxSpeed * predictionConfig.time_window;
|
||||
// 根据物体类型选择基准阈值
|
||||
double baseThreshold = (isAircraft1 || isAircraft2) ?
|
||||
areaConfig.aircraftGroundRadius :
|
||||
areaConfig.aircraftCollisionRadius :
|
||||
areaConfig.vehicleCollisionRadius;
|
||||
double predictionThreshold = std::max(predictionDistance, baseThreshold * 2.0);
|
||||
|
||||
@ -389,12 +338,26 @@ bool CollisionDetector::checkCollisionRisk(
|
||||
// 计算相对速度
|
||||
double vx = mv1.vx - mv2.vx;
|
||||
double vy = mv1.vy - mv2.vy;
|
||||
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
|
||||
|
||||
// 计算相对运动（点积），判断是否在接近
|
||||
double relativeMotion = dx*vx + dy*vy;
|
||||
|
||||
// 如果相对运动小于等于0，表示物体正在接近
|
||||
hasCollisionRisk = (relativeMotion <= 0) || prediction.willCollide || currentDistance <= alertDistance;
|
||||
// 如果相对运动小于等于0，且相对速度大于阈值，或者预测会碰撞，或者已经进入警戒区域，则有碰撞风险
|
||||
hasCollisionRisk = (relativeMotion <= 0 && relativeSpeed > 1.0) ||
|
||||
prediction.willCollide ||
|
||||
currentDistance <= alertDistance;
|
||||
|
||||
Logger::debug(
|
||||
"碰撞风险分析: id1=", id1,
|
||||
", id2=", id2,
|
||||
", 当前距离=", currentDistance,
|
||||
"m, 相对速度=", relativeSpeed,
|
||||
"m/s, 相对运动=", relativeMotion,
|
||||
", 预测碰撞=", prediction.willCollide ? "是" : "否",
|
||||
", 警戒距离=", alertDistance,
|
||||
"m, 有风险=", hasCollisionRisk ? "是" : "否"
|
||||
);
|
||||
}
|
||||
|
||||
if (hasCollisionRisk) {
|
||||
@ -451,7 +414,7 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision(
|
||||
result.minDistance = std::numeric_limits<double>::infinity();
|
||||
|
||||
// 计算安全距离
|
||||
double safeDistance = size1 + size2;
|
||||
double safeDistance = (size1 + size2) * 1.5;
|
||||
|
||||
// 计算速度分量
|
||||
MovementVector mv1(speed1, heading1);
|
||||
@ -480,7 +443,7 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision(
|
||||
", 正在远离=", result.isMovingAway ? "是" : "否"
|
||||
);
|
||||
|
||||
// 如果物体静止相对于此，直接返回当前状态
|
||||
// 如果物体相对静止，直接返回当前状态
|
||||
if (relativeSpeed < 1e-6) {
|
||||
result.minDistance = currentDistance;
|
||||
result.willCollide = (currentDistance <= safeDistance);
|
||||
@ -494,6 +457,25 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision(
|
||||
return result;
|
||||
}
|
||||
|
||||
// 如果当前距离小于安全距离，且相对运动为负（正在接近），直接判定为碰撞
|
||||
if (currentDistance <= safeDistance && relativeMotion < 0) {
|
||||
result.willCollide = true;
|
||||
result.timeToCollision = 0.0;
|
||||
result.minDistance = currentDistance;
|
||||
result.collisionPoint = {
|
||||
pos2.x + dx * (size1 / (size1 + size2)),
|
||||
pos2.y + dy * (size1 / (size1 + size2))
|
||||
};
|
||||
|
||||
Logger::debug(
|
||||
"预测到碰撞: 当前距离=", currentDistance,
|
||||
"m < 安全距离=", safeDistance,
|
||||
"m, 且正在接近, 相对运动=", relativeMotion
|
||||
);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 计算相对运动方程的系数
|
||||
double a = dvx * dvx + dvy * dvy;
|
||||
double b = 2.0 * (dx * dvx + dy * dvy);
|
||||
@ -568,6 +550,9 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision(
|
||||
", 正在远离=", result.isMovingAway ? "是" : "否"
|
||||
);
|
||||
}
|
||||
else {
|
||||
Logger::debug("没有预测到碰撞: 时间=", t, "s, 距离=", dist_t, "m, 位置=(", x2_t, ",", y2_t, ")", ", 正在远离=", result.isMovingAway ? "是" : "否");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
|
||||
#include "types/BasicTypes.h"
|
||||
#include "spatial/QuadTree.h"
|
||||
#include "spatial/AirportBounds.h"
|
||||
#include "config/AirportBounds.h"
|
||||
#include "vehicle/ControllableVehicles.h"
|
||||
#include "config/SystemConfig.h"
|
||||
#include <vector>
|
||||
|
||||
202
src/detector/SimpleCollisionDetector.cpp
Normal file
202
src/detector/SimpleCollisionDetector.cpp
Normal file
@ -0,0 +1,202 @@
|
||||
#include "detector/SimpleCollisionDetector.h"
|
||||
#include "utils/Logger.h"
|
||||
#include <cmath>
|
||||
|
||||
SimpleCollisionDetector::SimpleCollisionDetector(
|
||||
const IntersectionConfig& intersectionConfig,
|
||||
const ControllableVehicles& controllableVehicles)
|
||||
: intersection_config_(intersectionConfig)
|
||||
, controllable_vehicles_(controllableVehicles) {
|
||||
Logger::info("简单冲突检测器初始化完成");
|
||||
}
|
||||
|
||||
void SimpleCollisionDetector::updateTraffic(
|
||||
const std::vector<Aircraft>& aircraft,
|
||||
const std::vector<Vehicle>& vehicles) {
|
||||
aircraft_data_ = aircraft;
|
||||
vehicle_data_ = vehicles;
|
||||
Logger::debug("更新交通数据: 航空器=", aircraft.size(), " 车辆=", vehicles.size());
|
||||
}
|
||||
|
||||
std::vector<SimpleCollisionRisk> SimpleCollisionDetector::detectCollisions() {
|
||||
std::vector<SimpleCollisionRisk> risks;
|
||||
|
||||
// 检查可控车辆与飞机的冲突
|
||||
for (const auto& aircraft : aircraft_data_) {
|
||||
for (const auto& vehicle : vehicle_data_) {
|
||||
if (controllable_vehicles_.isControllable(vehicle.vehicleNo)) {
|
||||
auto risk = checkAircraftVehicleCollision(aircraft, vehicle);
|
||||
if (risk.level != SimpleRiskLevel::NONE) {
|
||||
risks.push_back(risk);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 检查可控车辆之间的冲突
|
||||
for (size_t i = 0; i < vehicle_data_.size(); ++i) {
|
||||
if (!controllable_vehicles_.isControllable(vehicle_data_[i].vehicleNo)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
for (size_t j = i + 1; j < vehicle_data_.size(); ++j) {
|
||||
auto risk = checkVehicleCollision(vehicle_data_[i], vehicle_data_[j]);
|
||||
if (risk.level != SimpleRiskLevel::NONE) {
|
||||
risks.push_back(risk);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return risks;
|
||||
}
|
||||
|
||||
SimpleCollisionRisk SimpleCollisionDetector::checkAircraftVehicleCollision(
|
||||
const Aircraft& aircraft, const Vehicle& vehicle) {
|
||||
|
||||
SimpleCollisionRisk risk;
|
||||
risk.id1 = aircraft.flightNo;
|
||||
risk.id2 = vehicle.vehicleNo;
|
||||
risk.level = SimpleRiskLevel::NONE;
|
||||
|
||||
// 计算当前距离
|
||||
risk.distance = calculateDistance(aircraft.position, vehicle.position);
|
||||
|
||||
// 检查是否在路口附近
|
||||
if (!isNearIntersection(vehicle.position, risk.intersectionId)) {
|
||||
return risk; // 不在路口，无风险
|
||||
}
|
||||
|
||||
risk.isIntersection = true;
|
||||
|
||||
// 计算到路口的距离
|
||||
double aircraftDist = calculateDistanceToIntersection(aircraft.position, risk.intersectionId);
|
||||
double vehicleDist = calculateDistanceToIntersection(vehicle.position, risk.intersectionId);
|
||||
|
||||
// 计算相对速度（取较大值）
|
||||
risk.relativeSpeed = std::max(aircraft.speed, vehicle.speed);
|
||||
if (risk.relativeSpeed < MIN_SPEED) {
|
||||
risk.relativeSpeed = MIN_SPEED;
|
||||
}
|
||||
|
||||
// 计算预计碰撞时间（取较大值）
|
||||
risk.timeToCollision = std::max(aircraftDist, vehicleDist) / risk.relativeSpeed;
|
||||
|
||||
// 安全距离 = 路口宽度 + 飞机安全缓冲
|
||||
double safeDistance = INTERSECTION_WIDTH + AIRCRAFT_BUFFER;
|
||||
|
||||
// 判断风险等级
|
||||
if (risk.distance < safeDistance || risk.timeToCollision < MIN_TIME_WINDOW) {
|
||||
risk.level = SimpleRiskLevel::CRITICAL;
|
||||
} else if (risk.distance < safeDistance * 1.5 || risk.timeToCollision < WARNING_TIME_WINDOW) {
|
||||
risk.level = SimpleRiskLevel::WARNING;
|
||||
}
|
||||
|
||||
return risk;
|
||||
}
|
||||
|
||||
SimpleCollisionRisk SimpleCollisionDetector::checkVehicleCollision(
|
||||
const Vehicle& v1, const Vehicle& v2) {
|
||||
|
||||
SimpleCollisionRisk risk;
|
||||
risk.id1 = v1.vehicleNo;
|
||||
risk.id2 = v2.vehicleNo;
|
||||
risk.level = SimpleRiskLevel::NONE;
|
||||
|
||||
// 计算当前距离
|
||||
risk.distance = calculateDistance(v1.position, v2.position);
|
||||
|
||||
// 检查是否在路口附近
|
||||
if (isNearIntersection(v1.position, risk.intersectionId) ||
|
||||
isNearIntersection(v2.position, risk.intersectionId)) {
|
||||
// 路口场景
|
||||
risk.isIntersection = true;
|
||||
|
||||
double v1Dist = calculateDistanceToIntersection(v1.position, risk.intersectionId);
|
||||
double v2Dist = calculateDistanceToIntersection(v2.position, risk.intersectionId);
|
||||
|
||||
risk.relativeSpeed = std::max(v1.speed, v2.speed);
|
||||
if (risk.relativeSpeed < MIN_SPEED) {
|
||||
risk.relativeSpeed = MIN_SPEED;
|
||||
}
|
||||
|
||||
risk.timeToCollision = std::max(v1Dist, v2Dist) / risk.relativeSpeed;
|
||||
|
||||
// 路口安全距离
|
||||
double safeDistance = INTERSECTION_WIDTH + VEHICLE_BUFFER;
|
||||
|
||||
if (risk.distance < safeDistance || risk.timeToCollision < MIN_TIME_WINDOW) {
|
||||
risk.level = SimpleRiskLevel::CRITICAL;
|
||||
} else if (risk.distance < safeDistance * 1.5 || risk.timeToCollision < WARNING_TIME_WINDOW) {
|
||||
risk.level = SimpleRiskLevel::WARNING;
|
||||
}
|
||||
} else {
|
||||
// 非路口场景（对向或同向）
|
||||
risk.isIntersection = false;
|
||||
|
||||
// 计算相对速度
|
||||
risk.relativeSpeed = std::abs(v1.speed - v2.speed); // 同向
|
||||
if (std::abs(v1.heading - v2.heading) > 150) { // 对向
|
||||
risk.relativeSpeed = v1.speed + v2.speed;
|
||||
}
|
||||
|
||||
if (risk.relativeSpeed < MIN_SPEED) {
|
||||
risk.relativeSpeed = MIN_SPEED;
|
||||
}
|
||||
|
||||
risk.timeToCollision = risk.distance / risk.relativeSpeed;
|
||||
|
||||
// 非路口安全距离
|
||||
double safeDistance = VEHICLE_BUFFER * 2;
|
||||
|
||||
if (risk.distance < safeDistance || risk.timeToCollision < MIN_TIME_WINDOW) {
|
||||
risk.level = SimpleRiskLevel::CRITICAL;
|
||||
} else if (risk.distance < safeDistance * 1.5 || risk.timeToCollision < WARNING_TIME_WINDOW) {
|
||||
risk.level = SimpleRiskLevel::WARNING;
|
||||
}
|
||||
}
|
||||
|
||||
return risk;
|
||||
}
|
||||
|
||||
double SimpleCollisionDetector::calculateDistance(const Vector2D& pos1, const Vector2D& pos2) {
|
||||
double dx = pos1.x - pos2.x;
|
||||
double dy = pos1.y - pos2.y;
|
||||
return std::sqrt(dx*dx + dy*dy);
|
||||
}
|
||||
|
||||
double SimpleCollisionDetector::calculateDistanceToIntersection(
|
||||
const Vector2D& position, const std::string& intersectionId) {
|
||||
|
||||
const auto* intersection = intersection_config_.findById(intersectionId);
|
||||
if (!intersection) {
|
||||
return std::numeric_limits<double>::max();
|
||||
}
|
||||
|
||||
// 将 IntersectionPosition 转换为 Vector2D
|
||||
Vector2D intersectionPos;
|
||||
intersectionPos.x = intersection->position.longitude;
|
||||
intersectionPos.y = intersection->position.latitude;
|
||||
|
||||
return calculateDistance(position, intersectionPos);
|
||||
}
|
||||
|
||||
bool SimpleCollisionDetector::isNearIntersection(
|
||||
const Vector2D& position, std::string& intersectionId) {
|
||||
|
||||
const double CHECK_DISTANCE = INTERSECTION_WIDTH * 5; // 检查范围是路口宽度的2倍
|
||||
|
||||
for (const auto& intersection : intersection_config_.getIntersections()) {
|
||||
// 将 IntersectionPosition 转换为 Vector2D
|
||||
Vector2D intersectionPos;
|
||||
intersectionPos.x = intersection.position.longitude;
|
||||
intersectionPos.y = intersection.position.latitude;
|
||||
|
||||
double distance = calculateDistance(position, intersectionPos);
|
||||
if (distance < CHECK_DISTANCE) {
|
||||
intersectionId = intersection.id;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
72
src/detector/SimpleCollisionDetector.h
Normal file
72
src/detector/SimpleCollisionDetector.h
Normal file
@ -0,0 +1,72 @@
|
||||
#ifndef AIRPORT_DETECTOR_SIMPLE_COLLISION_DETECTOR_H
|
||||
#define AIRPORT_DETECTOR_SIMPLE_COLLISION_DETECTOR_H
|
||||
|
||||
#include "types/BasicTypes.h"
|
||||
#include "config/IntersectionConfig.h"
|
||||
#include "vehicle/ControllableVehicles.h"
|
||||
#include <vector>
|
||||
|
||||
// 简单冲突检测器的风险等级
|
||||
enum class SimpleRiskLevel {
|
||||
NONE = 0, // 无风险
|
||||
WARNING = 1, // 预警
|
||||
CRITICAL = 2 // 告警
|
||||
};
|
||||
|
||||
// 简单冲突检测器的风险信息
|
||||
struct SimpleCollisionRisk {
|
||||
std::string id1, id2; // 冲突物体的ID
|
||||
SimpleRiskLevel level; // 风险等级
|
||||
double distance; // 当前距离
|
||||
double timeToCollision; // 预计碰撞时间
|
||||
double relativeSpeed; // 相对速度
|
||||
bool isIntersection; // 是否在路口
|
||||
std::string intersectionId; // 路口ID（如果在路口）
|
||||
};
|
||||
|
||||
class SimpleCollisionDetector {
|
||||
public:
|
||||
SimpleCollisionDetector(const IntersectionConfig& intersectionConfig,
|
||||
const ControllableVehicles& controllableVehicles);
|
||||
|
||||
// 更新交通数据
|
||||
void updateTraffic(const std::vector<Aircraft>& aircraft,
|
||||
const std::vector<Vehicle>& vehicles);
|
||||
|
||||
// 检测所有可能的冲突
|
||||
std::vector<SimpleCollisionRisk> detectCollisions();
|
||||
|
||||
private:
|
||||
// 检查车辆与飞机的冲突
|
||||
SimpleCollisionRisk checkAircraftVehicleCollision(
|
||||
const Aircraft& aircraft, const Vehicle& vehicle);
|
||||
|
||||
// 检查车辆与车辆的冲突
|
||||
SimpleCollisionRisk checkVehicleCollision(
|
||||
const Vehicle& v1, const Vehicle& v2);
|
||||
|
||||
// 计算到路口的距离
|
||||
double calculateDistanceToIntersection(
|
||||
const Vector2D& position, const std::string& intersectionId);
|
||||
|
||||
// 计算两点间距离
|
||||
double calculateDistance(const Vector2D& pos1, const Vector2D& pos2);
|
||||
|
||||
// 判断是否在路口附近
|
||||
bool isNearIntersection(const Vector2D& position, std::string& intersectionId);
|
||||
|
||||
// 配置参数
|
||||
static constexpr double INTERSECTION_WIDTH = 30.0; // 路口宽度
|
||||
static constexpr double AIRCRAFT_BUFFER = 20.0; // 飞机安全缓冲
|
||||
static constexpr double VEHICLE_BUFFER = 10.0; // 车辆安全缓冲
|
||||
static constexpr double MIN_TIME_WINDOW = 3.0; // 最小时间窗口（秒）
|
||||
static constexpr double WARNING_TIME_WINDOW = 5.0; // 预警时间窗口（秒）
|
||||
static constexpr double MIN_SPEED = 0.5; // 最小速度（米/秒）
|
||||
|
||||
const IntersectionConfig& intersection_config_;
|
||||
const ControllableVehicles& controllable_vehicles_;
|
||||
std::vector<Aircraft> aircraft_data_;
|
||||
std::vector<Vehicle> vehicle_data_;
|
||||
};
|
||||
|
||||
#endif // AIRPORT_DETECTOR_SIMPLE_COLLISION_DETECTOR_H
|
||||
@ -286,17 +286,12 @@ bool HTTPDataSource::parseVehicleResponse(const std::string& response, std::vect
|
||||
}
|
||||
veh.timestamp = static_cast<uint64_t>(time);
|
||||
|
||||
// 解析可选字段
|
||||
if (item.contains("direction")) {
|
||||
veh.heading = item["direction"].get<double>();
|
||||
}
|
||||
|
||||
// 更新位置信息
|
||||
veh.position = coordinateConverter_.toLocalXY(veh.geo.latitude, veh.geo.longitude);
|
||||
veh.updateMotion(veh.geo, veh.timestamp); // 更新速度和航向
|
||||
|
||||
Logger::debug("Parsed vehicle: id=", veh.id, " vehicleNo=", veh.vehicleNo,
|
||||
" timestamp=", veh.timestamp, " pos=(", veh.geo.longitude, ",", veh.geo.latitude, ")");
|
||||
// Logger::debug("Parsed vehicle: id=", veh.id, " vehicleNo=", veh.vehicleNo,
|
||||
// " timestamp=", veh.timestamp, " pos=(", veh.geo.longitude, ",", veh.geo.latitude, ")");
|
||||
|
||||
vehicles.push_back(veh);
|
||||
}
|
||||
@ -348,11 +343,6 @@ bool HTTPDataSource::parseAircraftResponse(const std::string& response, std::vec
|
||||
}
|
||||
ac.timestamp = static_cast<uint64_t>(time);
|
||||
|
||||
// 解析可选字段
|
||||
if (item.contains("direction")) {
|
||||
ac.heading = item["direction"].get<double>();
|
||||
}
|
||||
|
||||
// 更新位置信息
|
||||
ac.position = coordinateConverter_.toLocalXY(ac.geo.latitude, ac.geo.longitude);
|
||||
ac.updateMotion(ac.geo, ac.timestamp); // 更新速度和航向
|
||||
|
||||
@ -38,18 +38,21 @@ double MovingObject::calculateDistance(const GeoPosition& pos1, const GeoPositio
|
||||
}
|
||||
|
||||
double MovingObject::calculateHeading(const GeoPosition& from, const GeoPosition& to) {
|
||||
const double METERS_PER_DEGREE = 111000.0;
|
||||
double lat_rad = from.latitude * M_PI / 180.0;
|
||||
// 计算经纬度差值
|
||||
double dlon = to.longitude - from.longitude;
|
||||
double dlat = to.latitude - from.latitude;
|
||||
|
||||
double dx = dlon * METERS_PER_DEGREE * std::cos(lat_rad);
|
||||
double dy = dlat * METERS_PER_DEGREE;
|
||||
// 将经度差转换为实际距离（考虑纬度影响）
|
||||
double lat_rad = from.latitude * M_PI / 180.0;
|
||||
double dx = dlon * std::cos(lat_rad); // 东西方向的距离（经度）
|
||||
double dy = dlat; // 南北方向的距离（纬度）
|
||||
|
||||
// 计算航向角（正北为0度，顺时针增加）
|
||||
double angle = std::atan2(dx, dy) * 180.0 / M_PI;
|
||||
if (angle < 0) {
|
||||
angle += 360.0;
|
||||
}
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
@ -82,8 +85,6 @@ bool Vehicle::isValidSpeed(double speed) const {
|
||||
void MovingObject::updateMotion(const GeoPosition& newPos, uint64_t newTime) {
|
||||
// 检查时间戳
|
||||
if (!positionHistory.empty() && newTime <= positionHistory.back().timestamp) {
|
||||
Logger::debug("[Motion] Ignore outdated data: current=", newTime,
|
||||
" last=", positionHistory.back().timestamp);
|
||||
return; // 忽略重复或过时的数据
|
||||
}
|
||||
|
||||
@ -97,16 +98,17 @@ void MovingObject::updateMotion(const GeoPosition& newPos, uint64_t newTime) {
|
||||
if (positionHistory.size() >= 2) {
|
||||
// 使用最近的两个点来计算速度和航向
|
||||
const auto& curr = positionHistory.back();
|
||||
const auto& prev = positionHistory[positionHistory.size() - 2]; // 使用倒数第二个点
|
||||
const auto& prev = positionHistory[positionHistory.size() - 2];
|
||||
|
||||
// 计算距离和时间差
|
||||
double distance = calculateDistance(prev.geo, curr.geo); // 单位：米
|
||||
double timeDiff = static_cast<double>(curr.timestamp - prev.timestamp) / 1000.0; // 转换为秒
|
||||
|
||||
Logger::debug("[Motion] Position update: ",
|
||||
"\n Current: lat=", curr.geo.latitude, ", lon=", curr.geo.longitude,
|
||||
"\n Previous: lat=", prev.geo.latitude, ", lon=", prev.geo.longitude,
|
||||
"\n Distance=", distance, "m, TimeDiff=", timeDiff, "s");
|
||||
// 只有当位置变化足够大且时间差足够长时才更新速度和航向
|
||||
// Logger::debug("[Motion] Position update: ",
|
||||
// "\n Current: lat=", curr.geo.latitude, ", lon=", curr.geo.longitude,
|
||||
// "\n Previous: lat=", prev.geo.latitude, ", lon=", prev.geo.longitude,
|
||||
// "\n Distance=", distance, "m, TimeDiff=", timeDiff, "s");
|
||||
|
||||
// 只有当位置变化足够大且时间差足够长时才更新速度和航向
|
||||
static const double MIN_DISTANCE = 0.1; // 最小位置变化阈值（米）
|
||||
@ -124,36 +126,13 @@ void MovingObject::updateMotion(const GeoPosition& newPos, uint64_t newTime) {
|
||||
} else {
|
||||
speed = speed * (1 - alpha) + newSpeed * alpha; // 平滑更新
|
||||
}
|
||||
|
||||
Logger::debug("[Motion] Speed updated: distance=", distance, "m, timeDiff=", timeDiff,
|
||||
"s, newSpeed=", newSpeed, "m/s, smoothedSpeed=", speed, "m/s");
|
||||
} else {
|
||||
Logger::debug("[Motion] Invalid speed: ", newSpeed, "m/s (exceeds limit)");
|
||||
}
|
||||
|
||||
// 计算新的航向
|
||||
double newHeading = calculateHeading(prev.geo, curr.geo);
|
||||
|
||||
// 使用指数移动平均来平滑航向，增大平滑因子以加快更新
|
||||
const double beta = 0.8; // 增大航向平滑因子
|
||||
if (heading == 0) {
|
||||
heading = newHeading; // 第一次计算
|
||||
} else {
|
||||
// 处理航向角的循环性（0-360度）
|
||||
double diff = newHeading - heading;
|
||||
if (diff > 180) {
|
||||
diff -= 360;
|
||||
} else if (diff < -180) {
|
||||
diff += 360;
|
||||
}
|
||||
heading = fmod(heading + diff * beta + 360, 360);
|
||||
}
|
||||
|
||||
Logger::debug("[Motion] Heading updated: newHeading=", newHeading,
|
||||
"deg, smoothedHeading=", heading, "deg");
|
||||
// 计算并直接更新航向，不需要平滑处理
|
||||
heading = calculateHeading(prev.geo, curr.geo);
|
||||
} else {
|
||||
Logger::debug("[Motion] No update: distance=", distance, "m < ", MIN_DISTANCE,
|
||||
"m or timeDiff=", timeDiff, "s < ", MIN_TIME, "s");
|
||||
// Logger::debug("[Motion] No update: distance=", distance, "m < ", MIN_DISTANCE,
|
||||
// "m or timeDiff=", timeDiff, "s < ", MIN_TIME, "s");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#include "detector/CollisionDetector.h"
|
||||
#include "vehicle/ControllableVehicles.h"
|
||||
#include "spatial/AirportBounds.h"
|
||||
#include "config/AirportBounds.h"
|
||||
#include <gtest/gtest.h>
|
||||
#include <gmock/gmock.h>
|
||||
#include "utils/Logger.h"
|
||||
|
||||
@ -62,7 +62,7 @@ NORMAL_BRAKE_DECELERATION = 0.2 # 正常制动减速度 (每次更新减速 20%
|
||||
vehicle_data = [
|
||||
{
|
||||
"vehicleNo": "QN001", # 无人车1（西路口）
|
||||
"latitude": WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9), # 西路口北50米
|
||||
"latitude": WEST_INTERSECTION["latitude"] + (DIST_100M / 111319.9), # 西路口北50米
|
||||
"longitude": WEST_INTERSECTION["longitude"],
|
||||
"time": int(time.time() * 1000),
|
||||
"direction": -1, # -1表示向南
|
||||
@ -101,11 +101,11 @@ class VehicleState:
|
||||
self.command_reason = None # 指令原因
|
||||
self.last_command_time = time.time() # 最后一次指令时间
|
||||
self.traffic_light_state = None # 当前红绿灯状态
|
||||
self.target_lat = None # 目标纬度
|
||||
self.target_lon = None # 目标经度
|
||||
|
||||
def can_be_overridden_by(self, command_type):
|
||||
"""
|
||||
判断当前指令是否可以被新指令覆盖
|
||||
"""
|
||||
"""判断当前指令是否可以被新指令覆盖"""
|
||||
priority_map = {
|
||||
"ALERT": 5, # 告警指令，最高优先级
|
||||
"RED": 4, # 红灯指令，次高优先级
|
||||
@ -116,7 +116,7 @@ class VehicleState:
|
||||
new_priority = priority_map.get(command_type, 0)
|
||||
current_priority = priority_map.get(self.current_command, 0)
|
||||
|
||||
# ALERT 指令只能被 RESUME 解除
|
||||
# ALERT 指令可以被 RESUME 解除
|
||||
if self.current_command == "ALERT":
|
||||
return command_type == "RESUME"
|
||||
|
||||
@ -135,7 +135,7 @@ class VehicleState:
|
||||
# 其他情况按优先级判断
|
||||
return new_priority >= current_priority
|
||||
|
||||
def update_command(self, command_type):
|
||||
def update_command(self, command_type, target_lat=None, target_lon=None):
|
||||
"""更新指令状态"""
|
||||
priority_map = {
|
||||
"ALERT": 5, # 告警指令，最高优先级
|
||||
@ -145,6 +145,11 @@ class VehicleState:
|
||||
"RESUME": 1 # 恢复指令，最低优先级
|
||||
}
|
||||
|
||||
# 更新目标位置
|
||||
if target_lat is not None and target_lon is not None:
|
||||
self.target_lat = target_lat
|
||||
self.target_lon = target_lon
|
||||
|
||||
# 如果是红绿灯状态，只更新状态不改变当前指令
|
||||
if command_type in ["RED", "GREEN"]:
|
||||
self.traffic_light_state = command_type
|
||||
@ -167,12 +172,14 @@ class VehicleState:
|
||||
|
||||
def can_move(self):
|
||||
"""检查车辆是否可以移动"""
|
||||
# 如果有阻塞性指令，不能移动
|
||||
if self.current_command in ["ALERT", "WARNING", "RED"]:
|
||||
return False
|
||||
# 如果是红灯，不能移动
|
||||
if self.traffic_light_state == "RED":
|
||||
return False
|
||||
|
||||
# 如果有告警或预警指令，不能移动
|
||||
if self.current_command in ["ALERT", "WARNING"]:
|
||||
return False
|
||||
|
||||
# 其他情况可以移动
|
||||
return True
|
||||
|
||||
@ -188,6 +195,7 @@ class VehicleState:
|
||||
- 目标速度: {self.target_speed}
|
||||
- 制动模式: {self.brake_mode}
|
||||
- 指令原因: {self.command_reason}
|
||||
- 目标位置: ({self.target_lat}, {self.target_lon})
|
||||
""")
|
||||
|
||||
# 添加车辆状态管理
|
||||
@ -216,6 +224,8 @@ def handle_vehicle_command():
|
||||
command_type = data.get("type", "").upper()
|
||||
reason = data.get("reason", "").upper()
|
||||
signal_state = data.get("signalState", "").upper()
|
||||
target_lat = data.get("latitude", None)
|
||||
target_lon = data.get("longitude", None)
|
||||
|
||||
print(f"收到车辆控制指令: vehicle_id={vehicle_id}, type={command_type}, reason={reason}, signal_state={signal_state}")
|
||||
print(f"完整请求数据: {data}")
|
||||
@ -260,27 +270,33 @@ def handle_vehicle_command():
|
||||
f"new_command={command_type}, can_override={can_override}")
|
||||
|
||||
if not can_override:
|
||||
print(f"指令优先级过低，但仍继续处理: vehicle={vehicle_id}, current_priority={vehicle_state.command_priority}, "
|
||||
print(f"指令优先级过低: vehicle={vehicle_id}, current_priority={vehicle_state.command_priority}, "
|
||||
f"command={command_type}")
|
||||
return jsonify({
|
||||
"status": "error",
|
||||
"message": "Command priority too low"
|
||||
}), 400
|
||||
|
||||
# 处理不同类型的指令
|
||||
if command_type == "ALERT":
|
||||
print(f"执行告警指令: vehicle_id={vehicle_id}")
|
||||
vehicle_state.is_running = False
|
||||
vehicle_state.target_speed = 0
|
||||
vehicle_state.brake_mode = "emergency" # 告警紧急制动
|
||||
# 立即更新车辆速度
|
||||
if command_type in ["ALERT", "WARNING"]:
|
||||
# 查找当前车辆
|
||||
current_vehicle = None
|
||||
for v in vehicle_data:
|
||||
if v["vehicleNo"] == vehicle_id:
|
||||
v["speed"] = 0
|
||||
print(f"车辆 {vehicle_id} 速度已设置为0")
|
||||
current_vehicle = v
|
||||
break
|
||||
|
||||
elif command_type == "WARNING":
|
||||
print(f"执行预警指令: vehicle_id={vehicle_id}")
|
||||
|
||||
# 执行告警指令，直接停车
|
||||
print(f"执行{'紧急' if command_type == 'ALERT' else '正常'}制动: vehicle={vehicle_id}")
|
||||
vehicle_state.current_command = command_type
|
||||
vehicle_state.command_priority = COMMAND_PRIORITIES.get(command_type, 0)
|
||||
vehicle_state.is_running = False
|
||||
vehicle_state.target_speed = 0
|
||||
vehicle_state.brake_mode = "normal" # 预警正常制动
|
||||
vehicle_state.brake_mode = "emergency" if command_type == "ALERT" else "normal"
|
||||
vehicle_state.target_lat = target_lat
|
||||
vehicle_state.target_lon = target_lon
|
||||
# 立即更新车辆速度
|
||||
current_vehicle["speed"] = 0
|
||||
|
||||
elif command_type in ["RED", "GREEN"]:
|
||||
print(f"执行红绿灯指令: vehicle_id={vehicle_id}, state={command_type}")
|
||||
@ -289,21 +305,26 @@ def handle_vehicle_command():
|
||||
|
||||
elif command_type == "RESUME":
|
||||
print(f"执行恢复指令: vehicle_id={vehicle_id}")
|
||||
# RESUME 指令只能解除 ALERT 和 WARNING
|
||||
if vehicle_state.current_command not in ["ALERT", "WARNING", None]:
|
||||
print(f"车辆 {vehicle_id} 当前指令({vehicle_state.current_command})不能被 RESUME 解除")
|
||||
return jsonify({
|
||||
"status": "error",
|
||||
"message": "Current command cannot be resumed"
|
||||
}), 400
|
||||
# 检查当前车辆
|
||||
current_vehicle = None
|
||||
for v in vehicle_data:
|
||||
if v["vehicleNo"] == vehicle_id:
|
||||
current_vehicle = v
|
||||
break
|
||||
|
||||
# 恢复正常行驶
|
||||
vehicle_state.is_running = True
|
||||
vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED
|
||||
vehicle_state.brake_mode = None
|
||||
# 清除限制性指令
|
||||
if vehicle_state.current_command in ["ALERT", "WARNING"]:
|
||||
print(f"清除限制性指令: vehicle={vehicle_id}")
|
||||
vehicle_state.current_command = None
|
||||
vehicle_state.command_priority = 0
|
||||
vehicle_state.is_running = True
|
||||
vehicle_state.target_speed = DEFAULT_VEHICLE_SPEED
|
||||
vehicle_state.brake_mode = None
|
||||
vehicle_state.target_lat = None
|
||||
vehicle_state.target_lon = None
|
||||
|
||||
# 更新车辆状态
|
||||
vehicle_state.update_command(command_type)
|
||||
vehicle_state.update_command(command_type, target_lat, target_lon)
|
||||
vehicle_state.command_reason = reason
|
||||
vehicle_state.last_command_time = time.time()
|
||||
|
||||
@ -447,7 +468,7 @@ def update_vehicle_position(vehicle, elapsed_time):
|
||||
vehicle["phase"] = 1
|
||||
vehicle["direction"] = 1 # 向东
|
||||
print(f"QN001 到达西路口，切换到东西移动，方向=东")
|
||||
elif vehicle["direction"] == 1 and new_lat >= WEST_INTERSECTION["latitude"] + (DIST_100M / 111319.9):
|
||||
elif vehicle["direction"] == 1 and new_lat >= WEST_INTERSECTION["latitude"] + (DIST_50M / 111319.9):
|
||||
# 返回起点
|
||||
vehicle["phase"] = 0
|
||||
vehicle["direction"] = -1 # 向南
|
||||
@ -472,7 +493,7 @@ def update_vehicle_position(vehicle, elapsed_time):
|
||||
else: # QN002
|
||||
# 无人车2：西向往返（西路口）
|
||||
new_lon = vehicle["longitude"] + (dlon * vehicle["direction"])
|
||||
if new_lon >= EAST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))):
|
||||
if new_lon >= EAST_INTERSECTION["longitude"] + (DIST_150M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))):
|
||||
vehicle["direction"] = -1 # 向西
|
||||
print(f"QN002 到达东端，切换方向=西")
|
||||
elif new_lon <= EAST_INTERSECTION["longitude"] - (DIST_150M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))):
|
||||
@ -489,7 +510,7 @@ def update_vehicle_position(vehicle, elapsed_time):
|
||||
vehicle["phase"] = 1 # 切换到南北移动
|
||||
vehicle["direction"] = -1 # 向南
|
||||
print(f"TQ001 到达西路口，切换到南北移动，方向=南")
|
||||
elif vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_100M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))):
|
||||
elif vehicle["direction"] == 1 and new_lon >= WEST_INTERSECTION["longitude"] + (DIST_50M / (111319.9 * math.cos(math.radians(vehicle["latitude"])))):
|
||||
vehicle["direction"] = -1 # 向西
|
||||
print(f"TQ001 到达东端，切换方向=西")
|
||||
vehicle["longitude"] = new_lon
|
||||
@ -501,9 +522,9 @@ def update_vehicle_position(vehicle, elapsed_time):
|
||||
print(f"TQ001 到达南端，切换方向=北")
|
||||
elif new_lat >= WEST_INTERSECTION["latitude"]: # 返回路口
|
||||
vehicle["phase"] = 0 # 切换回东西移动
|
||||
vehicle["direction"] = 1 # 向北
|
||||
vehicle["direction"] = 1 # 向东
|
||||
vehicle["longitude"] = WEST_INTERSECTION["longitude"] # 重置到起始位置
|
||||
print(f"TQ001 返回西路口，切换到东西移动，方向=北")
|
||||
print(f"TQ001 返回西路口，切换到东西移动，方向=东")
|
||||
vehicle["latitude"] = new_lat
|
||||
print(f"TQ001 南北移动: lat={new_lat}, direction={vehicle['direction']}")
|
||||
|
||||
@ -520,9 +541,9 @@ def update_aircraft_position(aircraft, elapsed_time):
|
||||
dlat, dlon = meters_to_degrees(distance, aircraft["latitude"])
|
||||
|
||||
new_lat = aircraft["latitude"] + (dlat * aircraft["direction"])
|
||||
if new_lat >= EAST_INTERSECTION["latitude"] + (DIST_200M / 111319.9):
|
||||
if new_lat >= EAST_INTERSECTION["latitude"] + (DIST_150M / 111319.9):
|
||||
aircraft["direction"] = -1 # 向南
|
||||
elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_200M / 111319.9):
|
||||
elif new_lat <= EAST_INTERSECTION["latitude"] - (DIST_150M / 111319.9):
|
||||
aircraft["direction"] = 1 # 向北
|
||||
aircraft["latitude"] = new_lat
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user