修复了红绿灯和车辆预警下发逻辑

This commit is contained in:
sladro 2026-02-07 12:00:00 +08:00
parent 98d8da5a17
commit f406782d4c
9 changed files with 287 additions and 27 deletions

View File

@ -248,8 +248,41 @@ void System::processLoop() {
if (has_new_data) {
// 使用成员变量的引用构建 objects 向量
std::vector<MovingObject*> objects;
for (auto& ac : latest_aircraft_) {
objects.push_back(&ac);
// 仅计算前端选择的航空器vehicleType == HANGKONG
// - 注册表里未标记任何航空器(集合为空): 不参与航空器相关碰撞检测
auto selectedAircraftIds = controllableVehicles_.getSelectedAircraftIdsSnapshot();
bool enableAircraftCollision = !selectedAircraftIds.empty();
// 兼容“前端将车辆标记为航空器(HANGKONG)”的模拟方式:
// - 若选中的 HANGKONG id 出现在车辆位置数据中,则将该车辆虚拟为航空器参与碰撞/安全区
latest_virtual_aircraft_.clear();
if (enableAircraftCollision) {
for (const auto& veh : latest_vehicles_) {
if (selectedAircraftIds.find(veh.vehicleNo) == selectedAircraftIds.end()) {
continue;
}
Aircraft ac;
ac.flightNo = veh.vehicleNo;
ac.id = veh.vehicleNo;
ac.geo = veh.geo;
ac.position = veh.position;
ac.heading = veh.heading;
ac.speed = veh.speed;
ac.timestamp = veh.timestamp;
ac.altitude = 0.0;
latest_virtual_aircraft_.push_back(ac);
}
}
if (enableAircraftCollision) {
for (auto& ac : latest_aircraft_) {
if (selectedAircraftIds.find(ac.id) != selectedAircraftIds.end()) {
objects.push_back(&ac);
}
}
for (auto& ac : latest_virtual_aircraft_) {
objects.push_back(&ac);
}
}
for (auto& veh : latest_vehicles_) {
bool controllable = controllableVehicles_.isControllable(veh.vehicleNo);
@ -280,8 +313,37 @@ void System::processLoop() {
}
// 检查和处理常规碰撞风险
collisionDetector_->updateTraffic(latest_aircraft_, latest_vehicles_);
auto collisionRisks = collisionDetector_->detectCollisions();
std::vector<Aircraft> filteredAircraft;
if (enableAircraftCollision) {
filteredAircraft.reserve(latest_aircraft_.size() + latest_virtual_aircraft_.size());
std::unordered_set<std::string> added;
for (const auto& ac : latest_aircraft_) {
if (selectedAircraftIds.find(ac.id) != selectedAircraftIds.end()) {
if (added.insert(ac.id).second) {
filteredAircraft.push_back(ac);
}
}
}
for (const auto& ac : latest_virtual_aircraft_) {
if (selectedAircraftIds.find(ac.id) != selectedAircraftIds.end()) {
if (added.insert(ac.id).second) {
filteredAircraft.push_back(ac);
}
}
}
}
std::vector<CollisionRisk> collisionRisks;
if (enableAircraftCollision && !filteredAircraft.empty()) {
collisionDetector_->updateTraffic(filteredAircraft, latest_vehicles_);
collisionRisks = collisionDetector_->detectCollisions();
} else {
if (!enableAircraftCollision) {
Logger::debug("Skip collision detection: no selected aircraft from frontend registry");
} else {
Logger::debug("Skip collision detection: selected aircraft not found in aircraft/vehicle positions");
}
}
Logger::debug("碰撞检测器检测到风险数量: ", collisionRisks.size());
for (const auto& risk : collisionRisks) {
@ -312,6 +374,9 @@ void System::processLoop() {
broadcastTrafficLightStatus(signal);
trafficLightDetector_->processSignal(signal, latest_vehicles_);
}
// 车辆更新但红绿灯未推送新信号时,使用缓存信号补发近距离红灯(新上线车辆也能获知当前红灯)
trafficLightDetector_->processCachedSignals(latest_vehicles_);
}
// 等待下一次处理

View File

@ -132,6 +132,7 @@ private:
// 存储最新数据的成员变量
std::vector<Aircraft> latest_aircraft_;
std::vector<Aircraft> latest_virtual_aircraft_; // 前端将车辆标记为 HANGKONG 时,将该车辆位置虚拟为航空器参与碰撞/安全区
std::vector<Vehicle> latest_vehicles_;
std::vector<TrafficLightSignal> latest_traffic_lights_;
};

View File

@ -4,6 +4,14 @@
#include "core/System.h"
#include <chrono>
namespace {
// 红灯近距离下发阈值(米)
// 需求:保留原有“对所有受管车辆下发信号”的逻辑;并在红灯未变化的情况下,
// 若车辆进入近距离范围,则再对该车补发一次红灯信号。
// 参考 docs/warning_strategy.md距离路口 50 米处为停车点。
constexpr double kTrafficLightRedSignalDistanceM = 50.0;
} // namespace
TrafficLightDetector::TrafficLightDetector(const IntersectionConfig& intersectionConfig,
ControllableVehicles& controllableVehicles,
System& system)
@ -11,6 +19,79 @@ TrafficLightDetector::TrafficLightDetector(const IntersectionConfig& intersectio
, controllable_vehicles_(controllableVehicles)
, system_(system) {}
void TrafficLightDetector::handleSignal(const TrafficLightSignal& signal, const Intersection* intersection,
const std::vector<Vehicle>& vehicles, bool is_signal_updated) {
if (!intersection) {
return;
}
if (is_signal_updated) {
// FIXME: 临时修改 - 仅根据南北向状态发送指令,未考虑车辆方向和东西向状态。
// 需要根据车辆行驶方向判断应该参考 ns_status 还是 ew_status。
GeoPosition intersectionGeo;
intersectionGeo.latitude = intersection->position.latitude;
intersectionGeo.longitude = intersection->position.longitude;
for (const auto& vehicle : vehicles) {
if (!controllable_vehicles_.isManagedVehicle(vehicle.vehicleNo)) {
continue;
}
switch (signal.ns_status) {
case SignalStatus::RED:
sendSignalCommand(vehicle.vehicleNo, SignalState::RED);
break;
case SignalStatus::GREEN:
sendSignalCommand(vehicle.vehicleNo, SignalState::GREEN);
break;
case SignalStatus::YELLOW:
sendSignalCommand(vehicle.vehicleNo, SignalState::YELLOW);
break;
default:
Logger::warning("未知的信号灯状态");
break;
}
// 记录“红灯且接近路口”的状态,用于后续红灯未变化时的补发判断
double dist_m = MovingObject::calculateDistance(vehicle.geo, intersectionGeo);
bool is_near = dist_m <= kTrafficLightRedSignalDistanceM;
std::string key = signal.trafficLightId + "|" + vehicle.vehicleNo;
was_near_red_by_vehicle_key_[key] = (signal.ns_status == SignalStatus::RED) && is_near;
}
return;
}
// 信号未变化:若为红灯,车辆进入近距离范围时再补发一次红灯信号
if (signal.ns_status != SignalStatus::RED) {
return;
}
GeoPosition intersectionGeo;
intersectionGeo.latitude = intersection->position.latitude;
intersectionGeo.longitude = intersection->position.longitude;
for (const auto& vehicle : vehicles) {
if (!controllable_vehicles_.isManagedVehicle(vehicle.vehicleNo)) {
continue;
}
double dist_m = MovingObject::calculateDistance(vehicle.geo, intersectionGeo);
bool is_near = dist_m <= kTrafficLightRedSignalDistanceM;
std::string key = signal.trafficLightId + "|" + vehicle.vehicleNo;
bool was_near = false;
auto it = was_near_red_by_vehicle_key_.find(key);
if (it != was_near_red_by_vehicle_key_.end()) {
was_near = it->second;
}
if (is_near && !was_near) {
sendSignalCommand(vehicle.vehicleNo, SignalState::RED);
}
was_near_red_by_vehicle_key_[key] = is_near;
}
}
void TrafficLightDetector::processSignal(const TrafficLightSignal& signal,
const std::vector<Vehicle>& vehicles) {
// 根据红绿灯ID查找对应的路口
@ -28,28 +109,42 @@ void TrafficLightDetector::processSignal(const TrafficLightSignal& signal,
// 保存当前处理的信号
current_signal_ = signal;
last_signal_by_light_id_[signal.trafficLightId] = signal;
// 检查每个车辆
for (const auto& vehicle : vehicles) {
if (controllable_vehicles_.isManagedVehicle(vehicle.vehicleNo)) {
// 根据信号灯状态发送指令
// FIXME: 临时修改 - 仅根据南北向状态发送指令,未考虑车辆方向和东西向状态。
// 需要根据车辆行驶方向判断应该参考 ns_status 还是 ew_status。
switch (signal.ns_status) {
case SignalStatus::RED:
sendSignalCommand(vehicle.vehicleNo, SignalState::RED);
break;
case SignalStatus::GREEN:
sendSignalCommand(vehicle.vehicleNo, SignalState::GREEN);
break;
case SignalStatus::YELLOW:
sendSignalCommand(vehicle.vehicleNo, SignalState::YELLOW);
break;
default:
Logger::warning("未知的信号灯状态");
break;
}
// 判断信号是否有更新:保持原有逻辑(有新信号时,对所有受管车辆下发红/黄/绿)
bool is_signal_updated = true;
{
int state = (static_cast<int>(signal.ns_status) << 8) | static_cast<int>(signal.ew_status);
auto it = last_signal_state_by_light_id_.find(signal.trafficLightId);
if (it != last_signal_state_by_light_id_.end() && it->second == state) {
is_signal_updated = false;
}
last_signal_state_by_light_id_[signal.trafficLightId] = state;
}
handleSignal(signal, intersection, vehicles, is_signal_updated);
}
void TrafficLightDetector::processCachedSignals(const std::vector<Vehicle>& vehicles) {
if (last_signal_by_light_id_.empty()) {
return;
}
// 复制一份,避免 handleSignal 内部潜在更新造成遍历失效
std::vector<TrafficLightSignal> signals;
signals.reserve(last_signal_by_light_id_.size());
for (const auto& kv : last_signal_by_light_id_) {
signals.push_back(kv.second);
}
for (const auto& signal : signals) {
const Intersection* intersection = intersection_config_.findByTrafficLightId(signal.trafficLightId);
if (!intersection) {
continue;
}
// 使用缓存信号:视为“信号未变化”,仅触发红灯近距离补发逻辑
current_signal_ = signal;
handleSignal(signal, intersection, vehicles, false);
}
}

View File

@ -3,6 +3,8 @@
#include "config/IntersectionConfig.h"
#include "vehicle/ControllableVehicles.h"
#include "types/TrafficLightTypes.h"
#include <string>
#include <unordered_map>
#include <vector>
class System;
@ -15,13 +17,27 @@ public:
void processSignal(const TrafficLightSignal& signal,
const std::vector<Vehicle>& vehicles);
// 使用缓存的最后一次红绿灯信号进行处理(用于车辆更新但红绿灯未推送新信号的场景)
void processCachedSignals(const std::vector<Vehicle>& vehicles);
private:
const IntersectionConfig& intersection_config_;
ControllableVehicles& controllable_vehicles_;
System& system_;
TrafficLightSignal current_signal_;
// 记录每个红绿灯上一次处理的信号状态(用于判断信号是否有更新)
// state = (ns_status << 8) | ew_status
std::unordered_map<std::string, int> last_signal_state_by_light_id_;
std::unordered_map<std::string, TrafficLightSignal> last_signal_by_light_id_;
// 记录“红灯且接近路口”状态,便于车辆从远到近时补发一次红灯信号
std::unordered_map<std::string, bool> was_near_red_by_vehicle_key_;
// 发送信号灯指令
void sendSignalCommand(const std::string& vehicleNo, SignalState state);
void handleSignal(const TrafficLightSignal& signal, const Intersection* intersection,
const std::vector<Vehicle>& vehicles, bool is_signal_updated);
};

View File

@ -40,12 +40,22 @@ void ControllableVehicles::updateRegistry(const std::vector<VehicleRegistryEntry
}
applied++;
}
// 航空器选择集合:基于注册表的当前状态计算(支持前端增量更新)
selected_aircraft_ids_.clear();
for (const auto& kv : vehicle_type_by_id_) {
if (kv.second == "HANGKONG") {
// 约定vehicleID 直接对应航空器的 id/flightNo或“被前端标记为航空器的车辆 id”
selected_aircraft_ids_.insert(kv.first);
}
}
}
Logger::info("Vehicle registry updated. applied=", applied,
" total=", vehicle_type_by_id_.size(),
" controllable(WUREN)=", controllable_vehicle_ids_.size(),
" managed(WUREN+TEQIN)=", managed_vehicle_ids_.size());
" managed(WUREN+TEQIN)=", managed_vehicle_ids_.size(),
" selectedAircraft(HANGKONG)=", selected_aircraft_ids_.size());
}
std::optional<std::string> ControllableVehicles::getVehicleType(const std::string& vehicleID) const {
@ -62,6 +72,11 @@ std::unordered_set<std::string> ControllableVehicles::getControllableVehicleIdsS
return controllable_vehicle_ids_;
}
std::unordered_set<std::string> ControllableVehicles::getSelectedAircraftIdsSnapshot() const {
std::shared_lock lock(mutex_);
return selected_aircraft_ids_;
}
bool ControllableVehicles::isControllable(const std::string& vehicleNo) const {
std::shared_lock lock(mutex_);
return controllable_vehicle_ids_.find(vehicleNo) != controllable_vehicle_ids_.end();
@ -72,6 +87,11 @@ bool ControllableVehicles::isManagedVehicle(const std::string& vehicleNo) const
return managed_vehicle_ids_.find(vehicleNo) != managed_vehicle_ids_.end();
}
bool ControllableVehicles::isSelectedAircraft(const std::string& aircraftId) const {
std::shared_lock lock(mutex_);
return selected_aircraft_ids_.find(aircraftId) != selected_aircraft_ids_.end();
}
bool ControllableVehicles::sendCommand(const std::string& vehicleNo, const VehicleCommand& command) {
if (!isManagedVehicle(vehicleNo)) {
Logger::debug("Skip sendCommand: vehicle not managed(WUREN/TEQIN): ", vehicleNo);

View File

@ -6,6 +6,7 @@
#include <shared_mutex>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include "types/VehicleCommand.h"
#include "network/HTTPClient.h"
@ -26,6 +27,7 @@ private:
std::unordered_map<std::string, std::string> vehicle_type_by_id_;
std::unordered_set<std::string> controllable_vehicle_ids_; // vehicleType == WUREN
std::unordered_set<std::string> managed_vehicle_ids_; // vehicleType in {WUREN, TEQIN}
std::unordered_set<std::string> selected_aircraft_ids_; // vehicleType == HANGKONG前端选择参与碰撞的航空器 flightNo/id
std::unique_ptr<HTTPClient> http_client_;
@ -38,8 +40,10 @@ public:
// 查询
std::optional<std::string> getVehicleType(const std::string& vehicleID) const;
std::unordered_set<std::string> getControllableVehicleIdsSnapshot() const;
std::unordered_set<std::string> getSelectedAircraftIdsSnapshot() const;
bool isControllable(const std::string& vehicleNo) const;
bool isManagedVehicle(const std::string& vehicleNo) const;
bool isSelectedAircraft(const std::string& aircraftId) const;
bool sendCommand(const std::string& vehicleNo, const VehicleCommand& command);
};

Binary file not shown.

View File

@ -23,6 +23,65 @@ ldd /opt/collision_avoidance/bin/collision_avoidancenew | grep "not found" || tr
docker run --rm -it --network host -v D:/App/C++/CollisionAvoidance:/src -w /src centos:7 bash
docker run -it --network host -v D:/App/C++/CollisionAvoidance:/src -w /src centos:7 bash
docker commit --pause=false 5f4a906adb1b qdairporttestbackend:20260128
docker commit --pause=false 5f4a906adb1b qdairporttestbackend:20260128
journalctl -u collision-avoidance.service --since "2026-02-05 10:50:00" --until "2026-02-07 11:10:00" -o cat --no-pager \
| grep -nE "碰撞检测器检测到风险数量|开始处理碰撞风险,风险数量"
journalctl -u collision-avoidance.service --since "2026-02-05 10:50:00" --until "2026-02-07 11:10:00" -o cat --no-pager \
| grep -nE "准备发送指令|Skip sendCommand"
journalctl -u collision-avoidance.service --since "2026-02-07 10:50:00" --until "2026-02-05 11:10:00" -o cat --no-pager \
| grep -nE "HTTPClient sendCommand request|HTTPClient sendCommand response|Successfully sent command to vehicle|Failed to send command to vehicle"
journalctl -u collision-avoidance.service --since "2026-02-05 10:50:00" --until "2026-02-07 11:10:00" -o cat --no-pager \
| grep -nE "Vehicle registry updated|selectedAircraft\\(HANGKONG\\)="
```
#部署流程
[root@5g ~]# cd /opt/collision_avoidance/bin
[root@5g bin]# ls
collision_avoidance collision_avoidance.20250308 collision_avoidance.20260128 collision_avoidance_20260130 collision_avoidance.20260204 version.txt
collision_avoidance.20241231 collision_avoidance.20250509 collision_avoidance.20260129 collision_avoidance_20260130_1 collision_avoidance.20260204_1
collision_avoidance.20250110 collision_avoidance.20260127 collision_avoidance.20260129_1 collision_avoidance20260202 collision_avoidance.sha256
[root@5g bin]# sha256sum collision_avoidance
e95d157fa487a131cc072747aac0b65bdda9700b917b7e2d2cddd78cffc74967 collision_avoidance
[root@5g bin]# mv collision_avoidance collision_avoidance.20260207
[root@5g bin]# ls
collision_avoidance collision_avoidance.20250308 collision_avoidance.20260128 collision_avoidance_20260130 collision_avoidance.20260204 collision_avoidance.sha256
collision_avoidance.20241231 collision_avoidance.20250509 collision_avoidance.20260129 collision_avoidance_20260130_1 collision_avoidance.20260204_1 version.txt
collision_avoidance.20250110 collision_avoidance.20260127 collision_avoidance.20260129_1 collision_avoidance20260202 collision_avoidance.20260207
[root@5g bin]# sha256sum collision_avoidance
5e2ff929ec39530aa70e4ee205775aa91525ec53f7f2be8e9db6a31c4ec31e56 collision_avoidance
[root@5g bin]# ls
collision_avoidance collision_avoidance.20250308 collision_avoidance.20260128 collision_avoidance_20260130 collision_avoidance.20260204 collision_avoidance.sha256
collision_avoidance.20241231 collision_avoidance.20250509 collision_avoidance.20260129 collision_avoidance_20260130_1 collision_avoidance.20260204_1 version.txt
collision_avoidance.20250110 collision_avoidance.20260127 collision_avoidance.20260129_1 collision_avoidance20260202 collision_avoidance.20260207
[root@5g bin]# chmod +x collision_avoidance
[root@5g bin]# systemctl restart collision-avoidance.service
[root@5g bin]# systemctl status collision-avoidance.service
● collision-avoidance.service - Collision Avoidance Service
Loaded: loaded (/etc/systemd/system/collision-avoidance.service; enabled; vendor preset: disabled)
Active: active (running) since Sat 2026-02-07 11:33:14 CST; 10s ago
Main PID: 248907 (collision_avoid)
Tasks: 7
CGroup: /system.slice/collision-avoidance.service
└─248907 /opt/collision_avoidance/bin/collision_avoidance
Feb 07 11:33:24 5g.novalocal collision_avoidance[248907]: 2026-02-07 11:33:24.809 [DEBUG] 重置安全区类型为 NONE
Feb 07 11:33:24 5g.novalocal collision_avoidance[248907]: 2026-02-07 11:33:24.809 [DEBUG] 重置安全区类型为 NONE
Feb 07 11:33:24 5g.novalocal collision_avoidance[248907]: 2026-02-07 11:33:24.809 [DEBUG] 开始检查安全区冲突...
Feb 07 11:33:24 5g.novalocal collision_avoidance[248907]: 2026-02-07 11:33:24.809 [DEBUG] 安全区检测新增风险数量: 0
Feb 07 11:33:24 5g.novalocal collision_avoidance[248907]: 2026-02-07 11:33:24.809 [DEBUG] Skip collision detection: no selected aircraft from frontend registry
Feb 07 11:33:24 5g.novalocal collision_avoidance[248907]: 2026-02-07 11:33:24.809 [DEBUG] 碰撞检测器检测到风险数量: 0
Feb 07 11:33:24 5g.novalocal collision_avoidance[248907]: 2026-02-07 11:33:24.809 [DEBUG] 合并后新增风险数量: 0
Feb 07 11:33:24 5g.novalocal collision_avoidance[248907]: 2026-02-07 11:33:24.809 [DEBUG] 开始处理碰撞风险,风险数量: 0
Feb 07 11:33:24 5g.novalocal collision_avoidance[248907]: 2026-02-07 11:33:24.811 [INFO] 广播红绿灯状态: id=DI_PUSHED_T2路口, NS_Status=UNKNOWN, EW_Status=RED, intersection=T2路口, timestamp=1770435201052
Feb 07 11:33:24 5g.novalocal collision_avoidance[248907]: 2026-02-07 11:33:24.811 [INFO] 收到红绿灯信号: trafficLightId=DI_PUSHED_T2路口, intersection=T2路口, ns_status=3, ew_status=0, timestamp=1770435201052
[root@5g bin]#
```

Binary file not shown.