682 lines
27 KiB
C++
682 lines
27 KiB
C++
#include <gtest/gtest.h>
|
||
#include <gmock/gmock.h>
|
||
#include <memory>
|
||
#include <unordered_set>
|
||
#include <vector>
|
||
|
||
#include "detector/CollisionDetector.h"
|
||
#include "vehicle/ControllableVehicles.h"
|
||
#include "utils/Logger.h"
|
||
#include "config/SystemConfig.h"
|
||
#include "config/AirportBounds.h"
|
||
|
||
namespace {
|
||
std::unordered_set<std::string> g_registered_test_vehicle_ids;
|
||
|
||
void RegisterManagedVehicle(const std::string& vehicleId) {
|
||
ControllableVehicles::getInstance().updateRegistry({
|
||
VehicleRegistryEntry{vehicleId, "WUREN"}
|
||
});
|
||
g_registered_test_vehicle_ids.insert(vehicleId);
|
||
}
|
||
|
||
void CleanupRegisteredVehicles() {
|
||
if (g_registered_test_vehicle_ids.empty()) {
|
||
return;
|
||
}
|
||
|
||
std::vector<VehicleRegistryEntry> cleanupEntries;
|
||
cleanupEntries.reserve(g_registered_test_vehicle_ids.size());
|
||
for (const auto& vehicleId : g_registered_test_vehicle_ids) {
|
||
cleanupEntries.push_back(VehicleRegistryEntry{vehicleId, "PUTONG"});
|
||
}
|
||
ControllableVehicles::getInstance().updateRegistry(cleanupEntries);
|
||
g_registered_test_vehicle_ids.clear();
|
||
}
|
||
}
|
||
|
||
// Mock ControllableVehicles 类
|
||
class MockControllableVehicles {
|
||
public:
|
||
MockControllableVehicles() = default;
|
||
|
||
MOCK_METHOD(bool, isControllable, (const std::string& vehicleNo), (const));
|
||
MOCK_METHOD(const ControllableVehicleConfig*, findVehicle, (const std::string& vehicleNo), (const));
|
||
MOCK_METHOD(void, sendCommand, (const std::string& vehicleNo, const VehicleCommand& cmd));
|
||
|
||
// 转换为 ControllableVehicles& 的操作符
|
||
operator const ControllableVehicles&() const {
|
||
return ControllableVehicles::getInstance();
|
||
}
|
||
};
|
||
|
||
// Mock AirportBounds 类
|
||
class MockAirportBounds : public AirportBounds {
|
||
public:
|
||
MockAirportBounds() : AirportBounds("") {
|
||
// 设置测试区域边界
|
||
airportBounds_ = Bounds(0, 0, 5000, 4000);
|
||
areaBounds_[AreaType::TEST_ZONE] = airportBounds_;
|
||
|
||
// 设置测试区域配置
|
||
AreaConfig config;
|
||
config.collision_radius = {50.0, 50.0, 25.0}; // aircraft, special, unmanned
|
||
config.height_threshold = 10.0;
|
||
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));
|
||
|
||
const AreaConfig& getAreaConfig(AreaType type) const override {
|
||
auto it = areaConfigs_.find(type);
|
||
if (it == areaConfigs_.end()) {
|
||
throw std::runtime_error("Invalid area type");
|
||
}
|
||
return it->second;
|
||
}
|
||
};
|
||
|
||
class BasicCollisionTest : public ::testing::Test {
|
||
protected:
|
||
void SetUp() override {
|
||
Logger::setLogLevel(LogLevel::DEBUG); // 设置日志级别为 DEBUG
|
||
|
||
airportBounds_ = std::make_unique<MockAirportBounds>();
|
||
mockControllableVehicles_ = std::make_unique<MockControllableVehicles>();
|
||
|
||
// 设置 Mock 对象的行为
|
||
EXPECT_CALL(*airportBounds_, getAreaType(::testing::_))
|
||
.WillRepeatedly(::testing::Return(AreaType::TEST_ZONE));
|
||
|
||
// 设置 Mock ControllableVehicles 的行为
|
||
EXPECT_CALL(*mockControllableVehicles_, isControllable(::testing::_))
|
||
.WillRepeatedly(::testing::Return(true));
|
||
|
||
detector_ = std::make_unique<CollisionDetector>(*airportBounds_, *mockControllableVehicles_);
|
||
}
|
||
|
||
void TearDown() override {
|
||
CleanupRegisteredVehicles();
|
||
}
|
||
|
||
std::unique_ptr<MockAirportBounds> airportBounds_;
|
||
std::unique_ptr<MockControllableVehicles> mockControllableVehicles_;
|
||
std::unique_ptr<CollisionDetector> detector_;
|
||
};
|
||
|
||
// 1. 静态碰撞测试
|
||
TEST_F(BasicCollisionTest, StaticCollision) {
|
||
// 创建两个静止且重叠的物体
|
||
Vehicle v1;
|
||
v1.vehicleNo = "V1";
|
||
v1.position = {100.0, 100.0};
|
||
v1.speed = 0.0;
|
||
v1.heading = 0.0;
|
||
v1.type = MovingObjectType::UNMANNED;
|
||
|
||
Vehicle v2;
|
||
v2.vehicleNo = "V2";
|
||
v2.position = {120.0, 100.0}; // 距离20米，小于碰撞半径25米
|
||
v2.speed = 0.0;
|
||
v2.heading = 0.0;
|
||
v2.type = MovingObjectType::UNMANNED;
|
||
|
||
auto result = detector_->checkCollision(v1, v2, 30.0);
|
||
|
||
EXPECT_TRUE(result.willCollide) << "距离小于碰撞半径的静止物体应该检测为碰撞";
|
||
EXPECT_DOUBLE_EQ(result.timeToCollision, 0.0) << "静止物体的碰撞时间应该为0";
|
||
EXPECT_EQ(result.type, CollisionType::STATIC) << "应该识别为静态碰撞";
|
||
|
||
// 增加更多验证
|
||
EXPECT_DOUBLE_EQ(result.minDistance, 20.0) << "最小距离应该是当前距离20米";
|
||
EXPECT_DOUBLE_EQ(result.timeToMinDistance, 0.0) << "静止物体最小距离时间应该为0";
|
||
EXPECT_NEAR(result.collisionPoint.x, 110.0, 0.1) << "碰撞点应该在两车中点";
|
||
EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点应该在同一水平线上";
|
||
|
||
// 验证物体状态
|
||
EXPECT_NEAR(result.object1State.position.x, 100.0, 0.1);
|
||
EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object1State.speed, 0.0);
|
||
EXPECT_DOUBLE_EQ(result.object1State.heading, 0.0);
|
||
|
||
EXPECT_NEAR(result.object2State.position.x, 120.0, 0.1);
|
||
EXPECT_NEAR(result.object2State.position.y, 100.0, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object2State.speed, 0.0);
|
||
EXPECT_DOUBLE_EQ(result.object2State.heading, 0.0);
|
||
}
|
||
|
||
// 2. 相向碰撞测试
|
||
TEST_F(BasicCollisionTest, HeadOnCollision) {
|
||
// 创建两个相向运动的物体
|
||
Vehicle v1;
|
||
v1.vehicleNo = "V1";
|
||
v1.position = {100.0, 100.0};
|
||
v1.speed = 10.0;
|
||
v1.heading = 90.0; // 向东
|
||
v1.type = MovingObjectType::UNMANNED;
|
||
|
||
Vehicle v2;
|
||
v2.vehicleNo = "V2";
|
||
v2.position = {200.0, 100.0}; // 在v1东边100米
|
||
v2.speed = 10.0;
|
||
v2.heading = 270.0; // 向西
|
||
v2.type = MovingObjectType::UNMANNED;
|
||
|
||
auto result = detector_->checkCollision(v1, v2, 30.0);
|
||
|
||
EXPECT_TRUE(result.willCollide) << "相向运动的物体应该检测为碰撞";
|
||
EXPECT_NEAR(result.timeToCollision, 2.5, 0.1) << "碰撞时间应该接近2.5秒((100-50)米/20米每秒)";
|
||
EXPECT_EQ(result.type, CollisionType::HEAD_ON) << "应该识别为相向碰撞";
|
||
EXPECT_NEAR(result.collisionPoint.x, 150.0, 0.1) << "碰撞点应该在两车中点处(100+200)/2=150";
|
||
EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点应该在同一水平线上";
|
||
|
||
// 增加更多验证
|
||
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离该是碰撞半径之和50米";
|
||
EXPECT_NEAR(result.timeToMinDistance, 2.5, 0.1) << "最小距离时间应该等于碰撞时间";
|
||
|
||
// 验证物体状态
|
||
EXPECT_NEAR(result.object1State.position.x, 125.0, 0.1);
|
||
EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
|
||
EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0);
|
||
|
||
EXPECT_NEAR(result.object2State.position.x, 175.0, 0.1);
|
||
EXPECT_NEAR(result.object2State.position.y, 100.0, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0);
|
||
EXPECT_DOUBLE_EQ(result.object2State.heading, 270.0);
|
||
}
|
||
|
||
// 3. 平行运动测试
|
||
TEST_F(BasicCollisionTest, ParallelMotion) {
|
||
// 创建两个平行运动的物体
|
||
Vehicle v1;
|
||
v1.vehicleNo = "V1";
|
||
v1.position = {100.0, 100.0};
|
||
v1.speed = 10.0;
|
||
v1.heading = 90.0; // 向东
|
||
v1.type = MovingObjectType::UNMANNED;
|
||
|
||
Vehicle v2;
|
||
v2.vehicleNo = "V2";
|
||
v2.position = {100.0, 150.0}; // 在v1北边50米，大于碰撞半径
|
||
v2.speed = 10.0;
|
||
v2.heading = 90.0; // 向东
|
||
v2.type = MovingObjectType::UNMANNED;
|
||
|
||
auto result = detector_->checkCollision(v1, v2, 30.0);
|
||
|
||
EXPECT_FALSE(result.willCollide) << "平行运动且距离大于碰撞半径的物体不应该检测为碰撞";
|
||
EXPECT_EQ(result.type, CollisionType::PARALLEL) << "应该识别为平行运动";
|
||
|
||
// 增加更多验证
|
||
EXPECT_DOUBLE_EQ(result.minDistance, 50.0) << "最小距离应该是初始距离50米";
|
||
EXPECT_DOUBLE_EQ(result.timeToMinDistance, 0.0) << "平行运动的最小距离时间应该为0";
|
||
EXPECT_DOUBLE_EQ(result.timeToCollision, std::numeric_limits<double>::infinity()) << "平行运动不会碰撞";
|
||
|
||
// 不验证碰撞点，因为不会发生碰撞
|
||
}
|
||
|
||
// 4. 交叉路径测试
|
||
TEST_F(BasicCollisionTest, CrossingPaths) {
|
||
// 创建两个倾斜交叉运动的物体
|
||
Vehicle v1;
|
||
v1.vehicleNo = "V1";
|
||
v1.position = {100.0, 100.0};
|
||
v1.speed = 15.0;
|
||
v1.heading = 75.0; // 向东北偏东方向运动
|
||
v1.type = MovingObjectType::UNMANNED;
|
||
|
||
Vehicle v2;
|
||
v2.vehicleNo = "V2";
|
||
v2.position = {150.0, 150.0};
|
||
v2.speed = 10.0;
|
||
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, CollisionType::CROSSING) << "应该识别为交叉碰撞";
|
||
|
||
// 计算碰撞时间和位置：
|
||
// 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 = 2.07; // 根据实际计算得到
|
||
Vector2D collision_point = {126.34, 150.006}; // 两车位置的中点
|
||
|
||
EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，碰撞时间应该接近2.07秒";
|
||
EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在126.34";
|
||
EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在150.006";
|
||
|
||
// 增加更多验证
|
||
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米";
|
||
EXPECT_NEAR(result.timeToMinDistance, collision_time, 0.1) << "最小距离时间应该等于碰撞时间";
|
||
|
||
// 验证物体状态
|
||
EXPECT_NEAR(result.object1State.position.x, 108.04, 0.1);
|
||
EXPECT_NEAR(result.object1State.position.y, 130.007, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object1State.speed, 15.0);
|
||
EXPECT_DOUBLE_EQ(result.object1State.heading, 75.0);
|
||
|
||
EXPECT_NEAR(result.object2State.position.x, 144.64, 0.1);
|
||
EXPECT_NEAR(result.object2State.position.y, 170.005, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0);
|
||
EXPECT_DOUBLE_EQ(result.object2State.heading, 105.0);
|
||
}
|
||
|
||
// 5. 垂直交叉路径测试
|
||
TEST_F(BasicCollisionTest, PerpendicularCrossingPaths) {
|
||
// 创建两个垂直交叉运动的物体
|
||
Vehicle v1;
|
||
v1.vehicleNo = "V1";
|
||
v1.position = {100.0, 100.0};
|
||
v1.speed = 10.0;
|
||
v1.heading = 90.0; // 向东运动
|
||
v1.type = MovingObjectType::UNMANNED;
|
||
|
||
Vehicle v2;
|
||
v2.vehicleNo = "V2";
|
||
v2.position = {150.0, 150.0};
|
||
v2.speed = 10.0;
|
||
v2.heading = 180.0; // 向南运动，与 v1 夹角为 90 度
|
||
v2.type = MovingObjectType::UNMANNED;
|
||
|
||
auto result = detector_->checkCollision(v1, v2, 30.0);
|
||
|
||
EXPECT_TRUE(result.willCollide) << "垂直交叉路径的物体应该检测为碰撞";
|
||
EXPECT_EQ(result.type, CollisionType::CROSSING) << "应该识别为交叉碰撞";
|
||
|
||
// 计算碰撞时间和位置：
|
||
// v1: 速度分量 (10.0, 0.0) m/s
|
||
// v2: 速度分量 (0.0, -10.0) m/s
|
||
// 相对速度: (-10.0, -10.0) m/s
|
||
// 相对速度大小: 14.14 m/s
|
||
// 当前距离小于安全距离时，立即判定为碰撞
|
||
double collision_time = 0.0; // 当前距离小于安全距离，立即碰撞
|
||
Vector2D collision_point = {125.0, 125.0}; // 两车当前位置的中点
|
||
|
||
EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "当前距离小于安全距离，碰撞时间应该为0";
|
||
EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在125.0";
|
||
EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在125.0";
|
||
|
||
// 增加更多验证
|
||
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米";
|
||
EXPECT_NEAR(result.timeToMinDistance, collision_time, 0.1) << "最小距离时间应该等于碰撞时间";
|
||
|
||
// 验证物体状态
|
||
EXPECT_NEAR(result.object1State.position.x, 100.0, 0.1);
|
||
EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
|
||
EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0);
|
||
|
||
EXPECT_NEAR(result.object2State.position.x, 150.0, 0.1);
|
||
EXPECT_NEAR(result.object2State.position.y, 150.0, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object2State.speed, 10.0);
|
||
EXPECT_DOUBLE_EQ(result.object2State.heading, 180.0);
|
||
}
|
||
|
||
TEST_F(BasicCollisionTest, DivergentMotion) {
|
||
// 设置两个物体背向运动的场景
|
||
// 物体1在(150,100)，向右运动，航向90度
|
||
// 物体2在(200,100)，向右运动，航向90度
|
||
// 两物体都在远离对方，不应该发生碰撞
|
||
|
||
Vehicle obj1;
|
||
obj1.vehicleNo = "V1";
|
||
obj1.position = {150, 100};
|
||
obj1.speed = 10;
|
||
obj1.heading = 90; // 右运动
|
||
obj1.type = MovingObjectType::UNMANNED;
|
||
|
||
Vehicle obj2;
|
||
obj2.vehicleNo = "V2";
|
||
obj2.position = {300, 100}; // 增加初始距离到 150m
|
||
obj2.speed = 10;
|
||
obj2.heading = 90; // 也向右运动
|
||
obj2.type = MovingObjectType::SPECIAL;
|
||
|
||
auto result = detector_->checkCollision(obj1, obj2, 10.0);
|
||
|
||
// 验证结果
|
||
EXPECT_FALSE(result.willCollide) << "同向运动且距离大于安全距离的物体不应该发生碰撞";
|
||
EXPECT_EQ(result.type, CollisionType::PARALLEL) << "应该识别为平行运动";
|
||
|
||
// 初始距离应该是 150m
|
||
EXPECT_NEAR(result.minDistance, 150.0, 0.1);
|
||
|
||
// 由于两车都以相同速度向右运动，最小距离应该保持不变
|
||
EXPECT_NEAR(result.timeToMinDistance, 0.0, 0.1);
|
||
}
|
||
|
||
// 6. 追尾场景测试
|
||
TEST_F(BasicCollisionTest, TailgatingMotion) {
|
||
// 创建两个同向运动的物体，后车速度大于前车
|
||
Vehicle v1; // 前车
|
||
v1.vehicleNo = "V1";
|
||
v1.position = {60.0, 100.0}; // 前车在前方60米处
|
||
v1.speed = 10.0; // 前车速度10m/s
|
||
v1.heading = 90.0; // 向东运动
|
||
v1.type = MovingObjectType::UNMANNED;
|
||
|
||
Vehicle v2; // 后车
|
||
v2.vehicleNo = "V2";
|
||
v2.position = {0.0, 100.0}; // 后车在原点
|
||
v2.speed = 15.0; // 后车速度15m/s
|
||
v2.heading = 90.0; // 向东运动
|
||
v2.type = MovingObjectType::UNMANNED;
|
||
|
||
auto result = detector_->checkCollision(v1, v2, 30.0);
|
||
|
||
// 验证碰撞类型
|
||
EXPECT_EQ(result.type, CollisionType::PARALLEL) << "应该识别为平行运动";
|
||
|
||
// 验证会发生碰撞
|
||
EXPECT_TRUE(result.willCollide) << "后车速度大于前车，应该预测到碰撞";
|
||
|
||
// 验证碰撞时间（初始距离60米，相对速度5m/s，安全距离50米，需要缩短10米，所以碰撞时间应该是2秒）
|
||
EXPECT_NEAR(result.timeToCollision, 2.0, 0.1) << "碰撞时间应该接近2秒";
|
||
|
||
// 验证最小距离（应该是安全距离）
|
||
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是安全距离50米";
|
||
|
||
// 验证最小距离时间（应该等于碰撞时间）
|
||
EXPECT_NEAR(result.timeToMinDistance, 2.0, 0.1) << "最小距离时间应该等于碰撞时间";
|
||
|
||
// 验证碰撞点（在两车碰撞时的中点）
|
||
// 前车：初始位置 60 + 10 * 2 = 80
|
||
// 后车：初始位置 0 + 15 * 2 = 30
|
||
// 碰撞点应该在 (80 + 30) / 2 = 55
|
||
EXPECT_NEAR(result.collisionPoint.x, 55.0, 0.1) << "碰撞点x坐标应该在55米处";
|
||
EXPECT_NEAR(result.collisionPoint.y, 100.0, 0.1) << "碰撞点y坐标应该保持在100米";
|
||
|
||
// 验证碰撞时刻的物体状态
|
||
// 前车位置：60 + 10 * 2 = 80
|
||
EXPECT_NEAR(result.object1State.position.x, 80.0, 0.1);
|
||
EXPECT_NEAR(result.object1State.position.y, 100.0, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
|
||
EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0);
|
||
|
||
// 后车位置：0 + 15 * 2 = 30
|
||
EXPECT_NEAR(result.object2State.position.x, 30.0, 0.1);
|
||
EXPECT_NEAR(result.object2State.position.y, 100.0, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object2State.speed, 15.0);
|
||
EXPECT_DOUBLE_EQ(result.object2State.heading, 90.0);
|
||
}
|
||
|
||
// 6. QN002 与 AC001 交叉路径测试（实际场景）
|
||
TEST_F(BasicCollisionTest, QN002AircraftCrossing) {
|
||
// 创建 AC001 航空器，从 T7 出发
|
||
Aircraft aircraft;
|
||
aircraft.flightNo = "AC001";
|
||
aircraft.position = {0.0, 0.0}; // T7 点的相对坐标
|
||
aircraft.speed = 50.0 / 3.6; // 转换为米/秒
|
||
aircraft.heading = 45.0; // 根据 T7 到 T11 的方向计算
|
||
aircraft.type = MovingObjectType::AIRCRAFT;
|
||
|
||
// 创建 QN002 无人车，从 T4 到 T8 路径上的一点发
|
||
Vehicle qn002;
|
||
qn002.vehicleNo = "QN002";
|
||
qn002.position = {-2.0, 120.0}; // 在 T4-T8 路径上，距离 T7 约 120 米（小于检测范围 150 米）
|
||
qn002.speed = 36.0 / 3.6; // 转换为米/秒
|
||
qn002.heading = 135.0; // 根据 T4 到 T8 的方向计算
|
||
qn002.type = MovingObjectType::UNMANNED;
|
||
qn002.isControllable = true;
|
||
|
||
// 检测碰撞
|
||
auto result = detector_->checkCollision(aircraft, qn002, 30.0);
|
||
|
||
// 验证是否检测到碰撞
|
||
EXPECT_TRUE(result.willCollide) << "QN002 与 AC001 的交叉路径应该检测为碰撞";
|
||
EXPECT_EQ(result.type, CollisionType::CROSSING) << "应该识别为交叉碰撞";
|
||
|
||
// 记录详细的测试信息
|
||
Logger::debug("QN002-AC001 碰撞测试结果:");
|
||
Logger::debug("碰撞类型: ", static_cast<int>(result.type));
|
||
Logger::debug("最小距离: ", result.minDistance, "m");
|
||
Logger::debug("碰撞时间: ", result.timeToCollision, "s");
|
||
Logger::debug("碰撞点: (", result.collisionPoint.x, ",", result.collisionPoint.y, ")");
|
||
|
||
// 验证碰撞参数
|
||
EXPECT_GT(result.timeToCollision, 0.0) << "碰撞时间应该大于0";
|
||
EXPECT_LT(result.timeToCollision, 30.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 + 160.0, crossPoint.y}; // 航空器在交叉点东侧160米（大于安全距离150米）
|
||
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()) << "航空器已远离，无人车继续运动不应产生新的冲突";
|
||
}
|
||
|
||
TEST_F(BasicCollisionTest, DivergingReleaseDistanceResolvesConflictImmediately) {
|
||
RegisterManagedVehicle("UT_VEH_DIVERGE_1");
|
||
ASSERT_TRUE(detector_->setDivergingReleaseDistance(50.0));
|
||
|
||
Aircraft aircraft;
|
||
aircraft.id = "UT_AC_DIVERGE_1";
|
||
aircraft.flightNo = "UT_AC_DIVERGE_1";
|
||
aircraft.type = MovingObjectType::AIRCRAFT;
|
||
aircraft.altitude = 0.0;
|
||
aircraft.position = {100.0, 100.0};
|
||
aircraft.speed = 10.0;
|
||
aircraft.heading = 90.0;
|
||
|
||
Vehicle vehicle;
|
||
vehicle.id = "UT_VEH_DIVERGE_1";
|
||
vehicle.vehicleNo = "UT_VEH_DIVERGE_1";
|
||
vehicle.position = {180.0, 100.0};
|
||
vehicle.speed = 10.0;
|
||
vehicle.heading = 270.0;
|
||
|
||
detector_->updateTraffic({aircraft}, {vehicle});
|
||
auto risks = detector_->detectCollisions();
|
||
ASSERT_FALSE(risks.empty()) << "首帧应先建立冲突记录";
|
||
|
||
// 第二帧：距离在 50m 以上且正在远离，且不满足旧距离兜底(1.5 * warning = 225m)
|
||
aircraft.position = {200.0, 100.0};
|
||
aircraft.speed = 5.0;
|
||
aircraft.heading = 270.0;
|
||
vehicle.position = {120.0, 100.0};
|
||
vehicle.speed = 15.0;
|
||
vehicle.heading = 270.0;
|
||
|
||
detector_->updateTraffic({aircraft}, {vehicle});
|
||
risks = detector_->detectCollisions();
|
||
EXPECT_TRUE(risks.empty()) << "远离且达到解除距离时应立即解除冲突";
|
||
}
|
||
|
||
TEST_F(BasicCollisionTest, ApproachingAboveReleaseDistanceDoesNotRelease) {
|
||
RegisterManagedVehicle("UT_VEH_APPROACH_1");
|
||
ASSERT_TRUE(detector_->setDivergingReleaseDistance(50.0));
|
||
|
||
Aircraft aircraft;
|
||
aircraft.id = "UT_AC_APPROACH_1";
|
||
aircraft.flightNo = "UT_AC_APPROACH_1";
|
||
aircraft.type = MovingObjectType::AIRCRAFT;
|
||
aircraft.altitude = 0.0;
|
||
aircraft.position = {100.0, 100.0};
|
||
aircraft.speed = 10.0;
|
||
aircraft.heading = 90.0;
|
||
|
||
Vehicle vehicle;
|
||
vehicle.id = "UT_VEH_APPROACH_1";
|
||
vehicle.vehicleNo = "UT_VEH_APPROACH_1";
|
||
vehicle.position = {180.0, 100.0};
|
||
vehicle.speed = 10.0;
|
||
vehicle.heading = 270.0;
|
||
|
||
detector_->updateTraffic({aircraft}, {vehicle});
|
||
auto risks = detector_->detectCollisions();
|
||
ASSERT_FALSE(risks.empty()) << "首帧应先建立冲突记录";
|
||
|
||
// 第二帧：当前距离 149m(>50m) 但处于接近态，且 30s 内不会碰撞
|
||
aircraft.position = {200.0, 100.0};
|
||
aircraft.speed = 1.0;
|
||
aircraft.heading = 270.0;
|
||
vehicle.position = {51.0, 100.0};
|
||
vehicle.speed = 1.0;
|
||
vehicle.heading = 90.0;
|
||
|
||
detector_->updateTraffic({aircraft}, {vehicle});
|
||
risks = detector_->detectCollisions();
|
||
EXPECT_FALSE(risks.empty()) << "接近态不应被远离解除距离规则误解除";
|
||
}
|
||
|
||
TEST_F(BasicCollisionTest, LegacyReleaseFallbackStillWorksWhenNotDiverging) {
|
||
RegisterManagedVehicle("UT_VEH_FALLBACK_1");
|
||
ASSERT_TRUE(detector_->setDivergingReleaseDistance(50.0));
|
||
|
||
Aircraft aircraft;
|
||
aircraft.id = "UT_AC_FALLBACK_1";
|
||
aircraft.flightNo = "UT_AC_FALLBACK_1";
|
||
aircraft.type = MovingObjectType::AIRCRAFT;
|
||
aircraft.altitude = 0.0;
|
||
aircraft.position = {100.0, 100.0};
|
||
aircraft.speed = 10.0;
|
||
aircraft.heading = 90.0;
|
||
|
||
Vehicle vehicle;
|
||
vehicle.id = "UT_VEH_FALLBACK_1";
|
||
vehicle.vehicleNo = "UT_VEH_FALLBACK_1";
|
||
vehicle.position = {180.0, 100.0};
|
||
vehicle.speed = 10.0;
|
||
vehicle.heading = 270.0;
|
||
|
||
detector_->updateTraffic({aircraft}, {vehicle});
|
||
auto risks = detector_->detectCollisions();
|
||
ASSERT_FALSE(risks.empty()) << "首帧应先建立冲突记录";
|
||
|
||
// 第二帧：不处于远离态，但满足旧逻辑的 farEnoughAndNoRisk(>=225m)
|
||
aircraft.position = {300.0, 100.0};
|
||
aircraft.speed = 1.0;
|
||
aircraft.heading = 270.0;
|
||
vehicle.position = {70.0, 100.0};
|
||
vehicle.speed = 1.0;
|
||
vehicle.heading = 90.0;
|
||
|
||
detector_->updateTraffic({aircraft}, {vehicle});
|
||
risks = detector_->detectCollisions();
|
||
EXPECT_TRUE(risks.empty()) << "旧逻辑兜底应继续生效并解除冲突";
|
||
}
|