tian/CollisionAvoidance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加安全区冲突检测

Browse Source
This commit is contained in:
Tian jianyong 2024-12-18 02:58:44 +08:00
parent 6ce6e36d07
commit d6a6377ab9
22 changed files with 1258 additions and 317 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -49,6 +49,7 @@ set(LIB_SOURCES
src/collector/DataCollector.cpp
src/core/System.cpp
src/detector/CollisionDetector.cpp
src/detector/SafetyZone.cpp
src/network/HTTPDataSource.cpp
src/network/WebSocketServer.cpp
src/network/HTTPClient.cpp
@ -100,6 +101,7 @@ if(EXISTS "${CMAKE_SOURCE_DIR}/tests")
tests/BasicTypesTest.cpp
tests/HTTPDataSourceTest.cpp
tests/DataCollectorTest.cpp
tests/SafetyZoneTest.cpp
)
# 创建测试可执行文件

18
config/airport_bounds.json
View File

@ -121,20 +121,20 @@
},
"config": {
"collision_radius": {
"aircraft": 50.0,
"special": 25.0,
"unmanned": 25.0
"aircraft": 30.0,
"special": 15.0,
"unmanned": 15.0
},
"height_threshold": 10.0,
"warning_zone_radius": {
"aircraft": 100.0,
"special": 50.0,
"unmanned": 50.0
"aircraft": 70.0,
"special": 30.0,
"unmanned": 30.0
},
"alert_zone_radius": {
"aircraft": 50.0,
"special": 25.0,
"unmanned": 25.0
"aircraft": 35.0,
"special": 15.0,
"unmanned": 15.0
}
}
}

14
config/intersections.json
View File

@ -9,8 +9,11 @@
"latitude": 36.35448347,
"altitude": 9.543
},
"radius": 100.0,
"stopDistance": 50.0
"width": 20.0,
"safetyZone": {
"aircraftRadius": 50.0,
"vehicleRadius": 50.0
}
},
{
"id": "T6路口",
@ -21,8 +24,11 @@
"latitude": 36.35074527,
"altitude": 9.778
},
"radius": 100.0,
"stopDistance": 50.0
"width": 30.0,
"safetyZone": {
"aircraftRadius": 50.0,
"vehicleRadius": 50.0
}
}
]
}

8
config/vehicle_control.json
View File

