322 lines
13 KiB
C++
322 lines
13 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"
|
||
#include <chrono>
|
||
#include <filesystem>
|
||
|
||
// 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;
|
||
}
|
||
|
||
MOCK_CONST_METHOD1(getAreaType, AreaType(const Vector2D& position));
|
||
|
||
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;
|
||
}
|
||
|
||
const Bounds& getAirportBounds() const override {
|
||
return airportBounds_;
|
||
}
|
||
};
|
||
|
||
class CollisionDetectorTest : public ::testing::Test {
|
||
protected:
|
||
void SetUp() override {
|
||
// 设置日志级别为 DEBUG
|
||
Logger::initialize("", LogLevel::DEBUG);
|
||
|
||
// 打印当前工作目录
|
||
std::cout << "Current working directory: " << std::filesystem::current_path() << std::endl;
|
||
|
||
// 创建 Mock 对象
|
||
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_);
|
||
}
|
||
|
||
void TearDown() override {
|
||
Logger::initialize("", LogLevel::INFO);
|
||
}
|
||
|
||
std::unique_ptr<MockAirportBounds> airportBounds_;
|
||
std::unique_ptr<MockControllableVehicles> mockControllableVehicles_;
|
||
std::unique_ptr<CollisionDetector> detector_;
|
||
};
|
||
|
||
// 测试可控车辆（无人车）与航空器的碰撞检测
|
||
TEST_F(CollisionDetectorTest, UnmannedVehicleAircraftCollision) {
|
||
// 设置测试数据
|
||
Aircraft aircraft;
|
||
aircraft.flightNo = "TEST001";
|
||
aircraft.position = {100.0, 100.0};
|
||
aircraft.speed = 15.0;
|
||
aircraft.heading = 90.0; // 向东行驶
|
||
aircraft.type = MovingObjectType::AIRCRAFT;
|
||
|
||
Vehicle vehicle;
|
||
vehicle.vehicleNo = "VEH001";
|
||
vehicle.position = {120.0, 105.0}; // 在航空器前方偏北5米处
|
||
vehicle.speed = 8.0;
|
||
vehicle.heading = 270.0; // 向西行驶（与航空器相向而行）
|
||
vehicle.type = MovingObjectType::UNMANNED;
|
||
vehicle.isControllable = true;
|
||
|
||
// 测试1：相向而行且距离较近
|
||
auto collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "相向而行且距离较近的航空器和无人车应该检测到碰撞";
|
||
|
||
// 测试2：增加距离到边界值
|
||
vehicle.position = {150.0, 105.0}; // 增加到50米距离
|
||
collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "在警告距离内应该检测到碰撞";
|
||
|
||
// 测试3：超出碰撞检测范围
|
||
vehicle.position = {900.0, 100.0}; // 增加到800米距离
|
||
collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
|
||
EXPECT_FALSE(collisionResult.willCollide) << "距离较远时不应该检测到碰撞";
|
||
}
|
||
|
||
// 测试无人车与特勤车的碰撞检测
|
||
TEST_F(CollisionDetectorTest, UnmannedVehicleSpecialVehicleCollision) {
|
||
Vehicle unmanned;
|
||
unmanned.vehicleNo = "VEH001";
|
||
unmanned.position = {100.0, 100.0};
|
||
unmanned.speed = 10.0;
|
||
unmanned.heading = 90.0; // 向东行驶
|
||
unmanned.type = MovingObjectType::UNMANNED;
|
||
unmanned.isControllable = true;
|
||
|
||
Vehicle special;
|
||
special.vehicleNo = "VEH002";
|
||
special.position = {120.0, 100.0}; // 距离20米
|
||
special.speed = 5.0;
|
||
special.heading = 270.0; // 向西行驶
|
||
special.type = MovingObjectType::SPECIAL;
|
||
special.isControllable = false;
|
||
|
||
// 测试1：相向而行且距离较近
|
||
auto collisionResult = detector_->checkCollision(unmanned, special, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "相向而行且距离较近的无人车和特勤车应该检测到碰撞";
|
||
|
||
// 测试2：在警告距离边界
|
||
special.position = {150.0, 100.0}; // 增加到50米距离
|
||
collisionResult = detector_->checkCollision(unmanned, special, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "在警告距离应该检测到碰撞";
|
||
|
||
// 测试3：超出碰撞检测范围
|
||
special.position = {300.0, 100.0}; // 增加到200米距离
|
||
collisionResult = detector_->checkCollision(unmanned, special, 30.0);
|
||
EXPECT_FALSE(collisionResult.willCollide) << "超出警告距离时不应该检测到碰撞";
|
||
}
|
||
|
||
// 测试斜向叉路径碰撞
|
||
TEST_F(CollisionDetectorTest, DiagonalCrossingPathsCollision) {
|
||
Vehicle v1;
|
||
v1.vehicleNo = "VEH001";
|
||
v1.position = {100.0, 100.0};
|
||
v1.speed = 10.0;
|
||
v1.heading = 45.0; // 北行驶
|
||
v1.type = MovingObjectType::UNMANNED;
|
||
v1.isControllable = true;
|
||
|
||
Vehicle v2;
|
||
v2.vehicleNo = "VEH002";
|
||
v2.position = {150.0, 150.0};
|
||
v2.speed = 10.0;
|
||
v2.heading = 225.0; // 向西南行驶（与v1相向）
|
||
v2.type = MovingObjectType::UNMANNED;
|
||
v2.isControllable = true;
|
||
|
||
// 测试1：路径交叉且距离较近
|
||
auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "斜向相向而行且路径交叉的车辆应该检测到碰撞";
|
||
|
||
// 测试2：不同速度比
|
||
v1.speed = 5.0; // 降低速度
|
||
v2.speed = 15.0; // 提高速度
|
||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "不同速度比的斜向相向车辆应该检测到碰撞";
|
||
|
||
// 测试3：超出碰撞检测范围
|
||
v2.position = {300.0, 300.0}; // 增加到较远距离
|
||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_FALSE(collisionResult.willCollide) << "距离较远时不应该检测到碰撞";
|
||
}
|
||
|
||
// 测试垂直交叉路径碰撞
|
||
TEST_F(CollisionDetectorTest, PerpendicularCrossingPathsCollision) {
|
||
Vehicle v1;
|
||
v1.vehicleNo = "VEH001";
|
||
v1.position = {100.0, 100.0};
|
||
v1.speed = 10.0;
|
||
v1.heading = 90.0; // 向东行驶
|
||
v1.type = MovingObjectType::UNMANNED;
|
||
v1.isControllable = true;
|
||
|
||
Vehicle v2;
|
||
v2.vehicleNo = "VEH002";
|
||
v2.position = {120.0, 120.0}; // 在v1东北方20米处
|
||
v2.speed = 10.0;
|
||
v2.heading = 180.0; // 向南行驶
|
||
v2.type = MovingObjectType::UNMANNED;
|
||
v2.isControllable = true;
|
||
|
||
// 测试1：垂直相交且距离较近
|
||
auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "垂直交叉路径的车辆应该检测到碰撞";
|
||
|
||
// 测试2：不同速度比
|
||
v1.speed = 5.0; // 降低速度
|
||
v2.speed = 15.0; // 提高速度
|
||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "不同速度的垂直交叉路径车辆应该检测到碰撞";
|
||
|
||
// 测试3：超出碰撞检测范围
|
||
v2.position = {200.0, 200.0}; // 增加到较远距离
|
||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_FALSE(collisionResult.willCollide) << "距离较远时应该检测到碰撞";
|
||
}
|
||
|
||
// 测试静止辆的碰撞检测
|
||
TEST_F(CollisionDetectorTest, StationaryVehiclesCollision) {
|
||
Vehicle v1;
|
||
v1.vehicleNo = "VEH001";
|
||
v1.position = {100.0, 100.0};
|
||
v1.speed = 0.0; // 静止
|
||
v1.heading = 90.0;
|
||
v1.type = MovingObjectType::UNMANNED;
|
||
v1.isControllable = true;
|
||
|
||
Vehicle v2;
|
||
v2.vehicleNo = "VEH002";
|
||
v2.position = {105.0, 100.0}; // 距离5米
|
||
v2.speed = 0.0; // 静止
|
||
v2.heading = 90.0;
|
||
v2.type = MovingObjectType::UNMANNED;
|
||
v2.isControllable = true;
|
||
|
||
// 测试1：两个静止车辆距离小于安全距离
|
||
auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "距离小于安全距离的静止车辆应该检测到碰撞";
|
||
|
||
// 测试2：一动一静，移动车辆接近静止车辆
|
||
v2.position = {120.0, 100.0}; // 增加距离到20米
|
||
v2.speed = 5.0; // 开始移动
|
||
v2.heading = 270.0; // 向西行驶（朝向v1）
|
||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "移动车辆接近静止车辆时应该检测到碰撞";
|
||
|
||
// 测试3：一动一静，移动车辆远离静止车辆
|
||
v2.position = {160.0, 100.0}; // 增加到60米，大于安全距离(50米)
|
||
v2.speed = 5.0; // 开始移动
|
||
v2.heading = 90.0; // 向东行驶（远离v1）
|
||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_FALSE(collisionResult.willCollide) << "移动车辆远离静止车辆且距离于安全距离时不应该检测到碰撞";
|
||
}
|
||
|
||
// 测试同向运动的碰撞检测
|
||
TEST_F(CollisionDetectorTest, TailgatingCollision) {
|
||
Vehicle v1;
|
||
v1.vehicleNo = "VEH001";
|
||
v1.position = {100.0, 100.0};
|
||
v1.speed = 10.0;
|
||
v1.heading = 90.0; // 向东行驶
|
||
v1.type = MovingObjectType::UNMANNED;
|
||
v1.isControllable = true;
|
||
|
||
Vehicle v2;
|
||
v2.vehicleNo = "VEH002";
|
||
v2.position = {40.0, 100.0}; // 在v1西侧60米处（100-60=40）
|
||
v2.speed = 10.0;
|
||
v2.heading = 90.0; // 也向东行驶
|
||
v2.type = MovingObjectType::UNMANNED;
|
||
v2.isControllable = true;
|
||
|
||
// 测试1：同向同速
|
||
auto collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_FALSE(collisionResult.willCollide) << "同向同速的车辆不应该检测到碰撞";
|
||
|
||
// 测试2：同向不同速（v2速度更快，会追上v1）
|
||
v2.speed = 15.0;
|
||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "同向但速度较快的车辆追上前车时应该检测到碰撞";
|
||
|
||
// 测试3：同向不同速但距离较远
|
||
v2.position = {0.0, 100.0}; // 在原点
|
||
v2.speed = 11.0; // 减小速度差，从 15m/s 改为 11m/s
|
||
collisionResult = detector_->checkCollision(v1, v2, 30.0);
|
||
EXPECT_FALSE(collisionResult.willCollide) << "相对速度小且距离较远时不应该检测到碰撞";
|
||
}
|
||
|
||
// 测试航空器与静止车辆的碰撞检测
|
||
TEST_F(CollisionDetectorTest, AircraftStationaryVehicleCollision) {
|
||
Aircraft aircraft;
|
||
aircraft.flightNo = "TEST001";
|
||
aircraft.position = {100.0, 100.0};
|
||
aircraft.speed = 15.0;
|
||
aircraft.heading = 90.0; // 向东行驶
|
||
aircraft.type = MovingObjectType::AIRCRAFT;
|
||
|
||
Vehicle vehicle;
|
||
vehicle.vehicleNo = "VEH001";
|
||
vehicle.position = {150.0, 100.0}; // 在航空器前方50米处
|
||
vehicle.speed = 0.0; // 静止
|
||
vehicle.heading = 90.0;
|
||
vehicle.type = MovingObjectType::UNMANNED;
|
||
vehicle.isControllable = true;
|
||
|
||
// 测试1：航空器接近静止车辆
|
||
auto collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
|
||
EXPECT_TRUE(collisionResult.willCollide) << "航空器接近静止车辆时应该检测到碰撞";
|
||
|
||
// 测试2：静止车辆在航空器航向偏离处
|
||
vehicle.position = {200.0, 200.0}; // 在航空器前方偏北，距离约100米（大于安全距离75米）
|
||
collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
|
||
EXPECT_FALSE(collisionResult.willCollide) << "航空器与不在航向上的静止车辆不应该检测到碰撞";
|
||
|
||
// 测试3：静止车辆在航空器后方
|
||
vehicle.position = {0.0, 100.0}; // 在航空器后方100米（大于安全距离75米）
|
||
collisionResult = detector_->checkCollision(aircraft, vehicle, 30.0);
|
||
EXPECT_FALSE(collisionResult.willCollide) << "航空器与后方的静止车辆不应该检测到碰撞";
|
||
}
|