363 lines
15 KiB
C++
363 lines
15 KiB
C++
#include "detector/CollisionDetector.h"
|
||
#include "vehicle/ControllableVehicles.h"
|
||
#include "config/AirportBounds.h"
|
||
#include <gtest/gtest.h>
|
||
#include <gmock/gmock.h>
|
||
#include "utils/Logger.h"
|
||
|
||
// Mock ControllableVehicles 类
|
||
class MockControllableVehicles : public ControllableVehicles {
|
||
public:
|
||
MockControllableVehicles() : ControllableVehicles("") {}
|
||
MOCK_METHOD(bool, isControllable, (const std::string& vehicleNo), (const));
|
||
};
|
||
|
||
// 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;
|
||
}
|
||
|
||
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));
|
||
|
||
detector_ = std::make_unique<CollisionDetector>(*airportBounds_, *mockControllableVehicles_);
|
||
}
|
||
|
||
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) << "距离小于碰撞半径的静止物<EFBFBD><EFBFBD>应该检测为碰撞";
|
||
EXPECT_DOUBLE_EQ(result.timeToCollision, 0.0) << "静止物体的碰撞时间应该为0";
|
||
EXPECT_EQ(result.type, collision::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, collision::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, collision::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 = 10.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;
|
||
|
||
auto result = detector_->checkCollision(v1, v2, 30.0);
|
||
|
||
EXPECT_TRUE(result.willCollide) << "交叉路径的物体应该检测为碰撞";
|
||
EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞";
|
||
|
||
// 计算碰撞时间和位置：
|
||
// v1: 速度分量 (2.59, 9.66) m/s
|
||
// v2: 速度分量 (-2.59, 9.66) m/s
|
||
// 相对速度: (-5.18, 0) m/s
|
||
// 相对速度大小: 5.18 m/s
|
||
// 碰撞点在两车轨迹交点前的安全距离处
|
||
double collision_time = 6.12; // 根据实际计算得到
|
||
Vector2D collision_point = {125.0, 184.15}; // 根据实际计算得到
|
||
|
||
EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25米，撞时间应该接近6.12秒";
|
||
EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在125.0";
|
||
EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在184.15";
|
||
|
||
// 增加更多验证
|
||
EXPECT_NEAR(result.minDistance, 50.0, 0.1) << "最小距离应该是碰撞半径之和50米";
|
||
EXPECT_NEAR(result.timeToMinDistance, collision_time, 0.1) << "最小距离时间应该等于碰撞时间";
|
||
|
||
// 验证物体状态
|
||
EXPECT_NEAR(result.object1State.position.x, 115.85, 0.1);
|
||
EXPECT_NEAR(result.object1State.position.y, 159.15, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
|
||
EXPECT_DOUBLE_EQ(result.object1State.heading, 75.0);
|
||
|
||
EXPECT_NEAR(result.object2State.position.x, 134.15, 0.1);
|
||
EXPECT_NEAR(result.object2State.position.y, 209.15, 0.1);
|
||
EXPECT_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, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞";
|
||
|
||
// 计算碰撞时间和位置：
|
||
// v1: 速度分量 (0.0, 10.0) m/s
|
||
// v2: 速度分量 (-10.0, 0.0) m/s
|
||
// 相对速度: (-10.0, -10.0) m/s
|
||
// 相对速度大小: 14.14 m/s
|
||
// 由于碰撞半径为 25m，实际碰撞会提前发生
|
||
double collision_time = 1.46; // 根据实际计算得到
|
||
Vector2D collision_point = {117.68, 132.32}; // 根据实际计算得到
|
||
|
||
EXPECT_NEAR(result.timeToCollision, collision_time, 0.1) << "考虑碰撞半径25<EFBFBD><EFBFBD>，碰时间应该接近1.46秒";
|
||
EXPECT_NEAR(result.collisionPoint.x, collision_point.x, 0.1) << "碰撞点x坐标应该在117.68";
|
||
EXPECT_NEAR(result.collisionPoint.y, collision_point.y, 0.1) << "碰撞点y坐标应该在132.32";
|
||
|
||
// 增加更多验证
|
||
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, 114.64, 0.1);
|
||
EXPECT_DOUBLE_EQ(result.object1State.speed, 10.0);
|
||
EXPECT_DOUBLE_EQ(result.object1State.heading, 90.0);
|
||
|
||
EXPECT_NEAR(result.object2State.position.x, 135.36, 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, collision::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, collision::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);
|
||
}
|