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

删除命名空间

Browse Source
This commit is contained in:
Tian jianyong 2024-12-20 15:55:37 +08:00
parent c1a25b863a
commit 6d697bcc2c
11 changed files with 66 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/config/AirportBounds.h
View File

@ -1,9 +1,11 @@
#ifndef AIRPORT_SPATIAL_AIRPORT_BOUNDS_H
#define AIRPORT_SPATIAL_AIRPORT_BOUNDS_H
#ifndef AIRPORT_BOUNDS_H
#define AIRPORT_BOUNDS_H
#include <string>
#include <vector>
#include <stdexcept>
#include "types/BasicTypes.h"
#include "spatial/QuadTree.h"
#include "AreaConfig.h"
#include <string>
#include <unordered_map>
// 机场区域定义
@ -39,4 +41,4 @@ protected:
virtual void loadConfig(const std::string& configFile);
};
#endif // AIRPORT_SPATIAL_AIRPORT_BOUNDS_H
#endif // AIRPORT_BOUNDS_H

28
src/core/System.cpp
View File

@ -112,7 +112,7 @@ void System::initializeSafetyZones() {
);
// 创建安全区
auto safetyZone = std::make_unique<collision::SafetyZone>(
auto safetyZone = std::make_unique<SafetyZone>(
center,
intersection.safetyZone.aircraftRadius, // 飞机安全区半径
intersection.safetyZone.vehicleRadius // 特勤车安全区半径
@ -177,7 +177,7 @@ void System::processLoop() {
int64_t last_vehicle_timestamp = 0;
int64_t last_collision_timestamp = 0;
int64_t last_traffic_light_timestamp = 0;
int64_t last_safety_zone_timestamp = 0; // 添加<EFBFBD><EFBFBD><EFBFBD>全区时间戳
int64_t last_safety_zone_timestamp = 0; // 添加全区时间戳
Logger::debug("数据处理循环启动");
@ -328,7 +328,7 @@ void System::processLoop() {
// auto collision_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
// now - last_collision_update).count();
// if (collision_elapsed >= system_config.collision_detection.update_interval_ms) {
// // 只有有新的数据时才更新冲突检<EFBFBD><EFBFBD>
// // 只有有新的数据时才更新冲突检测
// if (last_aircraft_timestamp > last_collision_timestamp ||
// last_vehicle_timestamp > last_collision_timestamp) {
@ -341,14 +341,14 @@ void System::processLoop() {
// for (const auto& risk : collisions) {
// Logger::debug("碰撞风险详情: id1=", risk.id1,
// ", id2=", risk.id2,
// ", 距离=", risk.distance, "m, 预测最小距离=", risk.minDistance, "m, <EFBFBD><EFBFBD><EFBFBD>险等级=", static_cast<int>(risk.level),
// ", 距离=", risk.distance, "m, 预测最小距离=", risk.minDistance, "m, 风险等级=", static_cast<int>(risk.level),
// ", 区域类型=", static_cast<int>(risk.zoneType));
// }
// processCollisions(collisions);
// } else if (!lastVehiclesWithRisk_.empty()) {
// // 当前没有任何风险<EFBFBD><EFBFBD><EFBFBD><EFBFBD>上次有风险车辆，需要处理恢复指令
// Logger::debug("当前无碰撞风险，<EFBFBD><EFBFBD><EFBFBD>查是否需要发送恢复指令");
// // 当前没有任何风险上次有风险车辆，需要处理恢复指令
// Logger::debug("当前无碰撞风险，检查是否需要发送恢复指令");
// for (const auto& vehicleId : lastVehiclesWithRisk_) {
// Logger::debug("车辆 ", vehicleId, " 当前没有风险，准备发送恢复指令");
// VehicleCommand cmd;
@ -522,7 +522,7 @@ void System::processCollisions(const std::vector<CollisionRisk>& risks) {
if (id2_controllable) {
currentVehiclesWithRisk.insert(risk.id2);
Logger::debug("添加当前有风险的可控车辆: ", risk.id2,
", 当风险车辆数量: ", currentVehiclesWithRisk.size());
", 当前风险车辆数量: ", currentVehiclesWithRisk.size());
processVehicle(risk.id2, risk.id1);
}
@ -531,7 +531,7 @@ void System::processCollisions(const std::vector<CollisionRisk>& risks) {
}
Logger::debug("当前有风险的可控车辆数量: ", currentVehiclesWithRisk.size());
Logger::debug("上一次有风险的可车辆数量: ", lastVehiclesWithRisk_.size());
Logger::debug("上一次有风险的可控车辆数量: ", lastVehiclesWithRisk_.size());
// 对之前有风险但现在没有风险的车辆发送恢复指令
for (const auto& vehicleId : lastVehiclesWithRisk_) {
@ -761,7 +761,7 @@ const MovingObject* System::findVehicle(const std::string& vehicleId) const {
void System::updateSafetyZoneStates(const std::vector<MovingObject*>& objects) {
// 重置所有安全区状态为未激活，同时重置类型
for (auto& [id, zone] : safetyZones_) {
zone->setState(collision::SafetyZoneState::INACTIVE);
zone->setState(SafetyZoneState::INACTIVE);
zone->resetType(); // 重置安全区类型
}
@ -799,9 +799,9 @@ void System::checkSafetyZoneIntrusion(const MovingObject& obj) {
for (auto& [id, zone] : safetyZones_) {
// 检查是否在安全区内，同时会尝试设置安全区类型
if (zone->isObjectInZone(obj)) {
zone->setState(collision::SafetyZoneState::ACTIVE);
zone->setState(SafetyZoneState::ACTIVE);
Logger::debug("目标 ", obj.id, " 进入路口 ", id, " 安全区, 类型: ",
zone->getType() == collision::SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车",
zone->getType() == SafetyZoneType::AIRCRAFT ? "飞机" : "特勤车",
", 半径: ", zone->getCurrentRadius(),
", 中心点: (", zone->getCenter().x, ",", zone->getCenter().y, ")");
}
@ -818,7 +818,7 @@ void System::checkUnmannedVehicleSafetyZones(const std::vector<Vehicle>& vehicle
// 遍历所有路口安全区
for (const auto& [intersectionId, zone] : safetyZones_) {
// 如果安全区未激活，跳过
if (zone->getState() == collision::SafetyZoneState::INACTIVE) {
if (zone->getState() == SafetyZoneState::INACTIVE) {
continue;
}
@ -867,7 +867,7 @@ void System::checkUnmannedVehicleSafetyZones(const std::vector<Vehicle>& vehicle
}
bool System::handleSafetyZoneRisk(const Vehicle& vehicle,
const collision::SafetyZone* zone,
const SafetyZone* zone,
const std::vector<MovingObject*>& objects,
double distance,
const std::string& intersectionId,
@ -881,7 +881,7 @@ bool System::handleSafetyZoneRisk(const Vehicle& vehicle,
VehicleCommand cmd;
cmd.vehicleId = vehicle.id;
cmd.type = cmdType;
cmd.reason = zone->getType() == collision::SafetyZoneType::AIRCRAFT ?
cmd.reason = zone->getType() == SafetyZoneType::AIRCRAFT ?
CommandReason::AIRCRAFT_CROSSING :
CommandReason::SPECIAL_VEHICLE;
cmd.timestamp = std::chrono::system_clock::now().time_since_epoch().count();

4
src/core/System.h
View File

@ -61,7 +61,7 @@ private:
// 处理安全区风险
bool handleSafetyZoneRisk(const Vehicle& vehicle,
const collision::SafetyZone* zone,
const SafetyZone* zone,
const std::vector<MovingObject*>& objects,
double distance,
const std::string& intersectionId,
@ -88,7 +88,7 @@ private:
IntersectionConfig intersection_config_;
// 安全区管理
std::unordered_map<std::string, std::unique_ptr<collision::SafetyZone>> safetyZones_;
std::unordered_map<std::string, std::unique_ptr<SafetyZone>> safetyZones_;
static System* instance_;

55
src/detector/CollisionDetector.cpp
View File

@ -3,10 +3,9 @@
#include "utils/Logger.h"
#include "config/SystemConfig.h"
#include <cmath>
#include <format>
#include <unordered_set>
using namespace collision;
CollisionDetector::CollisionDetector(const AirportBounds& bounds, const ControllableVehicles& controllableVehicles)
: airportBounds_(bounds)
, vehicleTree_(bounds.getAirportBounds(), 8) // 使用机场总边界初始化四叉树
@ -288,7 +287,7 @@ bool CollisionDetector::checkCollisionResolved(const MovingObject& obj1,
// 统一的风险评估函数
std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
const collision::CollisionResult& collisionResult,
const CollisionResult& collisionResult,
const Vector2D& position,
MovingObjectType type1,
MovingObjectType type2) const {
@ -329,7 +328,7 @@ std::pair<RiskLevel, WarningZoneType> CollisionDetector::evaluateRisk(
}
// 统一的碰撞检测函数
collision::CollisionResult CollisionDetector::checkCollision(
CollisionResult CollisionDetector::checkCollision(
const MovingObject& obj1,
const MovingObject& obj2,
double timeWindow) const {
@ -367,7 +366,7 @@ collision::CollisionResult CollisionDetector::checkCollision(
// 如果当前距离大于警告区域，直接返回不会碰撞
if (current_distance > (warning_radius1 + warning_radius2)) {
collision::CollisionResult result;
CollisionResult result;
result.willCollide = false;
result.minDistance = current_distance;
return result;
@ -381,12 +380,12 @@ collision::CollisionResult CollisionDetector::checkCollision(
);
}
collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
CollisionResult CollisionDetector::predictCircleBasedCollision(
const Vector2D& pos1, double radius1, double speed1, double heading1,
const Vector2D& pos2, double radius2, double speed2, double heading2,
double timeWindow) const {
collision::CollisionResult result;
CollisionResult result;
// 计算安全距离（两个圆的半径之和）
double safe_distance = radius1 + radius2;
@ -422,7 +421,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
// 4. 确定碰撞类型
if (speed1 < 0.1 && speed2 < 0.1) {
result.type = collision::CollisionType::STATIC;
result.type = CollisionType::STATIC;
if (current_distance <= safe_distance) {
result.willCollide = true;
result.timeToCollision = 0.0;
@ -432,15 +431,15 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
(pos1.x + pos2.x) / 2.0,
(pos1.y + pos2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(pos1, speed1, heading1);
result.object2State = collision::CollisionObjectState(pos2, speed2, heading2);
result.object1State = CollisionObjectState(pos1, speed1, heading1);
result.object2State = CollisionObjectState(pos2, speed2, heading2);
}
return result;
}
double angle_diff = getAngleDifference(heading1, heading2);
if (angle_diff > 150.0) {
result.type = collision::CollisionType::HEAD_ON;
result.type = CollisionType::HEAD_ON;
// 对于相向运动，直接计算碰撞时间
double closing_speed = speed1 + speed2; // 相对接近速度是两个速度之和
if (closing_speed > 0.1) { // 避免除以零
@ -454,8 +453,8 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
(pos1.x + pos2.x) / 2.0,
(pos1.y + pos2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(pos1, speed1, heading1);
result.object2State = collision::CollisionObjectState(pos2, speed2, heading2);
result.object1State = CollisionObjectState(pos1, speed1, heading1);
result.object2State = CollisionObjectState(pos2, speed2, heading2);
} else {
result.timeToCollision = collision_distance / closing_speed;
result.willCollide = result.timeToCollision <= timeWindow;
@ -486,14 +485,14 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
(collision1.y + collision2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(collision1, speed1, heading1);
result.object2State = collision::CollisionObjectState(collision2, speed2, heading2);
result.object1State = CollisionObjectState(collision1, speed1, heading1);
result.object2State = CollisionObjectState(collision2, speed2, heading2);
}
}
return result;
}
} else if (angle_diff > 15.0 && angle_diff < 165.0) { // 放宽交叉路径的角度范围
result.type = collision::CollisionType::CROSSING;
result.type = CollisionType::CROSSING;
Logger::debug(
"交叉路径检测: angle_diff=", angle_diff,
@ -549,9 +548,13 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
// 如果满足所有条件，则预测为碰撞
if (valid_time) {
// 计算实际碰撞点（在交点之前）
double collision_distance = safe_distance; // 修改：使用完整的安全距离
double collision_distance = safe_distance;
double collision_time = first_arrival_time - collision_distance / first_arrival_speed;
if (collision_time < 0) {
collision_time = 0;
}
// 根据碰撞时间计算两个物体的位置
Vector2D pos1_at_collision = {
pos1.x + vx1 * collision_time,
@ -582,9 +585,9 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
result.minDistance = safe_distance;
// 更新物体状态
result.object1State = collision::CollisionObjectState(
result.object1State = CollisionObjectState(
pos1_at_collision, speed1, heading1);
result.object2State = collision::CollisionObjectState(
result.object2State = CollisionObjectState(
pos2_at_collision, speed2, heading2);
Logger::debug(
@ -598,14 +601,14 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
return result;
}
} else {
result.type = collision::CollisionType::PARALLEL;
result.type = CollisionType::PARALLEL;
// 平行运动判断：
// 1. 航向差很小（小于30度）
// 2. 横向距离大于安全距离
// 3. 相对速度很小（说明速度接近）
if (angle_diff < 30.0) {
// 计算垂直于运动方向的向距离
// 航向角转换为学坐标系中的<EFBFBD><EFBFBD><EFBFBD>转角度（逆时针为正）
// 航向角转换为学坐标系中的旋转角度（逆时针为正）
double rotation_angle = (90.0 - heading1) * M_PI / 180.0;
// 使用标准的二维坐标旋转式
@ -636,7 +639,7 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
}
}
// 5. <EFBFBD><EFBFBD><EFBFBD>时间窗口内预测碰撞
// 5. 在时间窗口内预测碰撞
const int STEPS = 120; // 增加采样点数以提高精度
double dt = timeWindow / STEPS; // 时间步长
@ -656,8 +659,8 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
(pos1.x + pos2.x) / 2.0,
(pos1.y + pos2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(pos1, speed1, heading1);
result.object2State = collision::CollisionObjectState(pos2, speed2, heading2);
result.object1State = CollisionObjectState(pos1, speed1, heading1);
result.object2State = CollisionObjectState(pos2, speed2, heading2);
return result;
}
@ -730,8 +733,8 @@ collision::CollisionResult CollisionDetector::predictCircleBasedCollision(
(interp1.x + interp2.x) / 2.0,
(interp1.y + interp2.y) / 2.0
};
result.object1State = collision::CollisionObjectState(interp1, speed1, heading1);
result.object2State = collision::CollisionObjectState(interp2, speed2, heading2);
result.object1State = CollisionObjectState(interp1, speed1, heading1);
result.object2State = CollisionObjectState(interp2, speed2, heading2);
break;
}

2
src/detector/CollisionDetector.h
View File

@ -11,8 +11,6 @@
#include <vector>
#include <unordered_map>
using namespace collision;
// 碰撞风险等级
enum class RiskLevel {
NONE = 0, // 无风险

13
src/detector/CollisionTypes.h
View File

@ -1,14 +1,9 @@
// Copyright 2024 Tianji Robotics. All rights reserved.
#ifndef AIRPORT_DETECTOR_COLLISION_TYPES_H
#define AIRPORT_DETECTOR_COLLISION_TYPES_H
#pragma once
#include "types/BasicTypes.h"
#include <limits>
#include <cmath>
namespace collision {
// 碰撞类型
enum class CollisionType {
HEAD_ON, // 相向碰撞
@ -49,8 +44,4 @@ struct CollisionResult {
distance(std::numeric_limits<double>::infinity()),
timeToMinDistance(std::numeric_limits<double>::infinity()),
type(CollisionType::STATIC) {}
};
} // namespace collision
#endif // AIRPORT_DETECTOR_COLLISION_TYPES_H
};

4
src/detector/RectangleCollision.h
View File

@ -6,8 +6,6 @@
#include <cmath>
#include <iostream>
namespace collision {
// 碰撞检测结果
struct RectangleCollisionResult {
bool willCollide = false; // 是否会发生碰撞
@ -116,6 +114,4 @@ inline bool predictRectangleBasedCollision(const Rectangle& rect1, const Rectang
return false;
}
} // namespace collision
#endif // AIRPORT_DETECTOR_RECTANGLE_COLLISION_H

6
src/detector/SafetyZone.cpp
View File

@ -3,8 +3,6 @@
#include "config/SystemConfig.h"
#include <cmath>
namespace collision {
SafetyZone::SafetyZone(const Vector2D& center, double aircraftRadius, double specialVehicleRadius)
: center_(center)
, aircraftRadius_(aircraftRadius)
@ -138,6 +136,4 @@ bool SafetyZone::isObjectInZone(const MovingObject& object) const {
// 使用 isInZone 检查是否在区内
return isInZone(object.position);
}
} // namespace collision
}

6
src/detector/SafetyZone.h
View File

@ -2,8 +2,6 @@
#include "types/BasicTypes.h"
namespace collision {
// 安全区状态
enum class SafetyZoneState {
INACTIVE, // 未激活
@ -59,6 +57,4 @@ private:
// 尝试设置安全区类型和尺寸
bool trySetType(const MovingObject& object);
};
} // namespace collision
};

20
tests/BasicCollisionTest.cpp
View File

@ -85,7 +85,7 @@ TEST_F(BasicCollisionTest, StaticCollision) {
EXPECT_TRUE(result.willCollide) << "距离小于碰撞半径的静止物体应该检测为碰撞";
EXPECT_DOUBLE_EQ(result.timeToCollision, 0.0) << "静止物体的碰撞时间应该为0";
EXPECT_EQ(result.type, collision::CollisionType::STATIC) << "应该识别为静态碰撞";
EXPECT_EQ(result.type, CollisionType::STATIC) << "应该识别为静态碰撞";
// 增加更多验证
EXPECT_DOUBLE_EQ(result.minDistance, 20.0) << "最小距离应该是当前距离20米";
@ -126,7 +126,7 @@ TEST_F(BasicCollisionTest, HeadOnCollision) {
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_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) << "碰撞点应该在同一水平线上";
@ -166,7 +166,7 @@ TEST_F(BasicCollisionTest, ParallelMotion) {
auto result = detector_->checkCollision(v1, v2, 30.0);
EXPECT_FALSE(result.willCollide) << "平行运动且距离大于碰撞半径的物体不应该检测为碰撞";
EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动";
EXPECT_EQ(result.type, CollisionType::PARALLEL) << "应该识别为平行运动";
// 增加更多验证
EXPECT_DOUBLE_EQ(result.minDistance, 50.0) << "最小距离应该是初始距离50米";
@ -195,7 +195,7 @@ TEST_F(BasicCollisionTest, CrossingPaths) {
// 添加航向角度差的调试日志
double angle_diff = std::abs(v1.heading - v2.heading);
Logger::debug("航向角度差检查:");
Logger::debug("航向<EFBFBD><EFBFBD><EFBFBD>度差检查:");
Logger::debug("v1 航向: " + std::to_string(v1.heading));
Logger::debug("v2 航向: " + std::to_string(v2.heading));
Logger::debug("航向角度差: " + std::to_string(angle_diff));
@ -203,7 +203,7 @@ TEST_F(BasicCollisionTest, CrossingPaths) {
auto result = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(result.willCollide) << "交叉路径的物体应该检测为碰撞";
EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞";
EXPECT_EQ(result.type, CollisionType::CROSSING) << "应该识别为交叉碰撞";
// 计算碰撞时间和位置：
// v1: 速度分量 (3.882, 14.489) m/s // 15 m/s * (cos(15°), sin(15°))
@ -254,7 +254,7 @@ TEST_F(BasicCollisionTest, PerpendicularCrossingPaths) {
auto result = detector_->checkCollision(v1, v2, 30.0);
EXPECT_TRUE(result.willCollide) << "垂直交叉路径的物体应该检测为碰撞";
EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞";
EXPECT_EQ(result.type, CollisionType::CROSSING) << "应该识别为交叉碰撞";
// 计算碰撞时间和位置：
// v1: 速度分量 (0.0, 10.0) m/s
@ -309,7 +309,7 @@ TEST_F(BasicCollisionTest, DivergentMotion) {
// 验证结果
EXPECT_FALSE(result.willCollide) << "同向运动且距离大于安全距离的物体不应该发生碰撞";
EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动";
EXPECT_EQ(result.type, CollisionType::PARALLEL) << "应该识别为平行运动";
// 初始距离应该是 150m
EXPECT_NEAR(result.minDistance, 150.0, 0.1);
@ -324,7 +324,7 @@ TEST_F(BasicCollisionTest, TailgatingMotion) {
Vehicle v1; // 前车
v1.vehicleNo = "V1";
v1.position = {60.0, 100.0}; // 前车在前方60米处
v1.speed = 10.0; // 前车<EFBFBD><EFBFBD><EFBFBD>度10m/s
v1.speed = 10.0; // 前车速度10m/s
v1.heading = 90.0; // 向东运动
v1.type = MovingObjectType::UNMANNED;
@ -338,7 +338,7 @@ TEST_F(BasicCollisionTest, TailgatingMotion) {
auto result = detector_->checkCollision(v1, v2, 30.0);
// 验证碰撞类型
EXPECT_EQ(result.type, collision::CollisionType::PARALLEL) << "应该识别为平行运动";
EXPECT_EQ(result.type, CollisionType::PARALLEL) << "应该识别为平行运动";
// 验证会发生碰撞
EXPECT_TRUE(result.willCollide) << "后车速度大于前车，应该预测到碰撞";
@ -397,7 +397,7 @@ TEST_F(BasicCollisionTest, QN002AircraftCrossing) {
// 验证是否检测到碰撞
EXPECT_TRUE(result.willCollide) << "QN002 与 AC001 的交叉路径应该检测为碰撞";
EXPECT_EQ(result.type, collision::CollisionType::CROSSING) << "应该识别为交叉碰撞";
EXPECT_EQ(result.type, CollisionType::CROSSING) << "应该识别为交叉碰撞";
// 记录详细的测试信息
Logger::debug("QN002-AC001 碰撞测试结果:");

2
tests/SafetyZoneTest.cpp
View File

@ -3,8 +3,6 @@
#include "utils/Logger.h"
#include "config/SystemConfig.h"
using namespace collision;
class SafetyZoneTest : public ::testing::Test {
protected:
void SetUp() override {