@ -2,11 +2,19 @@
"vehicles": [
{
"vehicleNo": "QN001",
"type": "UNMANNED",
"ip": "localhost",
"port": 8081
},
{
"vehicleNo": "QN002",
"type": "UNMANNED",
"ip": "localhost",
"port": 8081
},
{
"vehicleNo": "TQ001",
"type": "SPECIAL",
"ip": "localhost",
"port": 8081
}

20
src/config/IntersectionConfig.cpp
View File

@ -33,18 +33,24 @@ IntersectionConfig IntersectionConfig::load(const std::string& configFile) {
info.position.latitude = item["position"]["latitude"].get<double>();
info.position.altitude = item["position"]["altitude"].get<double>();
// 加载距离阈值，检查是否为 null
if (item["radius"].is_null() || item["stopDistance"].is_null()) {
throw std::runtime_error("Distance values cannot be null for intersection: " + info.id);
// 加载路口宽度和安全区配置，检查是否为 null
if (item["width"].is_null() ||
item["safetyZone"]["aircraftRadius"].is_null() ||
item["safetyZone"]["vehicleRadius"].is_null()) {
throw std::runtime_error("Width or safety zone values cannot be null for intersection: " + info.id);
}
info.radius = item["radius"].get<double>();
info.stopDistance = item["stopDistance"].get<double>();
info.width = item["width"].get<double>();
info.safetyZone.aircraftRadius = item["safetyZone"]["aircraftRadius"].get<double>();
info.safetyZone.vehicleRadius = item["safetyZone"]["vehicleRadius"].get<double>();
config.intersections_.push_back(info);
Logger::debug("Loaded intersection: id=", info.id,
" name=", info.name,
" trafficLightId=", info.trafficLightId);
", name=", info.name,
", trafficLightId=", info.trafficLightId,
", width=", info.width,
", aircraftRadius=", info.safetyZone.aircraftRadius,
", vehicleRadius=", info.safetyZone.vehicleRadius);
}
Logger::info("Loaded ", config.intersections_.size(), " intersections");

10
src/config/IntersectionConfig.h
View File

@ -10,13 +10,19 @@ struct IntersectionPosition {
double altitude;
};
// 路口安全区配置
struct SafetyZoneConfig {
double aircraftRadius; // 航空器安全区半径
double vehicleRadius; // 特勤车安全区半径
};
struct Intersection {
std::string id;
std::string name;
std::string trafficLightId;
IntersectionPosition position;
double radius; // 路口影响半径，单位：米
double stopDistance; // 建议停车距离，单位：米
double width; // 路口宽度，单位：米
SafetyZoneConfig safetyZone; // 安全区配置
};
class IntersectionConfig {

367
src/core/System.cpp
View File

@ -5,6 +5,7 @@
#include "System.h"
#include "utils/Logger.h"
#include "collector/DataCollector.h"
#include "spatial/CoordinateConverter.h"
System* System::instance_ = nullptr;
@ -74,6 +75,9 @@ bool System::initialize() {
system_config.warning.log_interval_ms
};
// 初始化安全区
initializeSafetyZones();
return dataCollector_->initialize(dataSourceConfig, warnConfig);
}
catch (const std::exception& e) {
@ -82,6 +86,45 @@ bool System::initialize() {
}
}
void System::initializeSafetyZones() {
// 清空现有的安全区
safetyZones_.clear();
// 获取所有路口配置
const auto& intersections = intersection_config_.getIntersections();
// 创建坐标转换器
CoordinateConverter converter;
converter.setReferencePoint(
SystemConfig::instance().airport.reference_point.latitude,
SystemConfig::instance().airport.reference_point.longitude
);
// 为每个路口创建安全区
for (const auto& intersection : intersections) {
// 获取路口中心点的地理坐标
Vector2D center = converter.toLocalXY(
intersection.position.latitude,
intersection.position.longitude
);
// 创建安全区
auto safetyZone = std::make_unique<collision::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;
@ -126,10 +169,12 @@ void System::processLoop() {
auto last_vehicle_update = std::chrono::steady_clock::now();
auto last_collision_update = std::chrono::steady_clock::now();
auto last_traffic_light_update = std::chrono::steady_clock::now();
auto last_safety_zone_update = std::chrono::steady_clock::now(); // 添加安全区更新时间
int64_t last_aircraft_timestamp = 0;
int64_t last_vehicle_timestamp = 0;
int64_t last_collision_timestamp = 0;
int64_t last_traffic_light_timestamp = 0;
int64_t last_safety_zone_timestamp = 0; // 添加安全区时间戳
Logger::debug("数据处理循环启动");
@ -147,6 +192,41 @@ void System::processLoop() {
auto vehicles = dataCollector_->getVehicleData();
auto traffic_lights = dataCollector_->getTrafficLightSignals();
// 合并航空器和车辆数据用于安全区检查
std::vector<MovingObject*> objects;
for (auto& ac : aircraft) {
objects.push_back(&ac);
}
for (auto& veh : vehicles) {
objects.push_back(&veh);
//Logger::debug("车辆 ", veh.vehicleNo, " 类型: ", veh.type);
//Logger::debug("车辆 ", veh.vehicleNo, " 是否可控: ", veh.isControllable);
const auto* config = controllableVehicles_->findVehicle(veh.vehicleNo);
if (config) {
veh.type = config->type == "UNMANNED" ? MovingObjectType::UNMANNED : MovingObjectType::SPECIAL;
veh.isControllable = config->type == "UNMANNED";
}
}
// 检查安全区更新
auto safety_zone_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
now - last_safety_zone_update).count();
if (safety_zone_elapsed >= system_config.collision_detection.update_interval_ms) {
// 只有在数据更新时才进行检测
if (last_aircraft_timestamp > last_safety_zone_timestamp ||
last_vehicle_timestamp > last_safety_zone_timestamp) {
// 更新安全区状态
updateSafetyZoneStates(objects);
Logger::debug("开始检查安全区冲突...");
checkUnmannedVehicleSafetyZones(vehicles, objects);
last_safety_zone_timestamp = std::max(last_aircraft_timestamp, last_vehicle_timestamp);
}
last_safety_zone_update = now;
}
// 检查无人车与安全区的冲突
checkUnmannedVehicleSafetyZones(vehicles, objects);
// 检查航空器更新
auto aircraft_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
now - last_aircraft_update).count();
@ -193,7 +273,7 @@ void System::processLoop() {
Logger::debug("距离上次更新的时间间隔: ", vehicle_elapsed, "ms");
Logger::debug("配置的更新间隔: ", system_config.websocket.position_update.vehicle_interval_ms, "ms");
Logger::debug("上次时间戳: ", last_vehicle_timestamp);
Logger::debug("收到车辆数量: ", vehicles.size());
Logger::debug("收辆数量: ", vehicles.size());
// 如果是第一次更新（last_vehicle_timestamp == 0），则广播所有车辆位置
if (last_vehicle_timestamp == 0 && !vehicles.empty()) {
@ -227,7 +307,7 @@ void System::processLoop() {
if (has_new_vehicles) {
Logger::debug("发现新数据，准备广播...");
Logger::debug("更新时间戳: ", last_vehicle_timestamp, " -> ", max_timestamp);
Logger::debug("更新间戳: ", last_vehicle_timestamp, " -> ", max_timestamp);
for (const auto& veh : vehicles) {
broadcastPositionUpdate(veh);
}
@ -241,59 +321,59 @@ void System::processLoop() {
Logger::debug("车辆位置更新检查结束 <<<<<<<<<<<<<<<");
}
// 检查冲突更新
auto collision_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
now - last_collision_update).count();
if (collision_elapsed >= system_config.collision_detection.update_interval_ms) {
// 只有当有新的数据时才更新冲突检测
if (last_aircraft_timestamp > last_collision_timestamp ||
last_vehicle_timestamp > last_collision_timestamp) {
// // 检查冲突更新
// auto collision_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
// now - last_collision_update).count();
// if (collision_elapsed >= system_config.collision_detection.update_interval_ms) {
// // 只有有新的数据时才更新冲突检测
// if (last_aircraft_timestamp > last_collision_timestamp ||
// last_vehicle_timestamp > last_collision_timestamp) {
// 更新冲突检测器
collisionDetector_->updateTraffic(aircraft, vehicles);
auto collisions = collisionDetector_->detectCollisions();
// // 更新冲突检测器
// collisionDetector_->updateTraffic(aircraft, vehicles);
// auto collisions = collisionDetector_->detectCollisions();
if (!collisions.empty()) {
Logger::debug("检测到 ", collisions.size(), "个碰撞风险");
for (const auto& risk : collisions) {
Logger::debug("碰撞风险详情: id1=", risk.id1,
", id2=", risk.id2,
", 距离=", risk.distance, "m, 预测最小距离=", risk.minDistance, "m, 风险等级=", static_cast<int>(risk.level),
", 区域类型=", static_cast<int>(risk.zoneType));
}
processCollisions(collisions);
// if (!collisions.empty()) {
// Logger::debug("检测到 ", collisions.size(), "个碰撞风险");
// for (const auto& risk : collisions) {
// Logger::debug("碰撞风险详情: id1=", risk.id1,
// ", id2=", risk.id2,
// ", 距离=", risk.distance, "m, 预测最小距离=", risk.minDistance, "m, <20><><EFBFBD>险等级=", static_cast<int>(risk.level),
// ", 区域类型=", static_cast<int>(risk.zoneType));
// }
// processCollisions(collisions);
} 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();
// } else if (!lastVehiclesWithRisk_.empty()) {
// // 当前没有任何风险，<E999A9><EFBC8C>上次有风险车辆，需要处理恢复指令
// 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();
}
// 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;
}
// last_collision_timestamp = std::max(last_aircraft_timestamp, last_vehicle_timestamp);
// }
// last_collision_update = now;
// }
// 处理红绿灯信号
auto traffic_light_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
@ -448,13 +528,13 @@ void System::processCollisions(const std::vector<CollisionRisk>& risks) {
}
Logger::debug("当前有风险的可控车辆数量: ", currentVehiclesWithRisk.size());
Logger::debug("上一次有风险的可控车辆数量: ", lastVehiclesWithRisk_.size());
Logger::debug("上一次有风险的可车辆数量: ", lastVehiclesWithRisk_.size());
// 对之前有风险但现在没有风险的车辆发送恢复指令
for (const auto& vehicleId : lastVehiclesWithRisk_) {
Logger::debug("检查车辆是否需要恢复: ", vehicleId);
if (currentVehiclesWithRisk.find(vehicleId) == currentVehiclesWithRisk.end()) {
Logger::debug("车辆 ", vehicleId, " 当前没有风险，准备发送恢复指令");
Logger::debug("车辆 ", vehicleId, " 前没有风险，准备发送恢复指令");
VehicleCommand cmd;
cmd.vehicleId = vehicleId;
cmd.type = CommandType::RESUME;
@ -593,7 +673,7 @@ void System::broadcastTimeoutWarning(const network::TimeoutWarningMessage& warni
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");
@ -640,7 +720,7 @@ void System::broadcastVehicleCommand(const VehicleCommand& cmd) {
j["intersectionId"] = cmd.intersectionId;
}
// 添加目标位置（对于所有非 RESUME 类型的指令）
// 添<EFBFBD><EFBFBD><EFBFBD>目标位置（对于所有非 RESUME 类型的指令）
if (cmd.type != CommandType::RESUME) {
j["targetLatitude"] = cmd.latitude;
j["targetLongitude"] = cmd.longitude;
@ -673,4 +753,183 @@ const MovingObject* System::findVehicle(const std::string& vehicleId) const {
}
return nullptr;
}
void System::updateSafetyZoneStates(const std::vector<MovingObject*>& objects) {
// 重置所有安全区状态为未激活，同时重置类型
for (auto& [id, zone] : safetyZones_) {
zone->setState(collision::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) {
// 创建坐标转换器
CoordinateConverter converter;
converter.setReferencePoint(
SystemConfig::instance().airport.reference_point.latitude,
SystemConfig::instance().airport.reference_point.longitude
);
// 转换目标位置到本地坐标系
Vector2D position = converter.toLocalXY(obj.geo.latitude, obj.geo.longitude);
Logger::debug("检查安全区入侵: id=", obj.id,
", type=", obj.isAircraft() ? "飞机" : (obj.isSpecialVehicle() ? "特勤车" : "其他"),
", position=(", position.x, ",", position.y, ")");
// 检查每个安全区
for (auto& [id, zone] : safetyZones_) {
// 检查是否在安全区内，同时会尝试设置安全区类型
if (zone->isObjectInZone(obj)) {
zone->setState(collision::SafetyZoneState::ACTIVE);
Logger::debug("目标 ", obj.id, " 进入路口 ", id, " 安全区, 类型: ",
zone->getType() == collision::SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车",
", 半径: ", zone->getCurrentRadius(),
", 中心点: (", zone->getCenter().x, ",", zone->getCenter().y, ")");
}
}
}
void System::checkUnmannedVehicleSafetyZones(const std::vector<Vehicle>& vehicles,
const std::vector<MovingObject*>& objects) {
// 遍历所有无人车
for (const auto& vehicle : vehicles) {
bool hasRisk = false;
std::string riskIntersectionId;
// 遍历所有路口安全区
for (const auto& [intersectionId, zone] : safetyZones_) {
// 如果安全区未激活，跳过
if (zone->getState() == collision::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();
// 检查是否在预警或告警范围内
if (distance <= zoneRadius) {
// 告警范围
Logger::warning("无人车 ", vehicle.id, " 进入路口 ", intersectionId, " 安全区告警范围");
hasRisk = handleSafetyZoneRisk(vehicle, zone.get(), objects, distance,
intersectionId, CommandType::ALERT, "告警");
riskIntersectionId = intersectionId;
} else if (distance <= 2 * zoneRadius) {
// 预警范围
Logger::warning("无人车 ", vehicle.id, " 进入路口 ", intersectionId, " 安全区预警范围");
hasRisk = handleSafetyZoneRisk(vehicle, zone.get(), objects, distance,
intersectionId, CommandType::WARNING, "预警");
riskIntersectionId = intersectionId;
}
}
// 检查是否需要发送恢<E98081><E681A2>指令
auto it = lastVehiclesWithSafetyZoneRisk_.find(vehicle.id);
if (!hasRisk && it != lastVehiclesWithSafetyZoneRisk_.end()) {
// 之前有风险，现在没有风险，发送恢复指令
Logger::info("无人车 ", vehicle.id, " 离开路口 ", it->second, " 安全区");
VehicleCommand cmd;
cmd.vehicleId = vehicle.id;
cmd.type = CommandType::RESUME;
cmd.reason = CommandReason::RESUME_TRAFFIC;
cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count();
broadcastVehicleCommand(cmd);
controllableVehicles_->sendCommand(vehicle.id, cmd);
lastVehiclesWithSafetyZoneRisk_.erase(it);
} else if (hasRisk) {
// 更新最后一次风险记录
lastVehiclesWithSafetyZoneRisk_[vehicle.id] = riskIntersectionId;
}
}
}
bool System::handleSafetyZoneRisk(const Vehicle& vehicle,
const collision::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() == collision::SafetyZoneType::AIRCRAFT ?
CommandReason::AIRCRAFT_CROSSING :
CommandReason::SPECIAL_VEHICLE;
cmd.timestamp = 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 = Vector2D(cmd.relativeMotionX, cmd.relativeMotionY);
risk.zoneType = WarningZoneType::WARNING;
broadcastCollisionWarning(risk);
return true;
}

30
src/core/System.h
View File

@ -4,10 +4,12 @@
#include <thread>
#include <string>
#include <atomic>
#include <unordered_map>
#include <unordered_set>
#include "types/BasicTypes.h"
#include "detector/CollisionDetector.h"
#include "detector/TrafficLightDetector.h"
#include "detector/SafetyZone.h"
#include "config/AirportBounds.h"
#include "vehicle/ControllableVehicles.h"
#include "network/WebSocketServer.h"
@ -44,6 +46,31 @@ private:
void processLoop();
void processCollisions(const std::vector<CollisionRisk>& collisions);
// 初始化安全区
void initializeSafetyZones();
// 更新安全区状态
void updateSafetyZoneStates(const std::vector<MovingObject*>& objects);
// 检查目标是否进入安全区
void checkSafetyZoneIntrusion(const MovingObject& obj);
// 检查无人车与安全区的冲突
void checkUnmannedVehicleSafetyZones(const std::vector<Vehicle>& vehicles,
const std::vector<MovingObject*>& objects);
// 处理安全区风险
bool handleSafetyZoneRisk(const Vehicle& vehicle,
const collision::SafetyZone* zone,
const std::vector<MovingObject*>& objects,
double distance,
const std::string& intersectionId,
CommandType cmdType,
const std::string& riskLevel);
// 记录上一次有安全区风险的无人车
std::unordered_map<std::string, std::string> lastVehiclesWithSafetyZoneRisk_; // <vehicleId, intersectionId>
std::atomic<bool> running_{false};
std::thread processThread_;
@ -63,6 +90,9 @@ private:
// 路口配置
IntersectionConfig intersection_config_;
// 安全区管理
std::unordered_map<std::string, std::unique_ptr<collision::SafetyZone>> safetyZones_;
static System* instance_;
// 记录上一次有风险的车辆列表

449
src/detector/CollisionDetector.cpp
View File

@ -4,6 +4,7 @@
#include "config/SystemConfig.h"
#include <cmath>
#include <format>
#include <unordered_set>
CollisionDetector::CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles)
: airportBounds_(bounds)
@ -35,12 +36,19 @@ void CollisionDetector::updateTraffic(const std::vector<Aircraft>& aircraft,
if (vehicle.position.x >= bounds.x && vehicle.position.x <= (bounds.x + bounds.width) &&
vehicle.position.y >= bounds.y && vehicle.position.y <= (bounds.y + bounds.height)) {
// 根据是否可控设置车辆类型
// 根据配置设置车辆类型
Vehicle updatedVehicle = vehicle;
if (controllableVehicles_->isControllable(vehicle.vehicleNo)) {
updatedVehicle.type = MovingObjectType::UNMANNED;
updatedVehicle.isControllable = true;
const auto* config = controllableVehicles_->findVehicle(vehicle.vehicleNo);
if (config) {
if (config->type == "UNMANNED") {
updatedVehicle.type = MovingObjectType::UNMANNED;
updatedVehicle.isControllable = true;
} else if (config->type == "SPECIAL") {
updatedVehicle.type = MovingObjectType::SPECIAL;
updatedVehicle.isControllable = false;
}
} else {
// 未配置的车辆默认为特勤车
updatedVehicle.type = MovingObjectType::SPECIAL;
updatedVehicle.isControllable = false;
}
@ -89,130 +97,194 @@ std::vector<CollisionRisk> CollisionDetector::detectCollisions() {
}
}
// 记录当前检测到的冲突对
std::unordered_set<std::pair<std::string, std::string>, CollisionPairHash> currentCollisions;
// 检测可控车辆与航空器的碰撞
for (const auto& aircraft : aircraftData_) {
for (const auto& vehicle : unmannedVehicles) {
// 获取冲突记录键
auto collisionKey = getCollisionKey(aircraft.id, vehicle.id);
// 获取区域配置
const auto& areaConfig = getCollisionParams(aircraft.position);
double safeDistance = areaConfig.warning_zone_radius.aircraft +
areaConfig.warning_zone_radius.unmanned;
// 检查是否存在未解除的冲突记录
auto it = collisionRecords_.find(collisionKey);
bool hasUnresolvedConflict = (it != collisionRecords_.end() && !it->second.resolved);
if (hasUnresolvedConflict) {
Logger::debug("存在未解除的冲突记录: obj1=", aircraft.id,
", obj2=", vehicle.id,
", maxLevel=", static_cast<int>(it->second.maxLevel),
", collisionPoint=(", it->second.collisionPoint.x,
",", it->second.collisionPoint.y, ")");
}
// 检测碰撞
auto collisionResult = checkCollision(
aircraft, vehicle, predictionConfig.time_window);
if (collisionResult.willCollide) {
// 计算相对运动
MovementVector av(aircraft.speed, aircraft.heading);
MovementVector vv(vehicle.speed, vehicle.heading);
double vx = av.vx - vv.vx;
double vy = av.vy - vv.vy;
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
// 根据相对运动方向调整相对速度符号
if (collisionResult.timeToCollision > 0) {
// 如果预测到未来会碰撞，说明当前在接近，相对速度应为负
relativeSpeed = -relativeSpeed;
// 计算相对运动
MovementVector av(aircraft.speed, aircraft.heading);
MovementVector vv(vehicle.speed, vehicle.heading);
double vx = av.vx - vv.vx;
double vy = av.vy - vv.vy;
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
// 根据相对运动方向调整相对速度符号
if (collisionResult.timeToCollision > 0) {
relativeSpeed = -relativeSpeed;
}
// 评估风险等级
auto [level, zoneType] = evaluateRisk(
collisionResult,
aircraft.position,
aircraft.type,
vehicle.type
);
if (level != RiskLevel::NONE) {
// 记录或更新冲突信息
auto& record = collisionRecords_[collisionKey];
if (!record.resolved) {
// 更新已存在的冲突记录
record.maxLevel = std::max(record.maxLevel, level);
record.collisionPoint = collisionResult.collisionPoint; // 更新冲突点
Logger::debug("更新冲突记录: obj1=", aircraft.id,
", obj2=", vehicle.id,
", maxLevel=", static_cast<int>(record.maxLevel),
", collisionPoint=(", record.collisionPoint.x,
",", record.collisionPoint.y, ")");
} else {
// 创建新的冲突记录
record = {
collisionResult.collisionPoint, // 使用当前检测到的冲突点
std::chrono::system_clock::now().time_since_epoch().count(),
level,
false
};
Logger::debug("创建新的冲突记录: obj1=", aircraft.id,
", obj2=", vehicle.id,
", level=", static_cast<int>(level),
", collisionPoint=(", collisionResult.collisionPoint.x,
",", collisionResult.collisionPoint.y, ")");
}
// 评估风险等级
auto [level, zoneType] = evaluateRisk(
collisionResult,
aircraft.position,
aircraft.type,
vehicle.type
);
// 添加到当前冲突集合
currentCollisions.insert(collisionKey);
if (level != RiskLevel::NONE) {
// 添加到风险列表
risks.push_back({
aircraft.id,
vehicle.id,
level,
collisionResult.minDistance,
collisionResult.minDistance,
relativeSpeed,
{vx, vy},
zoneType,
collisionResult.timeToCollision,
collisionResult.collisionPoint
});
Logger::debug("检测到碰撞风险: obj1=", aircraft.id,
", obj2=", vehicle.id,
", minDistance=", collisionResult.minDistance, "m",
", timeToCollision=", collisionResult.timeToCollision, "s",
", level=", static_cast<int>(level));
} else if (hasUnresolvedConflict) {
// 检查是否满足解除条件
if (checkCollisionResolved(aircraft, vehicle, it->second, safeDistance)) {
it->second.resolved = true;
Logger::debug("冲突已解除: obj1=", aircraft.id,
", obj2=", vehicle.id);
} else {
// 虽然当前没有检测到风险，但由于未满足解除条件，继续保持原有风险等级
currentCollisions.insert(collisionKey);
risks.push_back({
aircraft.id,
vehicle.id,
level,
it->second.maxLevel,
collisionResult.minDistance,
collisionResult.minDistance,
relativeSpeed,
{vx, vy},
zoneType,
collisionResult.timeToCollision,
collisionResult.collisionPoint
it->second.collisionPoint
});
Logger::debug("检测到碰撞风险: obj1=", aircraft.id,
Logger::debug("保持原有风险等级: obj1=", aircraft.id,
", obj2=", vehicle.id,
", maxLevel=", static_cast<int>(it->second.maxLevel),
", minDistance=", collisionResult.minDistance, "m",
", timeToCollision=", collisionResult.timeToCollision, "s",
", collisionPoint=(", collisionResult.collisionPoint.x, ",",
collisionResult.collisionPoint.y, ")",
", level=", static_cast<int>(level),
", zone=", static_cast<int>(zoneType),
", relativeSpeed=", relativeSpeed, "m/s",
", relativeMotion=(", vx, ",", vy, ")");
", timeToCollision=", collisionResult.timeToCollision, "s");
}
}
}
}
// 检测可控车辆与所有车辆的碰撞
for (const auto& vehicle1 : unmannedVehicles) {
for (const auto& vehicle2 : allVehicles) {
// 跳过自身
if (vehicle1.id == vehicle2.id) {
continue;
}
// 检测碰撞
auto collisionResult = checkCollision(
vehicle1, vehicle2, predictionConfig.time_window);
if (collisionResult.willCollide) {
// 计算相对运动
MovementVector v1v(vehicle1.speed, vehicle1.heading);
MovementVector v2v(vehicle2.speed, vehicle2.heading);
double vx = v1v.vx - v2v.vx;
double vy = v1v.vy - v2v.vy;
double relativeSpeed = std::sqrt(vx*vx + vy*vy);
// 根据相对运动方向调整相对速度符号
if (collisionResult.timeToCollision > 0) {
// 如果预测到未来会碰撞，说明当前在接近，相对速度应为负
relativeSpeed = -relativeSpeed;
}
// 评估风险等级
auto [level, zoneType] = evaluateRisk(
collisionResult,
vehicle1.position,
vehicle1.type,
vehicle2.type
);
if (level != RiskLevel::NONE) {
risks.push_back({
vehicle1.id,
vehicle2.id,
level,
collisionResult.minDistance,
collisionResult.minDistance,
relativeSpeed,
{vx, vy},
zoneType,
collisionResult.timeToCollision,
collisionResult.collisionPoint
});
Logger::debug("检测到碰撞风险: obj1=", vehicle1.id,
", obj2=", vehicle2.id,
", minDistance=", collisionResult.minDistance, "m",
", timeToCollision=", collisionResult.timeToCollision, "s",
", collisionPoint=(", collisionResult.collisionPoint.x, ",",
collisionResult.collisionPoint.y, ")",
", level=", static_cast<int>(level),
", zone=", static_cast<int>(zoneType),
", relativeSpeed=", relativeSpeed, "m/s",
", relativeMotion=(", vx, ",", vy, ")");
}
}
// 清理已解除的冲突记录
for (auto it = collisionRecords_.begin(); it != collisionRecords_.end();) {
if (it->second.resolved) {
it = collisionRecords_.erase(it);
} else {
++it;
}
}
return risks;
}
bool CollisionDetector::checkCollisionResolved(const MovingObject& obj1,
const MovingObject& obj2,
const CollisionRecord& record,
double safeDistance) const {
// 计算航空器与冲突点的距离
double dist1 = std::sqrt(
std::pow(obj1.position.x - record.collisionPoint.x, 2) +
std::pow(obj1.position.y - record.collisionPoint.y, 2)
);
double dist2 = std::sqrt(
std::pow(obj2.position.x - record.collisionPoint.x, 2) +
std::pow(obj2.position.y - record.collisionPoint.y, 2)
);
// 计算冲突点相对于航空器的位置向量与航空器航向的夹角
double dx = record.collisionPoint.x - obj1.position.x; // 冲突点相对于航空器的向量
double dy = record.collisionPoint.y - obj1.position.y;
double pointAngle = std::atan2(dy, dx) * 180.0 / M_PI; // 冲突点相对于航空器的角度
double angleDiff = normalizeAngle(pointAngle - (90.0 - obj1.heading)); // 修正航向角
Logger::debug("检查冲突解除条件: 航空器位置=(", obj1.position.x, ",", obj1.position.y,
"), 冲突点=(", record.collisionPoint.x, ",", record.collisionPoint.y,
"), 航向=", obj1.heading,
", 冲突点角度=", pointAngle,
", 角度差=", angleDiff,
", 距离=", dist1);
// 判断航空器是否已经通过交叉点
if (angleDiff > 90 && angleDiff < 270) {
// 航空器已经通过交叉点，只要航空器离开冲突点的距离大于安全距离就可以解除
Logger::debug("航空器已通过交叉点，航空器距离=", dist1, "m, 安全距离=", safeDistance, "m");
return dist1 >= safeDistance;
} else {
// 航空器还未通过交叉点，需要两者都在安全距离外且预计不会碰撞
auto collisionResult = checkCollision(obj1, obj2, 30.0); // 使用30秒的预测窗口
bool willCollide = collisionResult.willCollide;
bool bothOutside = (dist1 >= safeDistance && dist2 >= safeDistance);
Logger::debug("航空器未通过交叉点，航空器距离=", dist1, "m, 无人车距离=", dist2,
"m, 安全距离=", safeDistance, "m, 预测碰撞=", willCollide ? "是" : "否");
return !willCollide && bothOutside;
}
}
// 统一的风险评估函数
std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
const collision::CollisionResult& collisionResult,
@ -227,15 +299,15 @@ std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
RiskLevel level = RiskLevel::NONE;
WarningZoneType zoneType = WarningZoneType::NONE;
// 如果预测到碰撞，只根据距离判断风险等级
// 如果预测到碰撞，根据当前距离判断风险等级
if (collisionResult.willCollide) {
// 当前距离小于警报距离 -> 危险等级
if (collisionResult.minDistance <= thresholds.critical) {
if (collisionResult.distance <= thresholds.critical) {
level = RiskLevel::CRITICAL;
zoneType = WarningZoneType::DANGER;
}
// 当前距离在预警范围内 -> 预警等级
else if (collisionResult.minDistance <= thresholds.warning) {
else if (collisionResult.distance <= thresholds.warning) {
level = RiskLevel::WARNING;
zoneType = WarningZoneType::WARNING;
}
@ -243,7 +315,8 @@ std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
// 记录调试信息
Logger::debug(
"风险评估: 当前最小距离=", collisionResult.minDistance,
"风险评估: 当前距离=", collisionResult.distance,
"m, 预测最小距离=", collisionResult.minDistance,
"m, 预警阈值=", thresholds.warning,
"m, 警报阈值=", thresholds.critical,
"m, 预测碰撞=", collisionResult.willCollide ? "是" : "否",
@ -322,8 +395,9 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
double dy = pos2.y - pos1.y;
double current_distance = std::sqrt(dx*dx + dy*dy);
// 初始化最小距离
// 初始化最小距离和当前距离
result.minDistance = current_distance;
result.distance = current_distance;
result.timeToMinDistance = 0.0;
// 2. 计算速度分量
@ -405,7 +479,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
pos2.y + move_distance2 * std::cos(heading2 * M_PI / 180.0)
};
// 碰撞点在两个物体的中点
// 碰撞点两个物体的中点
result.collisionPoint = {
(collision1.x + collision2.x) / 2.0,
(collision1.y + collision2.y) / 2.0
@ -426,102 +500,101 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
", pos2=(", pos2.x, ",", pos2.y, ")",
", v1=(", vx1, ",", vy1, ")",
", v2=(", vx2, ",", vy2, ")",
", rel_v=(", rel_vx, ",", rel_vy, ")",
", current_distance=", current_distance,
", safe_distance=", safe_distance
);
// 计算两条轨迹的最近点时间
// 参考：https://geomalgorithms.com/a07-_distance.html
double a = vx1 * vx1 + vy1 * vy1; // v1 · v1
double b = -(vx1 * vx2 + vy1 * vy2); // -v1 · v2
double c = vx2 * vx2 + vy2 * vy2; // v2 · v2
double d = vx1 * dx + vy1 * dy; // v1 · w
double e = -(vx2 * dx + vy2 * dy); // -v2 · w
double det = a * c - b * b; // 行列式
// 计算行列式 (v1 x v2)
double det = vx1 * (-vy2) - (-vx2) * vy1;
// 计算 det1 = (P2-P1) x (-v2)
double det1 = dx * (-vy2) - (-vx2) * dy;
// 计算 det2 = v1 x (P2-P1)
double det2 = vx1 * dy - dx * vy1;
// 计算时间
double t1 = det1 / det; // 物体1的时间
double t2 = det2 / det; // 物体2的时间
Logger::debug(
"参数计算: a=", a,
", b=", b,
", c=", c,
", d=", d,
", e=", e,
", det=", det
"交叉路径检测: det1=", det1,
", det2=", det2,
", det=", det,
", t1=", t1,
", t2=", t2
);
// 对于交叉路径，们可以直接计算交时间
// 解方程组：
// x1 + vx1 * t = x2 + vx2 * t
// y1 + vy1 * t = y2 + vy2 * t
double t_cross;
if (std::abs(vx1 - vx2) > 0.1) {
t_cross = (pos2.x - pos1.x) / (vx1 - vx2);
} else {
t_cross = (pos2.y - pos1.y) / (vy1 - vy2);
}
// 计算相交点
Vector2D intersection = {
pos1.x + vx1 * t1,
pos1.y + vy1 * t1
};
Logger::debug(
"交叉时间计算: t_cross=", t_cross
"交叉路径检测: intersection=(", intersection.x, ",", intersection.y, ")",
", t1=", t1,
", t2=", t2
);
if (t_cross >= 0 && t_cross <= timeWindow) {
// 计算交叉点
Vector2D cross_point = {
pos1.x + vx1 * t_cross,
pos1.y + vy1 * t_cross
// 检查时间条件
double first_arrival_time = std::min(t1, t2);
double first_arrival_speed = (t1 < t2) ? speed1 : speed2;
double time_threshold = safe_distance / first_arrival_speed;
bool valid_time = (t1 >= 0 && t2 >= 0 &&
std::max(t1, t2) <= timeWindow &&
std::abs(t1 - t2) <= time_threshold);
// 如果满足所有条件，则预测为碰撞
if (valid_time) {
// 计算实际碰撞点（在交点之前）
double collision_distance = safe_distance; // 修改：使用完整的安全距离
double collision_time = first_arrival_time - collision_distance / first_arrival_speed;
// 根据碰撞时间计算两个物体的位置
Vector2D pos1_at_collision = {
pos1.x + vx1 * collision_time,
pos1.y + vy1 * collision_time
};
Vector2D pos2_at_collision = {
pos2.x + vx2 * collision_time,
pos2.y + vy2 * collision_time
};
// 碰撞点在两个物体位置的中点
result.collisionPoint = {
(pos1_at_collision.x + pos2_at_collision.x) / 2.0,
(pos1_at_collision.y + pos2_at_collision.y) / 2.0
};
// 由于有碰撞半径，实际碰撞会提前发生
// 对于叉路径，两车需要各自移动 safe_distance/√2 的距离才会相遇
double offset_distance = safe_distance / std::sqrt(2.0);
double offset_time = offset_distance / speed1; // 两车速度相同，用任意一个都以
double collision_time = t_cross - offset_time;
Logger::debug(
"碰撞时间计算: t_cross=", t_cross,
"碰撞时间计算: first_arrival_time=", first_arrival_time,
", safe_distance=", safe_distance,
", offset_distance=", offset_distance,
", offset_time=", offset_time,
", collision_distance=", collision_distance,
", collision_time=", collision_time
);
if (collision_time >= 0) {
result.willCollide = true;
result.timeToCollision = collision_time;
result.minDistance = safe_distance;
result.timeToMinDistance = collision_time;
// 计算碰撞点（在两个物体的中间）
Vector2D collision1 = {
pos1.x + vx1 * collision_time,
pos1.y + vy1 * collision_time
};
Vector2D collision2 = {
pos2.x + vx2 * collision_time,
pos2.y + vy2 * collision_time
};
result.willCollide = true;
result.timeToCollision = collision_time;
result.timeToMinDistance = collision_time;
result.minDistance = safe_distance;
// 更新物体状态
result.object1State = collision::CollisionObjectState(
pos1_at_collision, speed1, heading1);
result.object2State = collision::CollisionObjectState(
pos2_at_collision, speed2, heading2);
// 碰撞点在两车连线中点
result.collisionPoint = {
(collision1.x + collision2.x) / 2.0,
(collision1.y + collision2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(
collision1, speed1, heading1);
result.object2State = collision::CollisionObjectState(
collision2, speed2, heading2);
Logger::debug(
"交叉路径碰撞: collision_time=", collision_time,
", collision1=(", collision1.x, ",", collision1.y, ")",
", collision2=(", collision2.x, ",", collision2.y, ")",
", collision_point=(", result.collisionPoint.x,
",", result.collisionPoint.y, ")"
);
return result;
}
Logger::debug(
"交叉路径碰撞: collision_time=", collision_time,
", collision1=(", pos1_at_collision.x, ",", pos1_at_collision.y, ")",
", collision2=(", pos2_at_collision.x, ",", pos2_at_collision.y, ")",
", collision_point=(", result.collisionPoint.x,
",", result.collisionPoint.y, ")"
);
return result;
}
} else {
result.type = collision::CollisionType::PARALLEL;
@ -534,7 +607,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
// 航向角转换为学坐标系中的旋转角度（逆时针为正）
double rotation_angle = (90.0 - heading1) * M_PI / 180.0;
// 使用标准的二维坐标旋转公式
// 使用标准的二维坐标旋转式
double dx_rotated = dx * std::cos(rotation_angle) - dy * std::sin(rotation_angle);
double dy_rotated = dx * std::sin(rotation_angle) + dy * std::cos(rotation_angle);
@ -566,7 +639,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
const int STEPS = 120; // 增加采样点数以提高精度
double dt = timeWindow / STEPS; // 时间步长
// 重新计算速度分量（修正计算方式）
// 新计算速度分量（修正计算方式）
double vx1_sample = speed1 * std::cos((90.0 - heading1) * M_PI / 180.0); // x 方向
double vy1_sample = speed1 * std::sin((90.0 - heading1) * M_PI / 180.0); // y 方向
double vx2_sample = speed2 * std::cos((90.0 - heading2) * M_PI / 180.0);
@ -614,23 +687,15 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
double dx_t = future2.x - future1.x;
double dy_t = future2.y - future1.y;
double distance = std::sqrt(dx_t*dx_t + dy_t*dy_t); // 统一使用实际距离
Logger::debug(
"采样点状态: step=", i,
", t=", t,
", distance=", distance,
", prev_distance=", prev_distance,
", min_distance=", result.minDistance
);
// 更新小距离
if (distance < result.minDistance) {
result.minDistance = distance;
result.timeToMinDistance = t;
Logger::debug(
"更新最小距离: min_distance=", distance,
", time_to_min=", t
);
// Logger::debug(
// "更新最小距离: min_distance=", distance,
// ", time_to_min=", t
// );
}
// 检查是否会碰撞

31
src/detector/CollisionDetector.h
View File

@ -9,6 +9,7 @@
#include "CollisionTypes.h"
#include "utils/Logger.h"
#include <vector>
#include <unordered_map>
// 碰撞风险等级
enum class RiskLevel {
@ -37,6 +38,21 @@ struct CollisionRisk {
Vector2D collisionPoint; // 预计碰撞点
};
// 冲突记录信息
struct CollisionRecord {
Vector2D collisionPoint; // 冲突点位置
int64_t detectedTime; // 检测到冲突的时间
RiskLevel maxLevel; // 历史最高风险等级
bool resolved; // 是否已解除
};
// 冲突对象对的哈希函数
struct CollisionPairHash {
std::size_t operator()(const std::pair<std::string, std::string>& p) const {
return std::hash<std::string>()(p.first + p.second);
}
};
class CollisionDetector {
public:
CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles);
@ -60,6 +76,9 @@ private:
std::vector<Aircraft> aircraftData_;
const ControllableVehicles* controllableVehicles_;
// 冲突记录映射表：<(id1,id2), CollisionRecord>
std::unordered_map<std::pair<std::string, std::string>, CollisionRecord, CollisionPairHash> collisionRecords_;
// 将角度标准化到[0, 360)范围
static double normalizeAngle(double angle) {
while (angle >= 360.0) angle -= 360.0;
@ -88,6 +107,18 @@ private:
MovingObjectType type1,
MovingObjectType type2) const;
// 检查是否满足冲突解除条件
bool checkCollisionResolved(const MovingObject& obj1,
const MovingObject& obj2,
const CollisionRecord& record,
double safeDistance) const;
// 获取冲突记录的键
std::pair<std::string, std::string> getCollisionKey(const std::string& id1,
const std::string& id2) const {
return id1 < id2 ? std::make_pair(id1, id2) : std::make_pair(id2, id1);
}
// 计算相对运动
struct MovementVector {
double vx, vy;

2
src/detector/CollisionTypes.h
View File

@ -33,6 +33,7 @@ struct CollisionResult {
double timeToCollision; // 碰撞时间（从当前时刻开始的秒数）
Vector2D collisionPoint; // 碰撞点位置
double minDistance; // 最小距离
double distance; // 当前距离
double timeToMinDistance; // 到达最小距离的时间（从当前时刻开始的秒数）
CollisionType type; // 碰撞类型
CollisionObjectState object1State; // 物体1在碰撞时刻的状态
@ -44,6 +45,7 @@ struct CollisionResult {
timeToCollision(std::numeric_limits<double>::infinity()),
collisionPoint({0, 0}),
minDistance(std::numeric_limits<double>::infinity()),
distance(std::numeric_limits<double>::infinity()),
timeToMinDistance(std::numeric_limits<double>::infinity()),
type(CollisionType::STATIC) {}
};

143
src/detector/SafetyZone.cpp Normal file
View File

@ -0,0 +1,143 @@
#include "SafetyZone.h"
#include "utils/Logger.h"
#include "config/SystemConfig.h"
#include <cmath>
namespace collision {
SafetyZone::SafetyZone(const Vector2D& center, double aircraftRadius, double specialVehicleRadius)
: center_(center)
, aircraftRadius_(aircraftRadius)
, specialVehicleRadius_(specialVehicleRadius)
, currentRadius_(0.0)
, state_(SafetyZoneState::INACTIVE)
, type_(SafetyZoneType::NONE) {
Logger::debug("创建安全区: center=(", center_.x, ",", center_.y,
"), aircraftRadius=", aircraftRadius_,
", specialVehicleRadius=", specialVehicleRadius_);
}
bool SafetyZone::isInZone(const Vector2D& point) const {
// 如果未设置类型，直接返回 false
if (type_ == SafetyZoneType::NONE) {
return false;
}
// 计算点到中心的距离
double dx = point.x - center_.x;
double dy = point.y - center_.y;
double distance = std::sqrt(dx * dx + dy * dy);
// 使用当前设置的安全区半径
return distance <= currentRadius_;
}
void SafetyZone::resetType() {
type_ = SafetyZoneType::NONE;
currentRadius_ = 0.0;
Logger::debug("重置安全区类型为 NONE");
}
bool SafetyZone::trySetType(const MovingObject& object) {
// 判断目标类型
bool isAircraft = object.isAircraft();
bool isSpecialVehicle = object.isSpecialVehicle();
// 如果既不是飞机也不是特勤车，返回 false
if (!isAircraft && !isSpecialVehicle) {
return false;
}
// 如果安全区已有类型，检查是否可以改变
if (type_ != SafetyZoneType::NONE) {
// 如果当前类型与目标类型不匹配，不允许改变
if ((type_ == SafetyZoneType::AIRCRAFT && !isAircraft) ||
(type_ == SafetyZoneType::VEHICLE && !isSpecialVehicle)) {
return false;
}
// 如果类型匹配，保持当前类型
return true;
}
// 设置新的类型和尺寸
if (isAircraft) {
type_ = SafetyZoneType::AIRCRAFT;
currentRadius_ = aircraftRadius_;
state_ = SafetyZoneState::ACTIVE; // 激活安全区
Logger::debug("安全区设置为飞机类型，半径: ", currentRadius_);
} else {
type_ = SafetyZoneType::VEHICLE;
currentRadius_ = specialVehicleRadius_;
state_ = SafetyZoneState::ACTIVE; // 激活安全区
Logger::debug("安全区设置为特勤车类型，半径: ", currentRadius_);
}
return true;
}
bool SafetyZone::isObjectInZone(const MovingObject& object) const {
// 如果是无人车，直接返回 false
if (object.isUnmannedVehicle()) {
Logger::debug("目标 ", object.id, " 是无人车，不触发安全区");
return false;
}
// 从 SystemConfig 获取物体尺寸（半长度）
const auto& config = SystemConfig::instance().collision_detection.prediction;
double objectRadius = 0.0;
// 获取物体半径
if (object.isAircraft()) {
objectRadius = config.aircraft_size / 2.0;
Logger::debug("目标 ", object.id, " 是飞机，尺寸半径: ", objectRadius);
} else if (object.isSpecialVehicle()) {
objectRadius = config.vehicle_size / 2.0;
Logger::debug("目标 ", object.id, " 是特勤车，尺寸半径: ", objectRadius);
} else {
return false;
}
// 如果安全区类型未设置，尝试设置类型
if (type_ == SafetyZoneType::NONE) {
// 临时设置类型和半径来检查是否在区内
SafetyZoneType tempType = object.isAircraft() ? SafetyZoneType::AIRCRAFT : SafetyZoneType::VEHICLE;
double tempRadius = object.isAircraft() ? aircraftRadius_ : specialVehicleRadius_;
// 暂存当前值
auto currentType = type_;
auto currentRadius = currentRadius_;
// 临时设置类型和半径
const_cast<SafetyZone*>(this)->type_ = tempType;
const_cast<SafetyZone*>(this)->currentRadius_ = tempRadius;
// 检查是否在区内
bool inZone = isInZone(object.position);
// 如果在区内，保持新的类型和半径，否则恢复原值
if (!inZone) {
const_cast<SafetyZone*>(this)->type_ = currentType;
const_cast<SafetyZone*>(this)->currentRadius_ = currentRadius;
} else {
// 设置状态为激活
const_cast<SafetyZone*>(this)->state_ = SafetyZoneState::ACTIVE;
Logger::debug("安全区设置为", object.isAircraft() ? "飞机" : "特勤车", "类型，半径: ", tempRadius);
}
return inZone;
}
// 如果类型已设置，检查是否匹配
if ((type_ == SafetyZoneType::AIRCRAFT && !object.isAircraft()) ||
(type_ == SafetyZoneType::VEHICLE && !object.isSpecialVehicle())) {
Logger::debug("目标 ", object.id, " 类型与安全区类型不匹配，当前安全区类型: ",
type_ == SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车");
return false;
}
// 使用 isInZone 检查是否在区内
return isInZone(object.position);
}
} // namespace collision

64
src/detector/SafetyZone.h Normal file
View File

@ -0,0 +1,64 @@
#pragma once
#include "types/BasicTypes.h"
namespace collision {
// 安全区状态
enum class SafetyZoneState {
INACTIVE, // 未激活
ACTIVE // 已激活
};
// 安全区类型
enum class SafetyZoneType {
NONE, // 未设置
AIRCRAFT, // 飞机安全区
VEHICLE // 特勤车安全区
};
// 安全区定义
class SafetyZone {
public:
// 构造函数
SafetyZone(const Vector2D& center, double aircraftSize, double specialVehicleSize);
// 获取状态
SafetyZoneState getState() const { return state_; }
SafetyZoneType getType() const { return type_; }
// 设置状态
void setState(SafetyZoneState state) { state_ = state; }
// 重置类型为 NONE
void resetType();
// 检查点是否在安全区内
bool isInZone(const Vector2D& point) const;
// 检查目标是否在安全区内
bool isObjectInZone(const MovingObject& object) const;
// 获取中心点
const Vector2D& getCenter() const { return center_; }
// 获取安全区尺寸配置
double getAircraftRadius() const { return aircraftRadius_; }
double getSpecialVehicleRadius() const { return specialVehicleRadius_; }
// 获取当前安全区半径
double getCurrentRadius() const { return currentRadius_; }
private:
Vector2D center_; // 安全区中心点
double aircraftRadius_; // 飞机安全区半径
double specialVehicleRadius_; // 特勤车安全区半径
double currentRadius_; // 当前安全区半径
SafetyZoneState state_; // 当前状态
SafetyZoneType type_; // 当前类型
// 尝试设置安全区类型和尺寸
bool trySetType(const MovingObject& object);
};
} // namespace collision

2
src/network/HTTPDataSource.h
View File

@ -51,5 +51,5 @@ private:
static size_t WriteCallback(void* contents, size_t size, size_t nmemb, void* userp);
};
#endif // AIRPORT_NETWORK_HTTP_DATA_SOURCE_H

22
src/types/BasicTypes.h
View File

@ -39,11 +39,11 @@ public:
virtual ~MovingObject() = default;
std::string id;
GeoPosition geo;
Vector2D position;
GeoPosition geo;
double heading;
double speed;
uint64_t timestamp;
int64_t timestamp;
MovingObjectType type; // 添加类型字段
std::deque<PositionRecord> positionHistory;
@ -59,6 +59,11 @@ public:
void copyHistoryFrom(const MovingObject& other);
bool hasPositionChanged() const;
// 添加类型识别虚函数
virtual bool isAircraft() const { return type == MovingObjectType::AIRCRAFT; }
virtual bool isSpecialVehicle() const { return type == MovingObjectType::SPECIAL; }
virtual bool isUnmannedVehicle() const { return type == MovingObjectType::UNMANNED; }
static constexpr size_t MAX_HISTORY = 10;
};
@ -76,12 +81,18 @@ public:
static constexpr double MAX_SPEED = 100.0; // 最大速度（米/秒）
static constexpr double MAX_POSITION_JUMP = 50.0; // 最大位置跳变（米）
bool isAircraft() const override { return true; }
bool isSpecialVehicle() const override { return false; }
};
// 车辆类
class Vehicle : public MovingObject {
public:
Vehicle() { type = MovingObjectType::SPECIAL; } // 默认为特勤车
Vehicle() {
type = MovingObjectType::UNMANNED; // 默认为无人车类型
isControllable = true; // 默认为可控
}
std::string vehicleNo;
bool isControllable; // true 表示无人车，false 表示特勤车
@ -91,4 +102,9 @@ public:
static constexpr double MAX_SPEED = 20.0; // 最大速度（米/秒）
static constexpr double MAX_POSITION_JUMP = 10.0; // 最大位置跳变（米）
bool isAircraft() const override { return false; }
bool isSpecialVehicle() const override {
return type == MovingObjectType::SPECIAL; // 只判断类型
}
};

12
src/vehicle/ControllableVehicles.cpp
View File

@ -25,8 +25,13 @@ const ControllableVehicleConfig* ControllableVehicles::findVehicle(const std::st
}
bool ControllableVehicles::isControllable(const std::string& vehicleNo) const {
// 只检查是否在配置文件中
return findVehicle(vehicleNo) != nullptr;
// 查找车辆配置
const auto* config = findVehicle(vehicleNo);
if (!config) {
return false;
}
// 只有无人车类型是可控的
return config->type == "UNMANNED";
}
void ControllableVehicles::loadConfig(const std::string& configFile) {
@ -42,10 +47,11 @@ void ControllableVehicles::loadConfig(const std::string& configFile) {
for (const auto& item : jsonConfig["vehicles"]) {
ControllableVehicleConfig config;
config.vehicleNo = item["vehicleNo"].get<std::string>();
config.type = item["type"].get<std::string>();
config.ip = item["ip"].get<std::string>();
config.port = item["port"].get<int>();
vehicles_.push_back(config);
Logger::info("Added controllable vehicle: ", config.vehicleNo);
Logger::info("Added vehicle: ", config.vehicleNo, ", type: ", config.type);
}
Logger::info("Loaded ", vehicles_.size(), " controllable vehicles");

1
src/vehicle/ControllableVehicles.h
View File

@ -8,6 +8,7 @@
struct ControllableVehicleConfig {
std::string vehicleNo; // 车牌号
std::string type; // 车辆类型（UNMANNED 或 SPECIAL）
std::string ip; // IP地址
int port; // 端口号
};

160
tests/BasicCollisionTest.cpp
View File

@ -27,6 +27,10 @@ public:
config.warning_zone_radius = {100.0, 100.0, 50.0};
config.alert_zone_radius = {50.0, 50.0, 25.0};
areaConfigs_[AreaType::TEST_ZONE] = config;
// 设置系统配置
auto& system_config = SystemConfig::instance();
system_config.collision_detection.prediction.time_window = 30.0; // 设置预测时间窗口为30秒
}
MOCK_METHOD(AreaType, getAreaType, (const Vector2D& position), (const));
@ -127,7 +131,7 @@ TEST_F(BasicCollisionTest, HeadOnCollision) {
EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点应该在同一水平线上";
// 增加更多验证
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离<EFBFBD><EFBFBD>该是碰撞半径之和50米";
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离该是碰撞半径之和50米";
EXPECT_NEAR(result.timeToMinDistance, 2.5, 0.1) << "最小距离时间应该等于碰撞时间";
// 验证物体状态
@ -174,11 +178,11 @@ TEST_F(BasicCollisionTest, ParallelMotion) {
// 4. 交叉路径测试
TEST_F(BasicCollisionTest, CrossingPaths) {
// 创建两个垂直交叉运动的物体
// 创建两个倾斜交叉运动的物体
Vehicle v1;
v1.vehicleNo = "V1";
v1.position = {100.0, 100.0};
v1.speed = 10.0;
v1.speed = 15.0;
v1.heading = 75.0; // 向东北偏东方向运动
v1.type = MovingObjectType::UNMANNED;
@ -189,36 +193,43 @@ TEST_F(BasicCollisionTest, CrossingPaths) {
v2.heading = 105.0; // 向东南偏东方向运动，与 v1 夹角为 30 度
v2.type = MovingObjectType::UNMANNED;
// 添加航向角度差的调试日志
double angle_diff = std::abs(v1.heading - v2.heading);
Logger::debug("航向角度差检查:");
Logger::debug("v1 航向: " + std::to_string(v1.heading));
Logger::debug("v2 航向: " + std::to_string(v2.heading));
Logger::debug("航向角度差: " + std::to_string(angle_diff));
auto result = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(result.willCollide) << "交叉路径的物体应该检测为碰撞";
EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞";
// 计算碰撞时间和位置：
// v1: 速度分量 (2.59, 9.66) m/s
// v2: 速度分量 (-2.59, 9.66) m/s
// 相对速度: (-5.18, 0) m/s
// 相对速度大小: 5.18 m/s
// v1: 速度分量 (3.882, 14.489) m/s // 15 m/s * (cos(15°), sin(15°))
// v2: 速度分量 (-2.588, 9.659) m/s // 10 m/s * (cos(75°), sin(75°))
// 相对速度: (-6.47, -4.83) m/s
// 相对速度大小: 8.06 m/s
// 碰撞点在两车轨迹交点前的安全距离处
double collision_time = 6.12; // 根据实际计算得到
Vector2D collision_point = {125.0, 184.15}; // 根据实际计算得到
double collision_time = 2.6; // 根据实际计算得到
Vector2D collision_point = {126.68, 156.39}; // 两车位置的中点
EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，撞时间应该接近6.12秒";
EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在125.0";
EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在184.15";
EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，碰撞时间应该接近2.6秒";
EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在126.68";
EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在156.39";
// 增加更多验证
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米";
EXPECT_NEAR(result.timeToMinDistance, collision_time, 0.1) << "最小距离时间应该等于碰撞时间";
// 验证物体状态
EXPECT_NEAR(result.object1State.position.x, 115.85, 0.1);
EXPECT_NEAR(result.object1State.position.y, 159.15, 0.1);
EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
EXPECT_NEAR(result.object1State.position.x, 110.09, 0.1);
EXPECT_NEAR(result.object1State.position.y, 137.67, 0.1);
EXPECT_DOUBLE_EQ(result.object1State.speed, 15.0); // 更新为正确的速度
EXPECT_DOUBLE_EQ(result.object1State.heading, 75.0);
EXPECT_NEAR(result.object2State.position.x, 134.15, 0.1);
EXPECT_NEAR(result.object2State.position.y, 209.15, 0.1);
EXPECT_NEAR(result.object2State.position.x, 143.27, 0.1);
EXPECT_NEAR(result.object2State.position.y, 175.11, 0.1);
EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0);
EXPECT_DOUBLE_EQ(result.object2State.heading, 105.0);
}
@ -278,7 +289,7 @@ TEST_F(BasicCollisionTest, DivergentMotion) {
// 设置两个物体背向运动的场景
// 物体1在(150,100)，向右运动，航向90度
// 物体2在(200,100)，向右运动，航向90度
// 两个物体都在远离对方，不应该发生碰撞
// 两物体都在远离对方，不应该发生碰撞
Vehicle obj1;
obj1.vehicleNo = "V1";
@ -313,7 +324,7 @@ TEST_F(BasicCollisionTest, TailgatingMotion) {
Vehicle v1; // 前车
v1.vehicleNo = "V1";
v1.position = {60.0, 100.0}; // 前车在前方60米处
v1.speed = 10.0; // 前车速度10m/s
v1.speed = 10.0; // 前车<EFBFBD><EFBFBD><EFBFBD>度10m/s
v1.heading = 90.0; // 向东运动
v1.type = MovingObjectType::UNMANNED;
@ -332,7 +343,7 @@ TEST_F(BasicCollisionTest, TailgatingMotion) {
// 验证会发生碰撞
EXPECT_TRUE(result.willCollide) << "后车速度大于前车，应该预测到碰撞";
// 验证碰撞时间（初始距离60米，相对速<EFBFBD><EFBFBD>5m/s，安全距离50米，需要缩短10米，所以碰撞时间应该是2秒）
// 验证碰撞时间（初始距离60米，相对速度5m/s，安全距离50米，需要缩短10米，所以碰撞时间应该是2秒）
EXPECT_NEAR(result.timeToCollision, 2.0, 0.1) << "碰撞时间应该接近2秒";
// 验证最小距离（应该是安全距离）
@ -372,7 +383,7 @@ TEST_F(BasicCollisionTest, QN002AircraftCrossing) {
aircraft.heading = 45.0; // 根据 T7 到 T11 的方向计算
aircraft.type = MovingObjectType::AIRCRAFT;
// 创建 QN002 无人车，从 T4 到 T8 路径上的一点出发
// 创建 QN002 无人车，从 T4 到 T8 路径上的一点发
Vehicle qn002;
qn002.vehicleNo = "QN002";
qn002.position = {-2.0, 120.0}; // 在 T4-T8 路径上，距离 T7 约 120 米（小于检测范围 150 米）
@ -398,11 +409,116 @@ TEST_F(BasicCollisionTest, QN002AircraftCrossing) {
// 验证碰撞参数
EXPECT_GT(result.timeToCollision, 0.0) << "碰撞时间应该大于0";
EXPECT_LT(result.timeToCollision, 30.0) << "碰撞时间应该在预测窗口内";
EXPECT_LE(result.minDistance, 75.0) << "最小距离应该小于预警距离";
EXPECT_LE(result.minDistance, 75.0) << "小距离应该小于预警距离";
// 验证物体状态
EXPECT_DOUBLE_EQ(result.object1State.speed, 50.0 / 3.6);
EXPECT_DOUBLE_EQ(result.object1State.heading, 45.0);
EXPECT_DOUBLE_EQ(result.object2State.speed, 36.0 / 3.6);
EXPECT_DOUBLE_EQ(result.object2State.heading, 135.0);
}
TEST_F(BasicCollisionTest, CrossingBeforeAndAfter) {
// 设置 Mock 对象的预期行为
EXPECT_CALL(*mockControllableVehicles_, isControllable(::testing::StrEq("QN002")))
.WillRepeatedly(::testing::Return(true));
// 创建一个航空器和一个无人车
Aircraft aircraft;
aircraft.id = "AC001";
aircraft.type = MovingObjectType::AIRCRAFT;
aircraft.speed = 10.0; // 10米/秒
aircraft.heading = 90.0; // 向东
Vehicle vehicle;
vehicle.id = "QN002";
vehicle.vehicleNo = "QN002";
vehicle.type = MovingObjectType::UNMANNED;
vehicle.isControllable = true;
vehicle.speed = 10.0; // 10米/秒
vehicle.heading = 0.0; // 向北
// 设置初始位置：航空器在交叉点西侧，无人车在南侧
Vector2D crossPoint{300.0, 300.0}; // 交叉点坐标
// 1. 测试交叉前的情况
// 航空器在交叉点西侧，无人车在南侧
aircraft.position = {crossPoint.x - 80.0, crossPoint.y}; // 航空器在交叉点西侧80
vehicle.position = {crossPoint.x, crossPoint.y - 80.0}; // 无人车在交叉点南侧80米
// 更新交通数据
std::vector<Aircraft> aircrafts = {aircraft};
std::vector<Vehicle> vehicles = {vehicle};
detector_->updateTraffic(aircrafts, vehicles);
// 检测碰撞（使用30秒的预测窗口）
auto risks = detector_->detectCollisions();
// 记录详细的测试信息
Logger::debug("交叉前碰撞检测:");
Logger::debug("航空器位置: (", aircraft.position.x, ",", aircraft.position.y, ")");
Logger::debug("无人车位置: (", vehicle.position.x, ",", vehicle.position.y, ")");
Logger::debug("交叉点位置: (", crossPoint.x, ",", crossPoint.y, ")");
ASSERT_FALSE(risks.empty()) << "交叉前应该检测到碰撞风险";
EXPECT_EQ(risks[0].level, RiskLevel::WARNING) << "交叉前应该是预警级别";
// 2. 测试交叉过程中
// 航空器在交叉点西侧20米，无人车在南侧20米
aircraft.position = {crossPoint.x - 20.0, crossPoint.y};
vehicle.position = {crossPoint.x, crossPoint.y - 20.0};
// 更新交通数据
aircrafts = {aircraft};
vehicles = {vehicle};
detector_->updateTraffic(aircrafts, vehicles);
// 检测碰撞
risks = detector_->detectCollisions();
// 记录详细的测试信息
Logger::debug("交叉过程中碰撞检测:");
Logger::debug("航空器位置: (", aircraft.position.x, ",", aircraft.position.y, ")");
Logger::debug("无人车位置: (", vehicle.position.x, ",", vehicle.position.y, ")");
ASSERT_FALSE(risks.empty()) << "交叉过程中应该检测到碰撞风险";
EXPECT_EQ(risks[0].level, RiskLevel::CRITICAL) << "交叉过程中应该是告警级别";
// 3. 测试交叉后的情况
// 航空器已经通过交叉点并远离安全距离，无人车还在交叉点附近
aircraft.position = {crossPoint.x + 120.0, crossPoint.y}; // 航空器在交叉点东侧120米
vehicle.position = {crossPoint.x, crossPoint.y - 10.0}; // 无人车还在交叉点附近
// 更新交通数据
aircrafts = {aircraft};
vehicles = {vehicle};
detector_->updateTraffic(aircrafts, vehicles);
// 检测碰撞
risks = detector_->detectCollisions();
// 记录详细的测试信息
Logger::debug("交叉后碰撞检测:");
Logger::debug("航空器位置: (", aircraft.position.x, ",", aircraft.position.y, ")");
Logger::debug("无人车位置: (", vehicle.position.x, ",", vehicle.position.y, ")");
EXPECT_TRUE(risks.empty()) << "航空器通过远离安全距离后，应该解除冲突";
// 4. 测试无人车继续运动
// 无人车向北移动到交叉点
vehicle.position = {crossPoint.x, crossPoint.y};
// 更新交通数据
vehicles = {vehicle};
detector_->updateTraffic(aircrafts, vehicles);
// 检测碰撞
risks = detector_->detectCollisions();
// 记录详细的测试信息
Logger::debug("无人车继续运动碰撞检测:");
Logger::debug("航空器位置: (", aircraft.position.x, ",", aircraft.position.y, ")");
Logger::debug("无人车位置: (", vehicle.position.x, ",", vehicle.position.y, ")");
EXPECT_TRUE(risks.empty()) << "航空器已远离，无人车继续运动不应产生新的冲突";
}

5
tests/CollisionDetectorTest.cpp
View File

@ -49,6 +49,9 @@ public:
class CollisionDetectorTest : public ::testing::Test {
protected:
void SetUp() override {
// 设置日志级别为 DEBUG
Logger::initialize("", LogLevel::DEBUG);
// 打印当前工作目录
std::cout << "Current working directory: " << std::filesystem::current_path() << std::endl;
@ -299,7 +302,7 @@ TEST_F(CollisionDetectorTest, AircraftStationaryVehicleCollision) {
auto collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
EXPECT_TRUE(collisionResult.willCollide) << "航空器接近静止车辆时应该检测到碰撞";
// 测试2<EFBFBD><EFBFBD><EFBFBD>静止车辆在航空器航向偏离处
// 测试2：静止车辆在航空器航向偏离处
vehicle.position = {200.0, 200.0}; // 在航空器前方偏北，距离约100米（大于安全距离75米）
collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
EXPECT_FALSE(collisionResult.willCollide) << "航空器与不在航向上的静止车辆不应该检测到碰撞";

178
tests/SafetyZoneTest.cpp Normal file
View File

@ -0,0 +1,178 @@
#include <gtest/gtest.h>
#include "detector/SafetyZone.h"
#include "utils/Logger.h"
#include "config/SystemConfig.h"
using namespace collision;
class SafetyZoneTest : public ::testing::Test {
protected:
void SetUp() override {
// 确保 SystemConfig 已初始化
SystemConfig::instance();
// 创建一个安全区，中心点在(0,0)，飞机安全区半径50米，特勤车安全区半径30米
safetyZone = std::make_unique<SafetyZone>(Vector2D{0, 0}, 50.0, 30.0);
}
std::unique_ptr<SafetyZone> safetyZone;
// 创建测试用的移动对象
Aircraft createAircraft(const std::string& id, double x, double y) {
Aircraft obj;
obj.id = id;
obj.position = Vector2D{x, y};
obj.geo.latitude = 0; // 这里的经纬度不重要
obj.geo.longitude = 0;
obj.type = MovingObjectType::AIRCRAFT; // 设置类型为航空器
return obj;
}
Vehicle createSpecialVehicle(const std::string& id, double x, double y) {
Vehicle obj;
obj.id = id;
obj.position = Vector2D{x, y};
obj.geo.latitude = 0;
obj.geo.longitude = 0;
obj.type = MovingObjectType::SPECIAL; // 设置类型为特勤车
obj.isControllable = false; // 设置为非可控车辆（即特勤车）
return obj;
}
Vehicle createUnmannedVehicle(const std::string& id, double x, double y) {
Vehicle obj;
obj.id = id;
obj.position = Vector2D{x, y};
obj.geo.latitude = 0;
obj.geo.longitude = 0;
obj.type = MovingObjectType::UNMANNED; // 设置类型为无人车
obj.isControllable = true; // 设置为可控车辆（即无人车）
return obj;
}
// 获取飞机有效距离（安全区半径 + 飞机半长度）
double getAircraftEffectiveDistance() const {
const auto& config = SystemConfig::instance().collision_detection.prediction;
return safetyZone->getAircraftRadius() + config.aircraft_size / 2.0;
}
// 获取特勤车有效距离（安全区半径 + 车辆半长度）
double getVehicleEffectiveDistance() const {
const auto& config = SystemConfig::instance().collision_detection.prediction;
return safetyZone->getSpecialVehicleRadius() + config.vehicle_size / 2.0;
}
};
// 测试初始状态
TEST_F(SafetyZoneTest, InitialState) {
EXPECT_EQ(safetyZone->getState(), SafetyZoneState::INACTIVE);
EXPECT_EQ(safetyZone->getType(), SafetyZoneType::NONE);
EXPECT_EQ(safetyZone->getCurrentRadius(), 0.0);
}
// 测试飞机进入安全区
TEST_F(SafetyZoneTest, AircraftEntering) {
double effectiveDistance = getAircraftEffectiveDistance();
// 创建一个飞机，位置在安全区边缘内侧
auto aircraft = createAircraft("AC001", effectiveDistance - 1.0, 0.0);
// 检查飞机是否在安全区内
EXPECT_TRUE(safetyZone->isObjectInZone(aircraft));
EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT);
EXPECT_EQ(safetyZone->getCurrentRadius(), 50.0);
// 测试飞机完全离开安全区
aircraft = createAircraft("AC001", effectiveDistance + 1.0, 0.0);
EXPECT_FALSE(safetyZone->isObjectInZone(aircraft));
}
// 测试特勤车进入安全区
TEST_F(SafetyZoneTest, SpecialVehicleEntering) {
double effectiveDistance = getVehicleEffectiveDistance();
// 创建一个特勤车，位置在安全区边缘内侧
auto vehicle = createSpecialVehicle("TQ001", effectiveDistance - 1.0, 0.0);
// 检查特勤车是否在安全区内
EXPECT_TRUE(safetyZone->isObjectInZone(vehicle));
EXPECT_EQ(safetyZone->getType(), SafetyZoneType::VEHICLE);
EXPECT_EQ(safetyZone->getCurrentRadius(), 30.0);
// 测试特勤车完全离开安全区
vehicle = createSpecialVehicle("TQ001", effectiveDistance + 1.0, 0.0);
EXPECT_FALSE(safetyZone->isObjectInZone(vehicle));
}
// 测试无人车进入安全区
TEST_F(SafetyZoneTest, UnmannedVehicleEntering) {
// 创建一个无人车，位置在安全区内
auto vehicle = createUnmannedVehicle("UV001", 10.0, 0.0);
// 检查无人车是否在安全区内（应该返回false，因为无人车不能设置安全区类型）
EXPECT_FALSE(safetyZone->isObjectInZone(vehicle));
EXPECT_EQ(safetyZone->getType(), SafetyZoneType::NONE);
EXPECT_EQ(safetyZone->getCurrentRadius(), 0.0);
}
// 测试安全区类型锁定
TEST_F(SafetyZoneTest, SafetyZoneTypeLocking) {
double aircraftEffectiveDistance = getAircraftEffectiveDistance();
double vehicleEffectiveDistance = getVehicleEffectiveDistance();
// 先让飞机进入安全区
auto aircraft = createAircraft("AC001", aircraftEffectiveDistance - 5.0, 0.0);
EXPECT_TRUE(safetyZone->isObjectInZone(aircraft));
EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT);
// 尝试让特勤车进入已经被飞机设置类型的安全区
auto vehicle = createSpecialVehicle("TQ001", vehicleEffectiveDistance - 5.0, 0.0);
EXPECT_FALSE(safetyZone->isObjectInZone(vehicle));
// 确认安全区类型和尺寸没有改变
EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT);
EXPECT_EQ(safetyZone->getCurrentRadius(), 50.0);
}
// 测试边界条件
TEST_F(SafetyZoneTest, BoundaryConditions) {
double aircraftEffectiveDistance = getAircraftEffectiveDistance();
double vehicleEffectiveDistance = getVehicleEffectiveDistance();
// 测试飞机在边界上
auto aircraft1 = createAircraft("AC001", aircraftEffectiveDistance, 0.0);
EXPECT_TRUE(safetyZone->isObjectInZone(aircraft1));
// 测试飞机在边界外
auto aircraft2 = createAircraft("AC002", aircraftEffectiveDistance + 0.1, 0.0);
EXPECT_FALSE(safetyZone->isObjectInZone(aircraft2));
// 测试特勤车在边界上
safetyZone = std::make_unique<SafetyZone>(Vector2D{0, 0}, 50.0, 30.0);
auto vehicle1 = createSpecialVehicle("TQ001", vehicleEffectiveDistance, 0.0);
EXPECT_TRUE(safetyZone->isObjectInZone(vehicle1));
// 测试特勤车在边界外
auto vehicle2 = createSpecialVehicle("TQ002", vehicleEffectiveDistance + 0.1, 0.0);
EXPECT_FALSE(safetyZone->isObjectInZone(vehicle2));
}
// 测试状态重置
TEST_F(SafetyZoneTest, StateReset) {
double effectiveDistance = getAircraftEffectiveDistance();
// 先让飞机进入安全区
auto aircraft = createAircraft("AC001", effectiveDistance - 5.0, 0.0);
EXPECT_TRUE(safetyZone->isObjectInZone(aircraft));
// 设置状态为激活
safetyZone->setState(SafetyZoneState::ACTIVE);
EXPECT_EQ(safetyZone->getState(), SafetyZoneState::ACTIVE);
// 重置状态
safetyZone->setState(SafetyZoneState::INACTIVE);
EXPECT_EQ(safetyZone->getState(), SafetyZoneState::INACTIVE);
// 确认类型和尺寸保持不变
EXPECT_EQ(safetyZone->getType(), SafetyZoneType::AIRCRAFT);
EXPECT_EQ(safetyZone->getCurrentRadius(), 50.0);
}

14
tools/map_websocket.html
View File

@ -46,9 +46,21 @@
.aircraft-icon {
width: 50px;
height: 50px;
background-color: purple;
background-color: rgba(128, 0, 128, 0.5);
clip-path: polygon(0% 0%, 100% 0%, 100% 100%, 0% 100%);
border: 2px solid white;
position: relative;
}
.aircraft-icon::after {
content: '';
position: absolute;
width: 6px;
height: 6px;
background-color: black;
border-radius: 50%;
top: 50%;
left: 50%;
transform: translate(-50%, -50%);
}
.special-vehicle-icon {
width: 20px;

23
tools/mock_server.py
View File

@ -36,7 +36,7 @@ DIST_50M = 50
# 时间配置（秒）
WAIT_TIME_AFTER_RETURN = 10.0 # 返回起点后的等待时间
UPDATE_INTERVAL = 1.0 # 位置更新间隔
UPDATE_INTERVAL = 1.0 # 位置更新间隔, 默认 1 秒
TRAFFIC_LIGHT_SWITCH_INTERVAL = 10.0 # 红绿灯切换间隔
# 两个路口的位置
@ -92,12 +92,12 @@ aircraft_data = [
"lat": initial_target_lat,
"lon": initial_target_lon
},
"speed": 50.0 # 滑行速度 50km/h
"speed": 36.0 # 滑行速度 ，默认 50km/h
}
]
# 添加默认速度常量
DEFAULT_VEHICLE_SPEED = 36.0 # km/h
DEFAULT_VEHICLE_SPEED = 18.0 # km/h，默认 36km/h
EMERGENCY_BRAKE_DECELERATION = 0.8 # 紧急制动减速度 (每次更新减速 80%)
NORMAL_BRAKE_DECELERATION = 0.2 # 正常制动减速度 (每次更新减速 20%)
@ -470,27 +470,14 @@ def get_front_traffic_light(vehicle, distance_to_west, distance_to_east):
# QN001 的路线：西路口北侧 -> 东 -> 北
if vehicle["phase"] == 0: # 南北移动
# 在西路口以南时，向北移动需要判断西路口红绿灯
if vehicle["direction"] == 1 and vehicle["latitude"] < T2_INTERSECTION["latitude"]:
return traffic_light_data[0], distance_to_west # 西路口红绿灯
# 在西路口以北时，向南移动需要判断西路口红绿灯
elif vehicle["direction"] == -1 and vehicle["latitude"] > T2_INTERSECTION["latitude"]:
return traffic_light_data[0], distance_to_west # 西路口红绿灯
else: # 东西移动
# 在西路口以西时，向东移动需要判断西路口红绿灯
if vehicle["direction"] == 1 and vehicle["longitude"] < T2_INTERSECTION["longitude"]:
return traffic_light_data[0], distance_to_west # 西路口红绿灯
# 在西路口以东时，向西移动需要判断西路口红绿灯
elif vehicle["direction"] == -1 and vehicle["longitude"] > T2_INTERSECTION["longitude"]:
if vehicle["direction"] == -1 and vehicle["latitude"] > T2_INTERSECTION["latitude"]:
return traffic_light_data[0], distance_to_west # 西路口红绿灯
elif vehicle["vehicleNo"] == "QN002":
# 在东路口以西时，向东移动需要判断东路红绿灯
if vehicle["direction"] == 1 and vehicle["longitude"] < T6_INTERSECTION["longitude"]:
return traffic_light_data[1], distance_to_east # 东路口红绿灯
# 在东路口以东时，向西移动需要判断东路口红绿灯
elif vehicle["direction"] == -1 and vehicle["longitude"] > T6_INTERSECTION["longitude"]:
return traffic_light_data[1], distance_to_east # 东路口红绿灯
# 其他情况，表示车辆已经过路口或不需要判断红绿灯
return None, float('inf')
@ -781,7 +768,7 @@ def switch_traffic_light_state():
lat_diff = abs(aircraft["latitude"] - T6_INTERSECTION["latitude"]) * 111319.9
old_state = traffic_light_data[1]["state"]
traffic_light_data[1]["state"] = 1 if lat_diff > DIST_50M else 0
traffic_light_data[1]["state"] = 1 if lat_diff > DIST_50M else 1
if old_state != traffic_light_data[1]["state"]:
print(f"东路口红绿灯状态切换为: {'绿灯' if traffic_light_data[1]['state'] == 1 else '红灯'}")