增加了 AABB 算法的碰撞检测
This commit is contained in:
parent
dce6f88c77
commit
af458a0b16
@ -49,7 +49,6 @@ 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
|
||||
|
||||
@ -17,7 +17,7 @@
|
||||
"name": "无人车与飞机交叉路口",
|
||||
"trafficLightId": "TL002",
|
||||
"position": {
|
||||
"longitude": 120.089003,
|
||||
"longitude": 120.090003,
|
||||
"latitude": 36.361999,
|
||||
"altitude": 12.5
|
||||
},
|
||||
|
||||
@ -36,8 +36,8 @@
|
||||
"update_interval_ms": 200,
|
||||
"prediction": {
|
||||
"time_window": 20.0,
|
||||
"vehicle_size": 5.0,
|
||||
"aircraft_size": 25.0
|
||||
"vehicle_size": 20.0,
|
||||
"aircraft_size": 60.0
|
||||
}
|
||||
},
|
||||
"logging": {
|
||||
|
||||
@ -1,76 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <unordered_map>
|
||||
#include <unordered_set>
|
||||
#include <chrono>
|
||||
#include "config/SystemConfig.h"
|
||||
#include "config/IntersectionConfig.h"
|
||||
#include "network/WebSocketServer.h"
|
||||
#include "types/VehicleCommand.h"
|
||||
#include "types/MovingObject.h"
|
||||
|
||||
class AirportBounds;
|
||||
class ControllableVehicles;
|
||||
class CollisionDetector;
|
||||
class TrafficLightDetector;
|
||||
class DataCollector;
|
||||
class MovingObject;
|
||||
struct CollisionRisk;
|
||||
struct TrafficLightSignal;
|
||||
|
||||
namespace network {
|
||||
struct TimeoutWarningMessage;
|
||||
}
|
||||
|
||||
class System {
|
||||
public:
|
||||
System();
|
||||
virtual ~System();
|
||||
|
||||
bool initialize();
|
||||
void start();
|
||||
void stop();
|
||||
|
||||
static System* instance() { return instance_; }
|
||||
static void signalHandler(int signal);
|
||||
|
||||
const SystemConfig& getSystemConfig() const { return system_config_; }
|
||||
const IntersectionConfig& getIntersectionConfig() const { return intersection_config_; }
|
||||
|
||||
// 广播相关函数
|
||||
virtual void broadcastTimeoutWarning(const network::TimeoutWarningMessage& warning);
|
||||
void broadcastPositionUpdate(const MovingObject& obj);
|
||||
void broadcastCollisionWarning(const CollisionRisk& risk);
|
||||
void broadcastVehicleCommand(const VehicleCommand& cmd);
|
||||
void broadcastTrafficLightStatus(const TrafficLightSignal& signal);
|
||||
|
||||
private:
|
||||
static System* instance_;
|
||||
|
||||
void processLoop();
|
||||
|
||||
std::atomic<bool> running_{false};
|
||||
std::thread processThread_;
|
||||
std::thread ws_thread_;
|
||||
|
||||
// 系统组件
|
||||
std::unique_ptr<AirportBounds> airportBounds_;
|
||||
std::unique_ptr<ControllableVehicles> controllableVehicles_;
|
||||
std::unique_ptr<CollisionDetector> collisionDetector_;
|
||||
std::unique_ptr<TrafficLightDetector> trafficLightDetector_;
|
||||
std::unique_ptr<DataCollector> dataCollector_;
|
||||
std::unique_ptr<network::WebSocketServer> ws_server_;
|
||||
|
||||
// 配置
|
||||
SystemConfig system_config_;
|
||||
IntersectionConfig intersection_config_;
|
||||
|
||||
// 记录车辆停止的时间
|
||||
std::unordered_map<std::string, std::chrono::system_clock::time_point> vehicleStopTimes_;
|
||||
|
||||
// 记录上一次有风险的可控车辆列表
|
||||
std::unordered_set<std::string> lastVehiclesWithRisk_;
|
||||
};
|
||||
@ -73,11 +73,6 @@ bool System::initialize() {
|
||||
system_config.warning.warning_interval_ms,
|
||||
system_config.warning.log_interval_ms
|
||||
};
|
||||
|
||||
// 初始化简单冲突检测器
|
||||
// simpleCollisionDetector_ = std::make_unique<SimpleCollisionDetector>(
|
||||
// intersection_config_, *controllableVehicles_);
|
||||
// Logger::info("简单冲突检测器初始化完成");
|
||||
|
||||
return dataCollector_->initialize(dataSourceConfig, warnConfig);
|
||||
}
|
||||
@ -268,19 +263,6 @@ void System::processLoop() {
|
||||
}
|
||||
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("当前无碰撞风险，检查是否需要发送恢复指令");
|
||||
@ -361,141 +343,6 @@ 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;
|
||||
@ -716,17 +563,6 @@ 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;
|
||||
@ -751,30 +587,6 @@ 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());
|
||||
|
||||
@ -14,7 +14,6 @@
|
||||
#include "network/MessageTypes.h"
|
||||
#include "config/SystemConfig.h"
|
||||
#include "config/IntersectionConfig.h"
|
||||
#include "detector/SimpleCollisionDetector.h"
|
||||
|
||||
// 前向声明
|
||||
class DataCollector;
|
||||
@ -35,7 +34,6 @@ 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);
|
||||
|
||||
@ -45,7 +43,6 @@ 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_;
|
||||
@ -73,6 +70,4 @@ private:
|
||||
|
||||
// 辅助函数
|
||||
const MovingObject* findVehicle(const std::string& vehicleId) const;
|
||||
|
||||
std::unique_ptr<SimpleCollisionDetector> simpleCollisionDetector_;
|
||||
};
|
||||
@ -1,4 +1,5 @@
|
||||
#include "detector/CollisionDetector.h"
|
||||
#include "CollisionDetector.h"
|
||||
|
||||
#include "utils/Logger.h"
|
||||
#include "config/SystemConfig.h"
|
||||
#include <cmath>
|
||||
@ -354,9 +355,7 @@ bool CollisionDetector::checkCollisionRisk(
|
||||
", 当前距离=", currentDistance,
|
||||
"m, 相对速度=", relativeSpeed,
|
||||
"m/s, 相对运动=", relativeMotion,
|
||||
", 预测碰撞=", prediction.willCollide ? "是" : "否",
|
||||
", 警戒距离=", alertDistance,
|
||||
"m, 有风险=", hasCollisionRisk ? "是" : "否"
|
||||
", 正在远离=", prediction.isMovingAway ? "是" : "否"
|
||||
);
|
||||
}
|
||||
|
||||
@ -413,8 +412,8 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision(
|
||||
result.timeToCollision = std::numeric_limits<double>::infinity();
|
||||
result.minDistance = std::numeric_limits<double>::infinity();
|
||||
|
||||
// 计算安全距离
|
||||
double safeDistance = (size1 + size2) * 1.5;
|
||||
// 计算安全距离等于两个物体尺寸之和，加上 20m 的缓冲区
|
||||
double safeDistance = size1 + size2 + 20.0;
|
||||
|
||||
// 计算速度分量
|
||||
MovementVector mv1(speed1, heading1);
|
||||
@ -558,4 +557,69 @@ CollisionPrediction CollisionDetector::predictTrajectoryCollision(
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 基于矩形的航空器和车辆碰撞检测
|
||||
bool CollisionDetector::checkRectangleBasedAircraftVehicleCollision(
|
||||
const Aircraft& aircraft,
|
||||
const Vehicle& vehicle,
|
||||
double timeWindow) const {
|
||||
|
||||
// 获取系统配置
|
||||
const auto& config = SystemConfig::instance();
|
||||
|
||||
// 计算速度分量
|
||||
MovementVector mv1(aircraft.speed, aircraft.heading);
|
||||
MovementVector mv2(vehicle.speed, vehicle.heading);
|
||||
|
||||
// 创建航空器和车辆的正方形表示
|
||||
collision::Rectangle aircraftRect(
|
||||
aircraft.position, // 中心点
|
||||
config.collision_detection.prediction.aircraft_size * 2.0, // 边长
|
||||
Vector2D{mv1.vx, mv1.vy}, // 速度向量
|
||||
aircraft.heading // 朝向
|
||||
);
|
||||
|
||||
collision::Rectangle vehicleRect(
|
||||
vehicle.position, // 中心点
|
||||
config.collision_detection.prediction.vehicle_size * 2.0, // 边长
|
||||
Vector2D{mv2.vx, mv2.vy}, // 速度向量
|
||||
vehicle.heading // 朝向
|
||||
);
|
||||
|
||||
// 检测碰撞
|
||||
return aircraftRect.checkCollision(vehicleRect, timeWindow);
|
||||
}
|
||||
|
||||
// 基于矩形的车辆间碰撞检测
|
||||
bool CollisionDetector::checkRectangleBasedVehicleCollision(
|
||||
const Vehicle& v1,
|
||||
const Vehicle& v2,
|
||||
double timeWindow) const {
|
||||
|
||||
// 获取系统配置
|
||||
const auto& config = SystemConfig::instance();
|
||||
double vehicleSize = config.collision_detection.prediction.vehicle_size;
|
||||
|
||||
// 计算速度分量
|
||||
MovementVector mv1(v1.speed, v1.heading);
|
||||
MovementVector mv2(v2.speed, v2.heading);
|
||||
|
||||
// 创建两个车辆的正方形表示
|
||||
collision::Rectangle rect1(
|
||||
v1.position, // 中心点
|
||||
vehicleSize * 2.0, // 边长
|
||||
Vector2D{mv1.vx, mv1.vy}, // 速度向量
|
||||
v1.heading // 朝向
|
||||
);
|
||||
|
||||
collision::Rectangle rect2(
|
||||
v2.position, // 中心点
|
||||
vehicleSize * 2.0, // 边长
|
||||
Vector2D{mv2.vx, mv2.vy}, // 速度向量
|
||||
v2.heading // 朝向
|
||||
);
|
||||
|
||||
// 检测碰撞
|
||||
return rect1.checkCollision(rect2, timeWindow);
|
||||
}
|
||||
@ -6,10 +6,9 @@
|
||||
#include "config/AirportBounds.h"
|
||||
#include "vehicle/ControllableVehicles.h"
|
||||
#include "config/SystemConfig.h"
|
||||
#include "RectangleCollision.h"
|
||||
#include "utils/Logger.h"
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <limits>
|
||||
#include <utils/Logger.h>
|
||||
|
||||
// 碰撞风险等级
|
||||
enum class RiskLevel {
|
||||
@ -50,21 +49,35 @@ public:
|
||||
CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles);
|
||||
|
||||
// 更新交通数据
|
||||
void updateTraffic(const std::vector<Aircraft>& aircraft,
|
||||
void updateTraffic(const std::vector<Aircraft>& aircraft,
|
||||
const std::vector<Vehicle>& vehicles);
|
||||
|
||||
// 检测所有可能的碰撞
|
||||
std::vector<CollisionRisk> detectCollisions();
|
||||
|
||||
|
||||
// 基于矩形的航空器和车辆碰撞检测
|
||||
bool checkRectangleBasedAircraftVehicleCollision(
|
||||
const Aircraft& aircraft,
|
||||
const Vehicle& vehicle,
|
||||
double timeWindow = 30.0
|
||||
) const;
|
||||
|
||||
// 基于矩形的车辆间碰撞检测
|
||||
bool checkRectangleBasedVehicleCollision(
|
||||
const Vehicle& v1,
|
||||
const Vehicle& v2,
|
||||
double timeWindow = 30.0
|
||||
) const;
|
||||
|
||||
// 基于轨迹预测的碰撞检测
|
||||
CollisionPrediction predictTrajectoryCollision(
|
||||
const Vector2D& pos1, double speed1, double heading1, // 物体1的位置、速度、航向
|
||||
const Vector2D& pos2, double speed2, double heading2, // 物体2的位置、速度、航向
|
||||
double size1, // 物体1的尺寸
|
||||
double size2, // 物体2的尺寸
|
||||
double timeWindow = 30.0 // 预测时间窗口(默认30秒)
|
||||
const Vector2D& pos1, double speed1, double heading1,
|
||||
const Vector2D& pos2, double speed2, double heading2,
|
||||
double size1,
|
||||
double size2,
|
||||
double timeWindow = 30.0
|
||||
) const;
|
||||
|
||||
|
||||
private:
|
||||
const AirportBounds& airportBounds_;
|
||||
QuadTree<Vehicle> vehicleTree_;
|
||||
@ -90,7 +103,7 @@ private:
|
||||
|
||||
// 计算风险等级
|
||||
RiskLevel calculateRiskLevel(double distance, const Vector2D& position,
|
||||
bool isAircraft1, bool isAircraft2) const;
|
||||
bool isAircraft1, bool isAircraft2) const;
|
||||
|
||||
// 确定预警区域类型
|
||||
WarningZoneType determineWarningZone(double distance, double threshold) const;
|
||||
@ -120,6 +133,26 @@ private:
|
||||
"°, vx=", vx, "m/s, vy=", vy, "m/s");
|
||||
}
|
||||
};
|
||||
|
||||
// 创建正方形
|
||||
collision::Rectangle createRectangle(
|
||||
const Vector2D& position,
|
||||
double size,
|
||||
double heading,
|
||||
double speed
|
||||
) const {
|
||||
// 计算速度分量
|
||||
MovementVector mv(speed, heading);
|
||||
Logger::debug("速度分量计算: speed=" + std::to_string(speed) + "m/s, heading=" +
|
||||
std::to_string(heading) + "°, vx=" + std::to_string(mv.vx) +
|
||||
"m/s, vy=" + std::to_string(mv.vy) + "m/s");
|
||||
|
||||
return collision::Rectangle{
|
||||
position, // 中心点
|
||||
size, // 正方形边长
|
||||
{mv.vx, mv.vy} // 速度向量
|
||||
};
|
||||
}
|
||||
};
|
||||
|
||||
#endif // AIRPORT_DETECTOR_COLLISION_DETECTOR_H
|
||||
37
src/detector/CollisionTypes.h
Normal file
37
src/detector/CollisionTypes.h
Normal file
@ -0,0 +1,37 @@
|
||||
// Copyright 2024 Tianji Robotics. All rights reserved.
|
||||
|
||||
#ifndef AIRPORT_DETECTOR_COLLISION_TYPES_H
|
||||
#define AIRPORT_DETECTOR_COLLISION_TYPES_H
|
||||
|
||||
#include "types/BasicTypes.h"
|
||||
|
||||
namespace collision {
|
||||
|
||||
// 碰撞类型
|
||||
enum class CollisionType {
|
||||
HEAD_ON, // 相向碰撞
|
||||
CROSSING, // 交叉碰撞
|
||||
STATIC, // 静态碰撞
|
||||
PARALLEL // 平行运动
|
||||
};
|
||||
|
||||
// 碰撞检测结果
|
||||
struct CollisionResult {
|
||||
bool willCollide; // 是否会碰撞
|
||||
double timeToCollision; // 碰撞时间
|
||||
Vector2D collisionPoint; // 碰撞点
|
||||
double minDistance; // 最小距离
|
||||
CollisionType type; // 碰撞类型
|
||||
|
||||
// 构造函数
|
||||
CollisionResult() :
|
||||
willCollide(false),
|
||||
timeToCollision(std::numeric_limits<double>::infinity()),
|
||||
collisionPoint({0, 0}),
|
||||
minDistance(std::numeric_limits<double>::infinity()),
|
||||
type(CollisionType::STATIC) {}
|
||||
};
|
||||
|
||||
} // namespace collision
|
||||
|
||||
#endif // AIRPORT_DETECTOR_COLLISION_TYPES_H
|
||||
141
src/detector/RectangleCollision.h
Normal file
141
src/detector/RectangleCollision.h
Normal file
@ -0,0 +1,141 @@
|
||||
#ifndef AIRPORT_DETECTOR_RECTANGLE_COLLISION_H
|
||||
#define AIRPORT_DETECTOR_RECTANGLE_COLLISION_H
|
||||
|
||||
#include "types/BasicTypes.h"
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
|
||||
namespace collision {
|
||||
|
||||
// 碰撞检测结果
|
||||
struct RectangleCollisionResult {
|
||||
bool willCollide = false; // 是否会发生碰撞
|
||||
double timeToCollision = 0.0; // 到碰撞的时间
|
||||
Vector2D collisionPoint = {0, 0}; // 碰撞点
|
||||
double minDistance = 0.0; // 最小距离
|
||||
};
|
||||
|
||||
// 正方形表示
|
||||
struct Rectangle {
|
||||
Vector2D center; // 中心点
|
||||
double size; // 边长
|
||||
Vector2D velocity; // 速度向量
|
||||
double heading; // 朝向（角度）
|
||||
|
||||
// 构造函数
|
||||
Rectangle(const Vector2D& c, double s, const Vector2D& v, double h = 0.0)
|
||||
: center(c), size(s), velocity(v), heading(h) {
|
||||
#ifdef TESTING
|
||||
std::cout << "正方形构造: 中心点=(" << center.x << "," <<
|
||||
center.y << "), 边长=" << size <<
|
||||
", 速度=(" << velocity.x << "," <<
|
||||
velocity.y << ")" << std::endl;
|
||||
#endif
|
||||
}
|
||||
|
||||
// 默认构造函数
|
||||
Rectangle() : center({0, 0}), size(0), velocity({0, 0}), heading(0) {}
|
||||
|
||||
// 获取正方形的AABB（轴对齐包围盒）
|
||||
// 正方形不需要考虑朝向，因为旋转后的正方形AABB保持不变
|
||||
void getAABB(double& x_min, double& x_max, double& y_min, double& y_max) const {
|
||||
double half_size = size / 2.0;
|
||||
x_min = center.x - half_size;
|
||||
x_max = center.x + half_size;
|
||||
y_min = center.y - half_size;
|
||||
y_max = center.y + half_size;
|
||||
}
|
||||
|
||||
// 预测t时刻的位置
|
||||
Rectangle predictPosition(double t) const {
|
||||
Rectangle future = *this;
|
||||
future.center.x += velocity.x * t;
|
||||
future.center.y += velocity.y * t;
|
||||
return future;
|
||||
}
|
||||
|
||||
// 检测与另一个正方形是否碰撞
|
||||
bool checkCollision(const Rectangle& other, double timeWindow = 1.0) const {
|
||||
// 1. 首先检查当前是否碰撞
|
||||
double x1_min, x1_max, y1_min, y1_max;
|
||||
double x2_min, x2_max, y2_min, y2_max;
|
||||
|
||||
this->getAABB(x1_min, x1_max, y1_min, y1_max);
|
||||
other.getAABB(x2_min, x2_max, y2_min, y2_max);
|
||||
|
||||
// 检查当前是否重叠
|
||||
if (x1_max >= x2_min && x2_max >= x1_min &&
|
||||
y1_max >= y2_min && y2_max >= y1_min) {
|
||||
#ifdef TESTING
|
||||
std::cout << "检测到当<EFBFBD><EFBFBD><EFBFBD>碰撞" << std::endl;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
// 2. 检查未来一段时间内是否会碰撞
|
||||
// 计算相对速度
|
||||
double rel_vx = velocity.x - other.velocity.x;
|
||||
double rel_vy = velocity.y - other.velocity.y;
|
||||
double rel_speed = std::sqrt(rel_vx*rel_vx + rel_vy*rel_vy);
|
||||
|
||||
// 如果相对速度接近0，使用当前距离判断
|
||||
if (rel_speed < 0.1) {
|
||||
double dx = center.x - other.center.x;
|
||||
double dy = center.y - other.center.y;
|
||||
double distance = std::sqrt(dx*dx + dy*dy);
|
||||
double safe_distance = size/2.0 + other.size/2.0; // 两个正方形从中心到边缘的距离之和
|
||||
|
||||
if (distance <= safe_distance) {
|
||||
#ifdef TESTING
|
||||
std::cout << "静止状态检测到碰撞，距离=" << distance << "米" << std::endl;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// 3. 在时间窗口内采样检查
|
||||
const int STEPS = 20;
|
||||
double dt = timeWindow / STEPS;
|
||||
|
||||
for (int i = 1; i <= STEPS; ++i) {
|
||||
double t = i * dt;
|
||||
Rectangle future1 = predictPosition(t);
|
||||
Rectangle future2 = other.predictPosition(t);
|
||||
|
||||
future1.getAABB(x1_min, x1_max, y1_min, y1_max);
|
||||
future2.getAABB(x2_min, x2_max, y2_min, y2_max);
|
||||
|
||||
// 检查是否重叠
|
||||
if (x1_max >= x2_min && x2_max >= x1_min &&
|
||||
y1_max >= y2_min && y2_max >= y1_min) {
|
||||
#ifdef TESTING
|
||||
std::cout << "预测到碰撞，时间=" << t << "秒" << std::endl;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
// 计算中心点距离
|
||||
double dx = (future1.center.x - future2.center.x);
|
||||
double dy = (future1.center.y - future2.center.y);
|
||||
double distance = std::sqrt(dx*dx + dy*dy);
|
||||
double safe_distance = size/2.0 + other.size/2.0; // 两个正方形从中心到边缘的距离之和
|
||||
|
||||
if (distance <= safe_distance) {
|
||||
#ifdef TESTING
|
||||
std::cout << "预测到危险接近，时间=" << t
|
||||
<< "秒，距离=" << distance
|
||||
<< "米，安全距离=" << safe_distance << "米" << std::endl;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace collision
|
||||
|
||||
#endif // AIRPORT_DETECTOR_RECTANGLE_COLLISION_H
|
||||
@ -1,202 +0,0 @@
|
||||
#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;
|
||||
}
|
||||
@ -1,72 +0,0 @@
|
||||
#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
|
||||
@ -5,6 +5,29 @@
|
||||
#include <gmock/gmock.h>
|
||||
#include "utils/Logger.h"
|
||||
#include <chrono>
|
||||
#include <filesystem>
|
||||
|
||||
// 在所有测试开始前初始化日志
|
||||
class GlobalTestEnvironment : public ::testing::Environment {
|
||||
public:
|
||||
void SetUp() override {
|
||||
// 确保使用 DEBUG 级别
|
||||
Logger::initialize("logs/test.log", LogLevel::DEBUG);
|
||||
Logger::debug("=== 测试开始 ===");
|
||||
}
|
||||
|
||||
void TearDown() override {
|
||||
Logger::debug("=== 测试结束 ===");
|
||||
Logger::initialize("", LogLevel::INFO); // 关闭日志文件
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
// 添加全局测试环境
|
||||
::testing::AddGlobalTestEnvironment(new GlobalTestEnvironment);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
||||
// Mock ControllableVehicles 类
|
||||
class MockControllableVehicles : public ControllableVehicles {
|
||||
@ -42,6 +65,9 @@ public:
|
||||
class CollisionDetectorTest : public ::testing::Test {
|
||||
protected:
|
||||
void SetUp() override {
|
||||
// 打印当前工作目录
|
||||
std::cout << "Current working directory: " << std::filesystem::current_path() << std::endl;
|
||||
|
||||
// 创建 Mock 对象
|
||||
airportBounds_ = std::make_unique<MockAirportBounds>();
|
||||
mockControllableVehicles_ = std::make_unique<MockControllableVehicles>();
|
||||
@ -50,6 +76,10 @@ protected:
|
||||
detector_ = std::make_unique<CollisionDetector>(*airportBounds_, *mockControllableVehicles_);
|
||||
}
|
||||
|
||||
void TearDown() override {
|
||||
Logger::initialize("", LogLevel::INFO);
|
||||
}
|
||||
|
||||
std::unique_ptr<MockAirportBounds> airportBounds_;
|
||||
std::unique_ptr<MockControllableVehicles> mockControllableVehicles_;
|
||||
std::unique_ptr<CollisionDetector> detector_;
|
||||
@ -62,7 +92,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) {
|
||||
.WillRepeatedly(testing::Return(true));
|
||||
Logger::info("Set mock expectation: VEH001 is controllable");
|
||||
|
||||
// 设置<EFBFBD><EFBFBD><EFBFBD>试数据
|
||||
// 设置测试数据
|
||||
Aircraft aircraft;
|
||||
aircraft.flightNo = "TEST001";
|
||||
aircraft.position = {100, 100};
|
||||
@ -73,7 +103,7 @@ TEST_F(CollisionDetectorTest, DetectControllableVehicleAircraftCollision) {
|
||||
|
||||
Vehicle vehicle;
|
||||
vehicle.vehicleNo = "VEH001";
|
||||
vehicle.position = {120, 100}; // 距离航空器20米
|
||||
vehicle.position = {120, 100}; // 离航空器20米
|
||||
vehicle.speed = 5;
|
||||
vehicle.heading = 270;
|
||||
Logger::info("Created vehicle: vehicleNo=", vehicle.vehicleNo,
|
||||
@ -358,357 +388,193 @@ TEST_F(CollisionDetectorTest, PerformanceTest) {
|
||||
EXPECT_LT(duration.count(), 100000); // 期望处理时间小于100ms
|
||||
}
|
||||
|
||||
// 测试轨迹预测的准确性
|
||||
TEST_F(CollisionDetectorTest, TrajectoryPredictionAccuracy) {
|
||||
// 设置 Mock 期望
|
||||
EXPECT_CALL(*mockControllableVehicles_, isControllable(testing::_))
|
||||
.WillRepeatedly(testing::Return(true));
|
||||
// 修改矩形碰撞检测测试
|
||||
TEST_F(CollisionDetectorTest, RectangleCollisionParallelMotion) {
|
||||
// 设置系统配置
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
|
||||
|
||||
// 创建两个平行运动的车辆
|
||||
Vehicle v1;
|
||||
v1.vehicleNo = "VEH001";
|
||||
v1.position = {100.0, 100.0};
|
||||
v1.speed = 10.0;
|
||||
v1.heading = 90.0; // 向东行驶
|
||||
|
||||
Vehicle v2;
|
||||
v2.vehicleNo = "VEH002";
|
||||
v2.position = {100.0, 107.0}; // 在v1北侧7米处
|
||||
v2.speed = 10.0;
|
||||
v2.heading = 90.0; // 向东行驶
|
||||
|
||||
// 检测碰撞
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
|
||||
// 由于两车间距小于安全距离，应该检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "平行运动的车辆应该检测到碰撞";
|
||||
}
|
||||
|
||||
// 测试场景1：正面相遇碰撞
|
||||
{
|
||||
Vector2D pos1{100, 100}; // 第一个物体的位置
|
||||
double speed1 = 10; // 第一个物体的速度 (m/s)
|
||||
double heading1 = 90; // 第一个物体的航向（正东，90度）
|
||||
|
||||
Vector2D pos2{300, 100}; // 第二个物体的位置
|
||||
double speed2 = 10; // 第二个物体的速度 (m/s)
|
||||
double heading2 = 270; // 第二个物体的航向（正西，270度）
|
||||
|
||||
double size1 = 5; // 第一个物体的尺寸
|
||||
double size2 = 5; // 第二个物体的尺寸
|
||||
double timeWindow = 30; // 预测时间窗口
|
||||
|
||||
auto prediction = detector_->predictTrajectoryCollision(
|
||||
pos1, speed1, heading1,
|
||||
pos2, speed2, heading2,
|
||||
size1, size2, timeWindow
|
||||
);
|
||||
|
||||
// 验证结果：首先验证是否检测到碰撞
|
||||
EXPECT_TRUE(prediction.willCollide) << "应该检测到碰撞";
|
||||
if (prediction.willCollide) {
|
||||
// 验证碰撞点位置（允许1米的误差）
|
||||
EXPECT_NEAR(prediction.collisionPoint.x, 200.0, 1.0) << "碰撞点x坐标应该在中点附近";
|
||||
EXPECT_NEAR(prediction.collisionPoint.y, 100.0, 1.0) << "碰撞点y坐标应该保持不变";
|
||||
}
|
||||
}
|
||||
TEST_F(CollisionDetectorTest, RectangleCollisionCrossingPaths) {
|
||||
// 设置系统配置
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
|
||||
|
||||
// 测试场景2：平行路径，不会碰撞
|
||||
{
|
||||
Vector2D pos1{100, 100};
|
||||
double speed1 = 10;
|
||||
double heading1 = 90; // 正东（90度）
|
||||
|
||||
Vector2D pos2{100, 120}; // 平行路径，距20米
|
||||
double speed2 = 10;
|
||||
double heading2 = 90; // 正东（90度）
|
||||
|
||||
double size1 = 5;
|
||||
double size2 = 5;
|
||||
double timeWindow = 30;
|
||||
|
||||
auto prediction = detector_->predictTrajectoryCollision(
|
||||
pos1, speed1, heading1,
|
||||
pos2, speed2, heading2,
|
||||
size1, size2, timeWindow
|
||||
);
|
||||
|
||||
// 验证结果：不应该检测到碰撞
|
||||
EXPECT_FALSE(prediction.willCollide) << "平行路径不应该碰撞";
|
||||
// 验证最小距离（允许1米的误差）
|
||||
EXPECT_NEAR(prediction.minDistance, 20.0, 1.0) << "平行路径应该保持20米的距离";
|
||||
}
|
||||
// 创建两个交叉路径的车辆
|
||||
Vehicle v1;
|
||||
v1.vehicleNo = "VEH001";
|
||||
v1.position = {0.0, 100.0};
|
||||
v1.speed = 10.0;
|
||||
v1.heading = 90.0; // 向东行驶
|
||||
|
||||
// 测试场景3：垂直相交路径
|
||||
{
|
||||
Vector2D pos1{0, 100}; // 第一个物体从左开始
|
||||
double speed1 = 10;
|
||||
double heading1 = 90; // 正东（90度）
|
||||
|
||||
Vector2D pos2{200, 300}; // 第二个物体从上开始
|
||||
double speed2 = 10;
|
||||
double heading2 = 180; // 正南（180度）
|
||||
|
||||
double size1 = 5;
|
||||
double size2 = 5;
|
||||
double timeWindow = 30; // 30秒的预测窗口
|
||||
|
||||
Logger::info("垂直路径测试参数:");
|
||||
Logger::info(" 物体1: 位置=(" + std::to_string(pos1.x) + ", " + std::to_string(pos1.y) +
|
||||
"), 速度=" + std::to_string(speed1) + ", 航向=" + std::to_string(heading1));
|
||||
Logger::info(" 物体2: 位置=(" + std::to_string(pos2.x) + ", " + std::to_string(pos2.y) +
|
||||
"), 速度=" + std::to_string(speed2) + ", 航向=" + std::to_string(heading2));
|
||||
|
||||
// 计算速度分量用于调试
|
||||
double heading1Rad = heading1 * M_PI / 180.0;
|
||||
double heading2Rad = heading2 * M_PI / 180.0;
|
||||
double v1x = speed1 * std::sin(heading1Rad); // 东向为x轴正方向
|
||||
double v1y = speed1 * std::cos(heading1Rad); // 北向为y轴正方向
|
||||
double v2x = speed2 * std::sin(heading2Rad);
|
||||
double v2y = speed2 * std::cos(heading2Rad);
|
||||
Logger::info("速度分量:");
|
||||
Logger::info(" 物体1: vx=" + std::to_string(v1x) + ", vy=" + std::to_string(v1y));
|
||||
Logger::info(" 物体2: vx=" + std::to_string(v2x) + ", vy=" + std::to_string(v2y));
|
||||
|
||||
auto prediction = detector_->predictTrajectoryCollision(
|
||||
pos1, speed1, heading1,
|
||||
pos2, speed2, heading2,
|
||||
size1, size2, timeWindow
|
||||
);
|
||||
|
||||
Logger::info("预测结果:");
|
||||
Logger::info(" willCollide=" + std::to_string(prediction.willCollide));
|
||||
Logger::info(" minDistance=" + std::to_string(prediction.minDistance));
|
||||
if (prediction.willCollide) {
|
||||
Logger::info(" 碰撞点=(" + std::to_string(prediction.collisionPoint.x) + ", " +
|
||||
std::to_string(prediction.collisionPoint.y) + ")");
|
||||
Logger::info(" 碰撞时间=" + std::to_string(prediction.timeToCollision));
|
||||
|
||||
// 计算理论碰撞点
|
||||
// 物体1：x = 0 + 10t, y = 100
|
||||
// 物体2：x = 200, y = 300 - 10t
|
||||
// 碰撞时：x1 = x2 = 200, y1 = y2
|
||||
double collisionTime = 20.0; // t = 200/10 = 20秒
|
||||
double expectedX = 200.0; // x = 0 + 10*20 = 200
|
||||
double expectedY = 100.0; // y = 100 (物体1的y保持不变)
|
||||
|
||||
// 验证碰撞点位置（允许5米的误差，考虑到物体尺寸）
|
||||
EXPECT_NEAR(prediction.collisionPoint.x, expectedX, 5.0)
|
||||
<< "碰撞点x坐标应该在交叉点附近";
|
||||
EXPECT_NEAR(prediction.collisionPoint.y, expectedY, 5.0)
|
||||
<< "碰撞点y坐标应该在交叉点附近";
|
||||
|
||||
// 验证碰撞时间（允许1秒的误差）
|
||||
EXPECT_NEAR(prediction.timeToCollision, collisionTime, 1.0)
|
||||
<< "碰撞时间应该接近20秒";
|
||||
}
|
||||
}
|
||||
Vehicle v2;
|
||||
v2.vehicleNo = "VEH002";
|
||||
v2.position = {100.0, 200.0};
|
||||
v2.speed = 10.0;
|
||||
v2.heading = 180.0; // 向南行驶
|
||||
|
||||
// 测试场景4：静止物体
|
||||
{
|
||||
Vector2D pos1{0, 100}; // 第一个物体从远处开始
|
||||
double speed1 = 10;
|
||||
double heading1 = 90; // 正东（90度）
|
||||
|
||||
Vector2D pos2{200, 100}; // 静止物体在目标位置
|
||||
double speed2 = 0; // 静止物体
|
||||
double heading2 = 0; // 航向无关紧要，因为是静止的
|
||||
|
||||
double size1 = 5;
|
||||
double size2 = 5;
|
||||
double timeWindow = 30; // 30秒的预测窗口
|
||||
|
||||
Logger::info("静止物体测试参数:");
|
||||
Logger::info(" 物体1: 位置=(" + std::to_string(pos1.x) + ", " + std::to_string(pos1.y) +
|
||||
"), 速度=" + std::to_string(speed1) + ", 航向=" + std::to_string(heading1));
|
||||
Logger::info(" 物体2: 位置=(" + std::to_string(pos2.x) + ", " + std::to_string(pos2.y) +
|
||||
"), 速度=" + std::to_string(speed2) + ", 航向=" + std::to_string(heading2));
|
||||
|
||||
// 计算速度分量用于调试
|
||||
double heading1Rad = heading1 * M_PI / 180.0;
|
||||
double heading2Rad = heading2 * M_PI / 180.0;
|
||||
double v1x = speed1 * std::sin(heading1Rad); // 东向为x轴正方向
|
||||
double v1y = speed1 * std::cos(heading1Rad); // 北向为y轴正方向
|
||||
double v2x = speed2 * std::sin(heading2Rad);
|
||||
double v2y = speed2 * std::cos(heading2Rad);
|
||||
Logger::info("速度分量:");
|
||||
Logger::info(" 物体1: vx=" + std::to_string(v1x) + ", vy=" + std::to_string(v1y));
|
||||
Logger::info(" 物体2: vx=" + std::to_string(v2x) + ", vy=" + std::to_string(v2y));
|
||||
|
||||
auto prediction = detector_->predictTrajectoryCollision(
|
||||
pos1, speed1, heading1,
|
||||
pos2, speed2, heading2,
|
||||
size1, size2, timeWindow
|
||||
);
|
||||
|
||||
Logger::info("预测结果:");
|
||||
Logger::info(" willCollide=" + std::to_string(prediction.willCollide));
|
||||
Logger::info(" minDistance=" + std::to_string(prediction.minDistance));
|
||||
if (prediction.willCollide) {
|
||||
Logger::info(" 碰撞点=(" + std::to_string(prediction.collisionPoint.x) + ", " +
|
||||
std::to_string(prediction.collisionPoint.y) + ")");
|
||||
Logger::info(" 碰撞时间=" + std::to_string(prediction.timeToCollision));
|
||||
|
||||
// 计算理论碰撞点
|
||||
// 考虑物体尺寸：当两个物体中心点距离等于它们尺寸之和时就发生碰撞
|
||||
double safeDistance = size1 + size2; // 10米
|
||||
double expectedX = pos2.x - safeDistance; // 200 - 10 = 190米
|
||||
double expectedY = pos2.y; // 100米
|
||||
double collisionTime = expectedX / speed1; // 19秒
|
||||
|
||||
// 验证碰撞点位置（允许5米的误差，考虑到数值计算的精度）
|
||||
EXPECT_NEAR(prediction.collisionPoint.x, expectedX, 5.0)
|
||||
<< "碰撞点应该在考虑物体尺寸后的位置";
|
||||
EXPECT_NEAR(prediction.collisionPoint.y, expectedY, 5.0)
|
||||
<< "碰撞点y坐标应该保持不变";
|
||||
|
||||
// 验证碰撞时间（允许1秒的误差）
|
||||
EXPECT_NEAR(prediction.timeToCollision, collisionTime, 1.0)
|
||||
<< "碰撞时间应该接近19秒";
|
||||
}
|
||||
}
|
||||
// 检测碰撞
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
|
||||
// 测试场景5：超出时间窗口的碰撞
|
||||
{
|
||||
Vector2D pos1{100, 100};
|
||||
double speed1 = 10;
|
||||
double heading1 = 90; // 向东
|
||||
|
||||
Vector2D pos2{400, 100};
|
||||
double speed2 = 0;
|
||||
double heading2 = 0; // 静止物体的航向可以是任意值
|
||||
|
||||
double size1 = 5;
|
||||
double size2 = 5;
|
||||
double timeWindow = 20; // 20秒的时间窗口（不足以到达碰撞点）
|
||||
|
||||
auto prediction = detector_->predictTrajectoryCollision(
|
||||
pos1, speed1, heading1,
|
||||
pos2, speed2, heading2,
|
||||
size1, size2, timeWindow
|
||||
);
|
||||
|
||||
// 验证结果：不应该检测到碰撞（因为超出时间窗口）
|
||||
EXPECT_FALSE(prediction.willCollide) << "超出时间窗口应该报告碰撞";
|
||||
EXPECT_GT(prediction.minDistance, 0) << "最小距离该大于0";
|
||||
}
|
||||
// 两车将在(100, 100)处相遇，应该检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "交叉路径的车辆应该检测到碰撞";
|
||||
}
|
||||
|
||||
TEST_F(CollisionDetectorTest, RectangleCollisionAircraftVehicle) {
|
||||
// 设置系统配置
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
|
||||
SystemConfig::instance().collision_detection.prediction.aircraft_size = 10.0;
|
||||
|
||||
// 测试场景6：飞机和车辆垂直相交
|
||||
{
|
||||
// 飞机尺寸：波音737-800，翼展35.8米，长度39.5米
|
||||
double aircraftSize = 40.0; // 使用较大的值以确保安全
|
||||
|
||||
// 车辆尺寸：标准摆渡车，长度12米，宽度2.5米
|
||||
double vehicleSize = 12.0;
|
||||
|
||||
// 飞机从西向东，车辆从南向北
|
||||
Vector2D aircraftPos{0, 200}; // 飞机初始位置
|
||||
double aircraftSpeed = 12; // 飞机速度
|
||||
double aircraftHeading = 90; // 向东飞行
|
||||
|
||||
Vector2D vehiclePos{200, 100}; // 修改车辆初始位置，使其能与飞机相遇
|
||||
double vehicleSpeed = 6; // 车辆速度
|
||||
double vehicleHeading = 0; // 向北行驶
|
||||
|
||||
double timeWindow = 60; // 60秒的预测窗口，考虑到较低的速度
|
||||
|
||||
Logger::info("飞机和车辆垂直相交测试参数:");
|
||||
Logger::info(" 飞机: 位置=(" + std::to_string(aircraftPos.x) + ", " + std::to_string(aircraftPos.y) +
|
||||
"), 速度=" + std::to_string(aircraftSpeed) + ", 航向=" + std::to_string(aircraftHeading));
|
||||
Logger::info(" 车辆: 位置=(" + std::to_string(vehiclePos.x) + ", " + std::to_string(vehiclePos.y) +
|
||||
"), 速度=" + std::to_string(vehicleSpeed) + ", 航向=" + std::to_string(vehicleHeading));
|
||||
|
||||
// 计算速度分量
|
||||
double aircraftHeadingRad = aircraftHeading * M_PI / 180.0;
|
||||
double vehicleHeadingRad = vehicleHeading * M_PI / 180.0;
|
||||
double vax = aircraftSpeed * std::sin(aircraftHeadingRad); // 东向为x轴正方向
|
||||
double vay = aircraftSpeed * std::cos(aircraftHeadingRad); // 北向为y轴正方向
|
||||
double vvx = vehicleSpeed * std::sin(vehicleHeadingRad);
|
||||
double vvy = vehicleSpeed * std::cos(vehicleHeadingRad);
|
||||
|
||||
Logger::info("速度分量:");
|
||||
Logger::info(" 飞机: vx=" + std::to_string(vax) + ", vy=" + std::to_string(vay));
|
||||
Logger::info(" 车辆: vx=" + std::to_string(vvx) + ", vy=" + std::to_string(vvy));
|
||||
|
||||
// 计算相对运动方程的系数
|
||||
double dx = aircraftPos.x - vehiclePos.x;
|
||||
double dy = aircraftPos.y - vehiclePos.y;
|
||||
double dvx = vax - vvx;
|
||||
double dvy = vay - vvy;
|
||||
double safeDistance = aircraftSize + vehicleSize;
|
||||
double a = dvx * dvx + dvy * dvy;
|
||||
double b = 2.0 * (dx * dvx + dy * dvy);
|
||||
double c = dx * dx + dy * dy - safeDistance * safeDistance;
|
||||
|
||||
Logger::info("二次方程系数:");
|
||||
Logger::info(" 相对位置: dx=" + std::to_string(dx) + ", dy=" + std::to_string(dy));
|
||||
Logger::info(" 相对速度: dvx=" + std::to_string(dvx) + ", dvy=" + std::to_string(dvy));
|
||||
Logger::info(" 安全距离=" + std::to_string(safeDistance));
|
||||
Logger::info(" a=" + std::to_string(a) + ", b=" + std::to_string(b) + ", c=" + std::to_string(c));
|
||||
|
||||
auto prediction = detector_->predictTrajectoryCollision(
|
||||
aircraftPos, aircraftSpeed, aircraftHeading,
|
||||
vehiclePos, vehicleSpeed, vehicleHeading,
|
||||
aircraftSize, vehicleSize, timeWindow
|
||||
);
|
||||
|
||||
Logger::info("预测结果:");
|
||||
Logger::info(" willCollide=" + std::to_string(prediction.willCollide));
|
||||
Logger::info(" minDistance=" + std::to_string(prediction.minDistance));
|
||||
if (prediction.willCollide) {
|
||||
Logger::info(" 碰撞点=(" + std::to_string(prediction.collisionPoint.x) + ", " +
|
||||
std::to_string(prediction.collisionPoint.y) + ")");
|
||||
Logger::info(" 碰撞时间=" + std::to_string(prediction.timeToCollision));
|
||||
}
|
||||
|
||||
// 验证结果
|
||||
EXPECT_TRUE(prediction.willCollide) << "应该检测到飞机和车的潜在碰撞";
|
||||
if (prediction.willCollide) {
|
||||
// 考虑安全距离的影响，碰撞点会在实际交叉点之前
|
||||
// 飞机从(0,200)出发，速度12m/s，12.79秒到达x=164.22
|
||||
// 车辆从(200,100)出发，速度6m/s，12.79秒到达y=194.63
|
||||
EXPECT_NEAR(prediction.collisionPoint.x, 164.22, 5.0) << "碰撞点x坐标应该考虑安全距离的影响";
|
||||
EXPECT_NEAR(prediction.collisionPoint.y, 194.63, 5.0) << "碰撞点y坐标应该考虑安全距离的影响";
|
||||
|
||||
// 碰撞时间应该是12.79秒左右
|
||||
double expectedTime = 12.79; // 考虑安全距离后的预期碰撞时间
|
||||
EXPECT_NEAR(prediction.timeToCollision, expectedTime, 2.0) << "碰撞时间应该接近预期值";
|
||||
}
|
||||
}
|
||||
// 创建一个航空器和一个车辆
|
||||
Aircraft aircraft;
|
||||
aircraft.flightNo = "TEST001";
|
||||
aircraft.position = {100.0, 100.0};
|
||||
aircraft.speed = 15.0;
|
||||
aircraft.heading = 90.0; // 向东行驶
|
||||
|
||||
// 测试场景7：斜向相交路径
|
||||
{
|
||||
// 第一个物体从西南向东北运动
|
||||
Vector2D pos1{0, 0};
|
||||
double speed1 = 10;
|
||||
double heading1 = 45; // 东北方向（45度）
|
||||
|
||||
// 第二个物体从东南向西北运动
|
||||
Vector2D pos2{200, 0};
|
||||
double speed2 = 10;
|
||||
double heading2 = 315; // 西北方向（315度）
|
||||
|
||||
double size1 = 5;
|
||||
double size2 = 5;
|
||||
double timeWindow = 30;
|
||||
|
||||
Logger::info("斜向相交路径测试参数:");
|
||||
Logger::info(" 物体1: 位置=(" + std::to_string(pos1.x) + ", " + std::to_string(pos1.y) +
|
||||
"), 速度=" + std::to_string(speed1) + ", 航向=" + std::to_string(heading1));
|
||||
Logger::info(" 物体2: 位置=(" + std::to_string(pos2.x) + ", " + std::to_string(pos2.y) +
|
||||
"), 速度=" + std::to_string(speed2) + ", 航向=" + std::to_string(heading2));
|
||||
|
||||
// 计算速度分量
|
||||
double heading1Rad = heading1 * M_PI / 180.0;
|
||||
double heading2Rad = heading2 * M_PI / 180.0;
|
||||
double v1x = speed1 * std::sin(heading1Rad); // 东向为x轴正方向
|
||||
double v1y = speed1 * std::cos(heading1Rad); // 北向为y轴正方向
|
||||
double v2x = speed2 * std::sin(heading2Rad);
|
||||
double v2y = speed2 * std::cos(heading2Rad);
|
||||
|
||||
Logger::info("速度分量:");
|
||||
Logger::info(" 物体1: vx=" + std::to_string(v1x) + ", vy=" + std::to_string(v1y));
|
||||
Logger::info(" 物体2: vx=" + std::to_string(v2x) + ", vy=" + std::to_string(v2y));
|
||||
|
||||
auto prediction = detector_->predictTrajectoryCollision(
|
||||
pos1, speed1, heading1,
|
||||
pos2, speed2, heading2,
|
||||
size1, size2, timeWindow
|
||||
);
|
||||
|
||||
// 验证结果
|
||||
EXPECT_TRUE(prediction.willCollide) << "斜向相交路径应该检测到碰撞";
|
||||
if (prediction.willCollide) {
|
||||
// 考虑安全距离（两个物体各5米），碰撞检测会在实际交叉点之前触发
|
||||
// 由于两个物体速度相同且对称，碰撞点应该在中点附近，但会略低于理想位置
|
||||
EXPECT_NEAR(prediction.collisionPoint.x, 100.0, 5.0) << "碰撞点x坐标应该在预期位置";
|
||||
EXPECT_NEAR(prediction.collisionPoint.y, 95.0, 5.0) << "碰撞点y坐标应该考虑安全距离的影响";
|
||||
|
||||
// 到达碰撞点的时间
|
||||
// 物体1需要移动：√((100-0)² + (95-0)²) ≈ 137.8米
|
||||
// 速度为10m/s，所以时间应该约为13.78秒
|
||||
double expectedTime = 13.78;
|
||||
EXPECT_NEAR(prediction.timeToCollision, expectedTime, 2.0) << "碰撞时间应该接近预期值";
|
||||
}
|
||||
}
|
||||
Vehicle vehicle;
|
||||
vehicle.vehicleNo = "VEH001";
|
||||
vehicle.position = {120.0, 105.0}; // 在航空器前方偏北5米处
|
||||
vehicle.speed = 8.0;
|
||||
vehicle.heading = 270.0; // 向西行驶（与航空器相向而行）
|
||||
|
||||
std::cout << "\n=== 矩形碰撞检测测试（航空器-车辆） ===" << std::endl;
|
||||
std::cout << "配置信息：" << std::endl;
|
||||
std::cout << "- 航空器尺寸: " << SystemConfig::instance().collision_detection.prediction.aircraft_size << "米" << std::endl;
|
||||
std::cout << "- 车辆尺寸: " << SystemConfig::instance().collision_detection.prediction.vehicle_size << "米" << std::endl;
|
||||
|
||||
std::cout << "\n航空器信息：" << std::endl;
|
||||
std::cout << "- 编号: " << aircraft.flightNo << std::endl;
|
||||
std::cout << "- 位置: (" << aircraft.position.x << ", " << aircraft.position.y << ")" << std::endl;
|
||||
std::cout << "- 速度: " << aircraft.speed << "米/秒" << std::endl;
|
||||
std::cout << "- 朝向: " << aircraft.heading << "度" << std::endl;
|
||||
|
||||
std::cout << "\n车辆信息：" << std::endl;
|
||||
std::cout << "- 编号: " << vehicle.vehicleNo << std::endl;
|
||||
std::cout << "- 位置: (" << vehicle.position.x << ", " << vehicle.position.y << ")" << std::endl;
|
||||
std::cout << "- 速度: " << vehicle.speed << "米/秒" << std::endl;
|
||||
std::cout << "- 朝向: " << vehicle.heading << "度" << std::endl;
|
||||
|
||||
// 计算实际距离
|
||||
double dx = vehicle.position.x - aircraft.position.x;
|
||||
double dy = vehicle.position.y - aircraft.position.y;
|
||||
double distance = std::sqrt(dx*dx + dy*dy);
|
||||
std::cout << "\n实际距离: " << distance << "米" << std::endl;
|
||||
|
||||
// 计算相对速度
|
||||
double relativeSpeed = aircraft.speed + vehicle.speed; // 相向而行，速度相加
|
||||
std::cout << "相对速度: " << relativeSpeed << "米/秒" << std::endl;
|
||||
|
||||
// 预计碰撞时间
|
||||
double timeToCollision = distance / relativeSpeed;
|
||||
std::cout << "预计碰撞时间: " << timeToCollision << "秒" << std::endl;
|
||||
|
||||
// 检测碰撞
|
||||
bool hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
|
||||
std::cout << "\n碰撞检测结果: " << (hasCollision ? "是" : "否") << std::endl;
|
||||
|
||||
// 由于距离较近且相向而行，应该检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "相向而行且距离较近的航空器和车辆应该检测到碰撞";
|
||||
|
||||
// 测试边界情况：增加距离
|
||||
vehicle.position.x = 150.0; // 增加到50米距离
|
||||
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
|
||||
EXPECT_FALSE(hasCollision) << "距离较远时不应该检测到碰撞";
|
||||
|
||||
// 测试边界情况：静止状态
|
||||
vehicle.position.x = 110.0; // 距离缩短到10米
|
||||
vehicle.speed = 0.0;
|
||||
aircraft.speed = 0.0;
|
||||
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
|
||||
EXPECT_TRUE(hasCollision) << "静止状态下距离较近时应该检测到碰撞";
|
||||
|
||||
// 测试边界情况：垂直路径
|
||||
vehicle.position = {100.0, 120.0}; // 在航空器正北方
|
||||
vehicle.speed = 10.0;
|
||||
vehicle.heading = 180.0; // 向南行驶
|
||||
aircraft.speed = 10.0;
|
||||
aircraft.heading = 90.0; // 向东行驶
|
||||
hasCollision = detector_->checkRectangleBasedAircraftVehicleCollision(aircraft, vehicle);
|
||||
EXPECT_TRUE(hasCollision) << "垂直路径且距离较近时应该检测到碰撞";
|
||||
}
|
||||
|
||||
TEST_F(CollisionDetectorTest, RectangleCollisionWithRotation) {
|
||||
// 设置系统配置
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
|
||||
|
||||
// 创建两个有一定角度的车辆
|
||||
Vehicle v1;
|
||||
v1.vehicleNo = "VEH001";
|
||||
v1.position = {100.0, 100.0};
|
||||
v1.speed = 10.0;
|
||||
v1.heading = 45.0; // 向东北行驶
|
||||
|
||||
Vehicle v2;
|
||||
v2.vehicleNo = "VEH002";
|
||||
v2.position = {105.0, 105.0}; // 距离缩短到约7.07米
|
||||
v2.speed = 10.0;
|
||||
v2.heading = 315.0; // 向西北行驶
|
||||
|
||||
// 检测碰撞
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
|
||||
// 由于两车轨迹交叉且距离较近，应该检测到碰撞
|
||||
EXPECT_TRUE(hasCollision) << "不同角度的车辆相交时应该检测到碰撞";
|
||||
|
||||
// 将v2移动到更远的位置
|
||||
v2.position = {150.0, 150.0};
|
||||
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
|
||||
// 距离较远时不应该检测到碰撞
|
||||
EXPECT_FALSE(hasCollision) << "距离较远时不应该检测到碰撞";
|
||||
}
|
||||
|
||||
TEST_F(CollisionDetectorTest, RectangleCollisionEdgeCases) {
|
||||
// 设置系统配置
|
||||
SystemConfig::instance().collision_detection.prediction.vehicle_size = 5.0;
|
||||
|
||||
Vehicle v1;
|
||||
v1.vehicleNo = "VEH001";
|
||||
v1.position = {100.0, 100.0};
|
||||
v1.speed = 0.0; // 静止
|
||||
v1.heading = 90.0;
|
||||
|
||||
Vehicle v2;
|
||||
v2.vehicleNo = "VEH002";
|
||||
|
||||
// 测试1：两个静止车辆，刚好接触
|
||||
v2.position = {110.0, 100.0}; // 距离为10米（两车长度和）
|
||||
v2.speed = 0.0;
|
||||
v2.heading = 90.0;
|
||||
|
||||
bool hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
EXPECT_TRUE(hasCollision) << "两个静止且刚好接触的车辆应该检测到碰撞";
|
||||
|
||||
// 测试2：两个静止车辆，距离略大于安全距离
|
||||
v2.position = {111.0, 100.0}; // 距离为11米
|
||||
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
EXPECT_FALSE(hasCollision) << "两个静止且距离大于安全距离的车辆不应该检测到碰撞";
|
||||
|
||||
// 测试3：极低速度
|
||||
v2.position = {120.0, 100.0};
|
||||
v2.speed = 0.1; // 极低速度
|
||||
hasCollision = detector_->checkRectangleBasedVehicleCollision(v1, v2);
|
||||
EXPECT_FALSE(hasCollision) << "距离足够且速度极低的车辆不应该检测到碰撞";
|
||||
}
|
||||
@ -37,8 +37,8 @@
|
||||
.warning { color: #f90; }
|
||||
.command { color: #800080; }
|
||||
.vehicle-icon {
|
||||
width: 10px;
|
||||
height: 10px;
|
||||
width: 20px;
|
||||
height: 20px;
|
||||
background-color: black;
|
||||
clip-path: polygon(0% 0%, 100% 0%, 100% 100%, 0% 100%);
|
||||
border: 2px solid white;
|
||||
@ -51,8 +51,8 @@
|
||||
border: 2px solid white;
|
||||
}
|
||||
.special-vehicle-icon {
|
||||
width: 10px;
|
||||
height: 10px;
|
||||
width: 20px;
|
||||
height: 20px;
|
||||
background-color: orange;
|
||||
clip-path: polygon(0% 0%, 100% 0%, 100% 100%, 0% 100%);
|
||||
border: 2px solid white;
|
||||
@ -65,10 +65,10 @@
|
||||
border: 2px solid white;
|
||||
}
|
||||
.traffic-light {
|
||||
width: 10px;
|
||||
height: 10px;
|
||||
width: 12px;
|
||||
height: 12px;
|
||||
border-radius: 50%;
|
||||
border: 1px solid white;
|
||||
border: 2px solid white;
|
||||
z-index: 1000;
|
||||
}
|
||||
.traffic-light-red {
|
||||
@ -85,6 +85,21 @@
|
||||
text-align: center;
|
||||
white-space: nowrap;
|
||||
}
|
||||
.command-text {
|
||||
position: absolute;
|
||||
top: 0;
|
||||
left: 0;
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
display: flex;
|
||||
justify-content: center;
|
||||
align-items: center;
|
||||
color: white;
|
||||
font-size: 10px;
|
||||
font-weight: bold;
|
||||
pointer-events: none;
|
||||
z-index: 1000;
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
@ -117,7 +132,7 @@
|
||||
};
|
||||
const EAST_INTERSECTION = {
|
||||
latitude: 36.361999,
|
||||
longitude: 120.089003
|
||||
longitude: 120.090003
|
||||
};
|
||||
|
||||
// 存储所有标记
|
||||
@ -126,22 +141,38 @@
|
||||
const intersections = new Map();
|
||||
|
||||
// 创建自定义图标
|
||||
function createIcon(className) {
|
||||
function createIcon(className, command = '') {
|
||||
let size;
|
||||
if (className.includes('aircraft')) {
|
||||
size = [50, 50]; // 50米正方形
|
||||
} else if (className.includes('vehicle')) {
|
||||
size = [10, 10]; // 10米正方形
|
||||
size = [20, 20]; // 10米正方形
|
||||
} else if (className.includes('traffic-light')) {
|
||||
size = [10, 10]; // 10像素的红绿灯
|
||||
size = [12, 12]; // 10像素的红绿灯
|
||||
} else {
|
||||
size = [20, 20]; // 其他图标保持原样
|
||||
}
|
||||
|
||||
// 如果有指令，创建带指令的图标
|
||||
if (command && className === 'vehicle-icon') {
|
||||
const html = `
|
||||
<div style="width:${size[0]}px;height:${size[1]}px;position:relative;">
|
||||
<div class="${className}"></div>
|
||||
<div class="command-text">${command}</div>
|
||||
</div>`;
|
||||
return L.divIcon({
|
||||
html: html,
|
||||
className: '',
|
||||
iconSize: size,
|
||||
iconAnchor: [size[0]/2, size[1]/2]
|
||||
});
|
||||
}
|
||||
|
||||
// 没有指令时，创建普通图标
|
||||
return L.divIcon({
|
||||
className: className,
|
||||
iconSize: size,
|
||||
iconAnchor: [size[0]/2, size[1]/2] // 设置图标锚点为中心
|
||||
iconAnchor: [size[0]/2, size[1]/2]
|
||||
});
|
||||
}
|
||||
|
||||
@ -207,6 +238,48 @@
|
||||
}
|
||||
}
|
||||
|
||||
function updateVehicleCommand(vehicleId, commandType) {
|
||||
console.log('更新车辆指令:', vehicleId, commandType);
|
||||
|
||||
// 只处理无人车
|
||||
if (!vehicleId.startsWith('QN')) {
|
||||
return;
|
||||
}
|
||||
|
||||
// 如果是 SIGNAL 指令，不更新显示
|
||||
if (commandType === 'SIGNAL') {
|
||||
console.log('忽略 SIGNAL 指令');
|
||||
return;
|
||||
}
|
||||
|
||||
// 获取指令字母
|
||||
let commandText = '';
|
||||
switch(commandType) {
|
||||
case 'ALERT':
|
||||
commandText = 'A';
|
||||
break;
|
||||
case 'WARNING':
|
||||
commandText = 'W';
|
||||
break;
|
||||
case 'RESUME':
|
||||
commandText = 'R';
|
||||
break;
|
||||
default:
|
||||
commandText = '';
|
||||
}
|
||||
|
||||
console.log('指令文本:', commandText);
|
||||
|
||||
// 更新图标
|
||||
const marker = markers.get(vehicleId);
|
||||
if (marker && commandText) {
|
||||
console.log('设置新图标:', vehicleId, commandText);
|
||||
marker.setIcon(createIcon('vehicle-icon', commandText));
|
||||
} else if (marker) {
|
||||
marker.setIcon(createIcon('vehicle-icon'));
|
||||
}
|
||||
}
|
||||
|
||||
function connect() {
|
||||
if (ws) {
|
||||
log('已经连接，请先断开', 'error');
|
||||
@ -232,11 +305,6 @@
|
||||
ws.onmessage = (event) => {
|
||||
try {
|
||||
const data = JSON.parse(event.data);
|
||||
const formattedData = JSON.stringify(data, null, 2);
|
||||
|
||||
// 根据消息类型使用不同的样式和处理方式
|
||||
let type = 'info';
|
||||
let message = '收到消息:\n' + formattedData;
|
||||
|
||||
switch (data.type) {
|
||||
case 'position_update':
|
||||
@ -251,6 +319,8 @@
|
||||
break;
|
||||
case 'vehicle_command':
|
||||
type = 'command';
|
||||
console.log('收到指令消息:', data); // 调试日志
|
||||
updateVehicleCommand(data.vehicleId, data.commandType);
|
||||
// 为控制指令添加中文描述
|
||||
const commandTypes = {
|
||||
'SIGNAL': '信号灯指令',
|
||||
@ -275,6 +345,8 @@
|
||||
break;
|
||||
}
|
||||
|
||||
const formattedData = JSON.stringify(data, null, 2);
|
||||
let message = '收到消息:\n' + formattedData;
|
||||
log(message, type);
|
||||
} catch (e) {
|
||||
log('收到消息: ' + event.data, 'info');
|
||||
@ -326,7 +398,7 @@
|
||||
|
||||
// 添加距离标签（以米为单位）
|
||||
const distance = Math.abs(Math.round(i / step) * 50);
|
||||
if (distance > 0) { // 只显示非零<EFBFBD><EFBFBD><EFBFBD>离
|
||||
if (distance > 0) { // 只显示非零距离
|
||||
const label = L.divIcon({
|
||||
className: 'distance-label',
|
||||
html: distance + 'm',
|
||||
|
||||
@ -36,11 +36,15 @@ DIST_50M = 50
|
||||
|
||||
# 两个路口的位置
|
||||
WEST_INTERSECTION = {"longitude": 120.086003, "latitude": 36.361999}
|
||||
EAST_INTERSECTION = {"longitude": 120.089003, "latitude": 36.361999}
|
||||
EAST_INTERSECTION = {"longitude": 120.090003, "latitude": 36.361999}
|
||||
|
||||
# 位置更新间隔（秒）
|
||||
UPDATE_INTERVAL = 1.0
|
||||
|
||||
# 飞机和车辆尺寸（半径 米）
|
||||
AIRCRAFT_SIZE_M = 30.0
|
||||
VEHICLE_SIZE_M = 10.0
|
||||
|
||||
# 航空器数据
|
||||
aircraft_data = [
|
||||
{
|
||||
@ -120,9 +124,9 @@ class VehicleState:
|
||||
if self.current_command == "ALERT":
|
||||
return command_type == "RESUME"
|
||||
|
||||
# RED 指令不能被 RESUME 解除
|
||||
# RED 指令可以被 GREEN 或更高优先级指令覆盖
|
||||
if self.current_command == "RED":
|
||||
return new_priority >= current_priority
|
||||
return command_type == "GREEN" or new_priority > current_priority
|
||||
|
||||
# WARNING 指令可以被 RESUME 解除
|
||||
if self.current_command == "WARNING":
|
||||
@ -150,19 +154,27 @@ class VehicleState:
|
||||
self.target_lat = target_lat
|
||||
self.target_lon = target_lon
|
||||
|
||||
# 如果是红绿灯状态，只更新状态不改变当前指令
|
||||
# 如果是红绿灯状态，更新状态和指令
|
||||
if command_type in ["RED", "GREEN"]:
|
||||
self.traffic_light_state = command_type
|
||||
if command_type == "RED":
|
||||
self.current_command = command_type
|
||||
self.command_priority = priority_map[command_type]
|
||||
self.is_running = False
|
||||
print(f"车辆 {self.vehicle_no} 收到红灯指令，停止运行")
|
||||
else: # GREEN
|
||||
# 如果没有其他阻塞性指令，则清除当前指令
|
||||
if self.current_command not in ["ALERT", "WARNING", "RED"]:
|
||||
self.current_command = None
|
||||
self.command_priority = 0
|
||||
# 清除 RED 指令
|
||||
if self.current_command == "RED":
|
||||
print(f"车辆 {self.vehicle_no} 收到绿灯指令，清除红灯指令")
|
||||
|
||||
# 设置为绿灯指令
|
||||
self.current_command = command_type
|
||||
self.command_priority = priority_map[command_type]
|
||||
|
||||
# 如果没有其他阻塞性指令，允许移动
|
||||
if self.current_command not in ["ALERT", "WARNING"]:
|
||||
self.is_running = True
|
||||
print(f"车辆 {self.vehicle_no} 收到绿灯指令，恢复运行")
|
||||
else:
|
||||
# 其他指令正常更新
|
||||
self.current_command = command_type
|
||||
@ -172,12 +184,16 @@ class VehicleState:
|
||||
|
||||
def can_move(self):
|
||||
"""检查车辆是否可以移动"""
|
||||
# 如果是红灯，不能移动
|
||||
if self.traffic_light_state == "RED":
|
||||
# 如果有告警指令，不能移动
|
||||
if self.current_command == "ALERT":
|
||||
return False
|
||||
|
||||
# 如果有告警或预警指令，不能移动
|
||||
if self.current_command in ["ALERT", "WARNING"]:
|
||||
# 如果有预警指令，不能移动
|
||||
if self.current_command == "WARNING":
|
||||
return False
|
||||
|
||||
# 如果是红灯且当前指令是 RED，不能移动
|
||||
if self.traffic_light_state == "RED" and self.current_command == "RED":
|
||||
return False
|
||||
|
||||
# 其他情况可以移动
|
||||
@ -256,9 +272,9 @@ def handle_vehicle_command():
|
||||
"status": "ok",
|
||||
"message": "Special vehicle only responds to traffic light signals"
|
||||
})
|
||||
# 更新红绿灯状态
|
||||
vehicle_state.traffic_light_state = command_type
|
||||
print(f"特勤车 {vehicle_id} 更新红绿灯状态: {command_type}")
|
||||
# 更新红绿灯状态和指令状态
|
||||
vehicle_state.update_command(command_type, target_lat, target_lon)
|
||||
print(f"特勤车 {vehicle_id} 更新状态: command={command_type}, traffic_light={vehicle_state.traffic_light_state}")
|
||||
return jsonify({
|
||||
"status": "ok",
|
||||
"message": "Traffic light state updated"
|
||||
@ -273,9 +289,9 @@ def handle_vehicle_command():
|
||||
print(f"指令优先级过低: vehicle={vehicle_id}, current_priority={vehicle_state.command_priority}, "
|
||||
f"command={command_type}")
|
||||
return jsonify({
|
||||
"status": "error",
|
||||
"status": "warning",
|
||||
"message": "Command priority too low"
|
||||
}), 400
|
||||
})
|
||||
|
||||
# 处理不同类型的指令
|
||||
if command_type in ["ALERT", "WARNING"]:
|
||||
@ -322,7 +338,27 @@ def handle_vehicle_command():
|
||||
vehicle_state.brake_mode = None
|
||||
vehicle_state.target_lat = None
|
||||
vehicle_state.target_lon = None
|
||||
|
||||
|
||||
# 更新车辆运行状态
|
||||
vehicle_state.is_running = vehicle_state.can_move()
|
||||
if vehicle_state.is_running:
|
||||
print(f"车辆 {vehicle_id} 恢复运行")
|
||||
current_vehicle["speed"] = DEFAULT_VEHICLE_SPEED
|
||||
|
||||
# 记录状态变化但不更新指令
|
||||
vehicle_state.command_reason = reason
|
||||
vehicle_state.last_command_time = time.time()
|
||||
|
||||
print(f"Vehicle {vehicle_id} state updated: running={vehicle_state.is_running}, "
|
||||
f"command={vehicle_state.current_command}, traffic_light={vehicle_state.traffic_light_state}, "
|
||||
f"reason={reason}, priority={vehicle_state.command_priority}, "
|
||||
f"target_speed={vehicle_state.target_speed}, brake_mode={vehicle_state.brake_mode}")
|
||||
|
||||
return jsonify({
|
||||
"status": "ok",
|
||||
"message": "Command executed successfully"
|
||||
})
|
||||
|
||||
# 更新车辆状态
|
||||
vehicle_state.update_command(command_type, target_lat, target_lon)
|
||||
vehicle_state.command_reason = reason
|
||||
@ -382,37 +418,30 @@ def get_front_traffic_light(vehicle, distance_to_west, distance_to_east):
|
||||
|
||||
# QN001 的路线：西路口北侧 -> 东 -> 北
|
||||
if vehicle["phase"] == 0: # 南北移动
|
||||
if vehicle["direction"] == -1: # 向南
|
||||
# 在西路口以南时，向北移动需要判断西路口红绿灯
|
||||
if vehicle["direction"] == 1 and vehicle["latitude"] < WEST_INTERSECTION["latitude"]:
|
||||
return traffic_light_data[0], distance_to_west # 西路口红绿灯
|
||||
else: # 向北
|
||||
# 在西路口以北时，向南移动需要判断西路口红绿灯
|
||||
elif vehicle["direction"] == -1 and vehicle["latitude"] > WEST_INTERSECTION["latitude"]:
|
||||
return traffic_light_data[0], distance_to_west # 西路口红绿灯
|
||||
else: # 东西移动
|
||||
if vehicle["direction"] == 1: # 向东
|
||||
return None, float('inf') # 已过西路口，无红绿灯
|
||||
else: # 向西
|
||||
# 在西路口以西时，向东移动需要判断西路口红绿灯
|
||||
if vehicle["direction"] == 1 and vehicle["longitude"] < WEST_INTERSECTION["longitude"]:
|
||||
return traffic_light_data[0], distance_to_west # 西路口红绿灯
|
||||
elif vehicle["vehicleNo"] == "QN002":
|
||||
if vehicle["direction"] == 1: # 向东
|
||||
return traffic_light_data[1], distance_to_east # 东路口红绿灯
|
||||
else: # 向西
|
||||
return traffic_light_data[1], distance_to_east # 东路口红绿灯
|
||||
elif vehicle["vehicleNo"] == "TQ001":
|
||||
# 特勤车：先东西后南北
|
||||
if "phase" not in vehicle:
|
||||
vehicle["phase"] = 0
|
||||
|
||||
if vehicle["phase"] == 0: # 东西移动
|
||||
if vehicle["direction"] == 1: # 向东
|
||||
return None, float('inf') # 已过西路口，无红绿灯
|
||||
else: # 向西
|
||||
# 在西路口以东时，向西移动需要判断西路口红绿灯
|
||||
elif vehicle["direction"] == -1 and vehicle["longitude"] > WEST_INTERSECTION["longitude"]:
|
||||
return traffic_light_data[0], distance_to_west # 西路口红绿灯
|
||||
else: # 南北移动
|
||||
if vehicle["direction"] == 1: # 向北
|
||||
return traffic_light_data[0], distance_to_west # 西路口红绿灯
|
||||
else: # 向南
|
||||
return None, float('inf') # 已过西路口，无红绿灯
|
||||
|
||||
return None, float('inf') # 默认返回无红绿灯
|
||||
elif vehicle["vehicleNo"] == "QN002":
|
||||
# 在东路口以西时，向东移动需要判断东路口红绿灯
|
||||
if vehicle["direction"] == 1 and vehicle["longitude"] < EAST_INTERSECTION["longitude"]:
|
||||
return traffic_light_data[1], distance_to_east # 东路口红绿灯
|
||||
# 在东路口以东时，向西移动需要判断东路口红绿灯
|
||||
elif vehicle["direction"] == -1 and vehicle["longitude"] > EAST_INTERSECTION["longitude"]:
|
||||
return traffic_light_data[1], distance_to_east # 东路口红绿灯
|
||||
|
||||
# 其他情况，表示车辆已经通过路口或不需要判断红绿灯
|
||||
return None, float('inf')
|
||||
|
||||
def update_vehicle_position(vehicle, elapsed_time):
|
||||
"""更新车辆位置"""
|
||||
@ -426,23 +455,36 @@ def update_vehicle_position(vehicle, elapsed_time):
|
||||
vehicle["phase"] = 0
|
||||
print(f"初始化车辆 {vehicle['vehicleNo']} 的 phase 属性为 0")
|
||||
|
||||
# 获取前方红绿灯状态和距离
|
||||
traffic_light, distance = get_front_traffic_light(vehicle,
|
||||
calculate_distance_to_intersection(vehicle, WEST_INTERSECTION),
|
||||
calculate_distance_to_intersection(vehicle, EAST_INTERSECTION))
|
||||
|
||||
# 特勤车只响应红绿灯
|
||||
if vehicle["vehicleNo"].startswith("TQ"):
|
||||
# 只有红灯时才停止
|
||||
if vehicle_state.traffic_light_state == "RED":
|
||||
# 红灯且距离停止线小于等于 50 米时停车
|
||||
if traffic_light and traffic_light["state"] == 0 and distance <= DIST_50M:
|
||||
vehicle["speed"] = 0
|
||||
print(f"特勤车 {vehicle['vehicleNo']} 遇红灯停止")
|
||||
print(f"特勤车 {vehicle['vehicleNo']} 遇红灯在停止线前停车，距路口 {distance:.1f}米")
|
||||
return
|
||||
# 其他情况正常行驶
|
||||
vehicle["speed"] = DEFAULT_VEHICLE_SPEED
|
||||
print(f"特勤车 {vehicle['vehicleNo']} 正常行驶，速度={vehicle['speed']}km/h")
|
||||
else:
|
||||
# 普通车辆的移动逻辑
|
||||
if not vehicle_state.can_move():
|
||||
vehicle["speed"] = 0
|
||||
print(f"车辆 {vehicle['vehicleNo']} 不能移动: command={vehicle_state.current_command}, "
|
||||
f"traffic_light={vehicle_state.traffic_light_state}")
|
||||
return
|
||||
# 先判断是否在红灯影响范围内（50米）
|
||||
if traffic_light and traffic_light["state"] == 0 and distance <= DIST_50M:
|
||||
# 在红灯影响范围内，检查是否需要停车
|
||||
if not vehicle_state.can_move():
|
||||
vehicle["speed"] = 0
|
||||
print(f"车辆 {vehicle['vehicleNo']} 遇红灯在停止线前停车，距路口 {distance:.1f}米")
|
||||
return
|
||||
else:
|
||||
# 不在红灯影响范围内，只检查其他指令
|
||||
if vehicle_state.current_command in ["ALERT", "WARNING"]:
|
||||
vehicle["speed"] = 0
|
||||
print(f"车辆 {vehicle['vehicleNo']} 不能移动: command={vehicle_state.current_command}")
|
||||
return
|
||||
|
||||
# 可以移动，设置正常速度
|
||||
vehicle["speed"] = DEFAULT_VEHICLE_SPEED
|
||||
@ -457,6 +499,7 @@ def update_vehicle_position(vehicle, elapsed_time):
|
||||
# 计算经纬度变化
|
||||
dlat, dlon = meters_to_degrees(distance, vehicle["latitude"])
|
||||
|
||||
# 更新位置
|
||||
if vehicle["vehicleNo"].startswith("QN"):
|
||||
# 无人车的路径更新逻辑
|
||||
if vehicle["vehicleNo"] == "QN001":
|
||||
@ -547,24 +590,6 @@ def update_aircraft_position(aircraft, elapsed_time):
|
||||
aircraft["direction"] = 1 # 向北
|
||||
aircraft["latitude"] = new_lat
|
||||
|
||||
def update_traffic_light_for_aircraft():
|
||||
"""根据航空器位置更新东路口红绿灯状态"""
|
||||
if not aircraft_data:
|
||||
return
|
||||
|
||||
aircraft = aircraft_data[0]
|
||||
# 计算到东路口的距离（米）
|
||||
lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9
|
||||
|
||||
# 更新TL002（东路口）的状态
|
||||
for light in traffic_light_data:
|
||||
if light["id"] == "TL002":
|
||||
if lat_diff <= 50.0: # 距离小于50米时为红灯
|
||||
light["state"] = 1 # 红灯
|
||||
else:
|
||||
light["state"] = 1 # 绿灯
|
||||
break
|
||||
|
||||
# 定义路口信息
|
||||
INTERSECTIONS = {
|
||||
"TL001": {
|
||||
@ -624,22 +649,38 @@ def get_flight_positions():
|
||||
|
||||
return jsonify(aircraft_data)
|
||||
|
||||
def switch_traffic_light_state():
|
||||
"""统一处理红绿灯状态切换"""
|
||||
global last_light_switch_time
|
||||
current_time = time.time()
|
||||
|
||||
# 西路口红绿灯每15秒切换一次
|
||||
elapsed_since_switch = current_time - last_light_switch_time
|
||||
if elapsed_since_switch >= 15:
|
||||
traffic_light_data[0]["state"] = 1 if traffic_light_data[0]["state"] == 0 else 0
|
||||
last_light_switch_time = current_time
|
||||
print(f"西路口红绿灯状态切换为: {'绿灯' if traffic_light_data[0]['state'] == 1 else '红灯'}")
|
||||
|
||||
# 更新东路口红绿灯（根据航空器位置）
|
||||
if aircraft_data:
|
||||
aircraft = aircraft_data[0]
|
||||
lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9
|
||||
|
||||
old_state = traffic_light_data[1]["state"]
|
||||
traffic_light_data[1]["state"] = 1 if lat_diff > (AIRCRAFT_SIZE_M + DIST_50M) else 0
|
||||
|
||||
if old_state != traffic_light_data[1]["state"]:
|
||||
print(f"东路口红绿灯状态切换为: {'绿灯' if traffic_light_data[1]['state'] == 1 else '红灯'}")
|
||||
|
||||
@app.route('/api/getCurrentVehiclePositions')
|
||||
def get_vehicle_positions():
|
||||
global last_vehicle_update_time, last_light_switch_time
|
||||
global last_vehicle_update_time
|
||||
current_time = time.time()
|
||||
elapsed_time = current_time - last_vehicle_update_time
|
||||
|
||||
try:
|
||||
# 更新红绿灯状态
|
||||
elapsed_since_switch = current_time - last_light_switch_time
|
||||
if elapsed_since_switch >= 1500: # 每15秒切换一次
|
||||
traffic_light_data[0]["state"] = 1 if traffic_light_data[0]["state"] == 0 else 1
|
||||
last_light_switch_time = current_time
|
||||
print(f"西路口红绿灯状态切换为: {'绿灯' if traffic_light_data[0]['state'] == 1 else '红灯'}")
|
||||
|
||||
# 根据航空器位置更新东路口红绿灯
|
||||
update_traffic_light_for_aircraft()
|
||||
# 统一处理红绿灯状态切换
|
||||
switch_traffic_light_state()
|
||||
|
||||
# 只在达到更新间隔时更新位置
|
||||
if elapsed_time >= UPDATE_INTERVAL:
|
||||
@ -656,35 +697,14 @@ def get_vehicle_positions():
|
||||
|
||||
@app.route('/api/getTrafficLightSignals')
|
||||
def get_traffic_light_signals():
|
||||
global last_light_switch_time
|
||||
current_time = time.time()
|
||||
|
||||
# 更新时间戳
|
||||
for light in traffic_light_data:
|
||||
light["timestamp"] = int(current_time * 1000)
|
||||
|
||||
# TL001（西路口）状态每15秒切换一次
|
||||
elapsed_since_switch = current_time - last_light_switch_time
|
||||
if elapsed_since_switch >= 1500: # 每15秒切换一次
|
||||
traffic_light_data[0]["state"] = 1 if traffic_light_data[0]["state"] == 0 else 1
|
||||
last_light_switch_time = current_time
|
||||
print(f"西路口红绿灯状态切换为: {'绿灯' if traffic_light_data[0]['state'] == 1 else '红灯'}")
|
||||
|
||||
# 根据航空器位置更新TL002（东路口）
|
||||
if aircraft_data:
|
||||
aircraft = aircraft_data[0]
|
||||
# 计算到东路口的距离（米）
|
||||
lat_diff = abs(aircraft["latitude"] - EAST_INTERSECTION["latitude"]) * 111319.9
|
||||
|
||||
# 更新TL002（东路口）的状态
|
||||
old_state = traffic_light_data[1]["state"]
|
||||
if lat_diff <= 50.0: # 距离小于50米时为红灯
|
||||
traffic_light_data[1]["state"] = 1 # 红灯
|
||||
else:
|
||||
traffic_light_data[1]["state"] = 1 # 绿灯
|
||||
|
||||
if old_state != traffic_light_data[1]["state"]:
|
||||
print(f"东路口红绿灯状态切换为: {'绿灯' if traffic_light_data[1]['state'] == 1 else '红灯'}")
|
||||
# 统一处理红绿灯状态切换
|
||||
switch_traffic_light_state()
|
||||
|
||||
return jsonify(traffic_light_data)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user