72 lines
2.9 KiB
C++
72 lines
2.9 KiB
C++
#include "detector/TrafficLightDetector.h"
|
|
#include "utils/Logger.h"
|
|
#include "types/BasicTypes.h"
|
|
#include "core/System.h"
|
|
#include <chrono>
|
|
|
|
TrafficLightDetector::TrafficLightDetector(const IntersectionConfig& intersectionConfig,
|
|
ControllableVehicles& controllableVehicles,
|
|
System& system)
|
|
: intersection_config_(intersectionConfig)
|
|
, controllable_vehicles_(controllableVehicles)
|
|
, system_(system) {}
|
|
|
|
void TrafficLightDetector::processSignal(const TrafficLightSignal& signal,
|
|
const std::vector<Vehicle>& vehicles) {
|
|
// 根据红绿灯ID查找对应的路口
|
|
const Intersection* intersection = intersection_config_.findByTrafficLightId(signal.trafficLightId);
|
|
if (!intersection) {
|
|
Logger::warning("未找到红绿灯对应的路口: ", signal.trafficLightId);
|
|
return;
|
|
}
|
|
|
|
// 保存当前处理的信号
|
|
current_signal_ = signal;
|
|
|
|
// 检查每个车辆
|
|
for (const auto& vehicle : vehicles) {
|
|
if (controllable_vehicles_.isControllable(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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void TrafficLightDetector::sendSignalCommand(const std::string& vehicleNo, SignalState state) {
|
|
VehicleCommand cmd;
|
|
cmd.vehicleId = vehicleNo;
|
|
cmd.type = CommandType::SIGNAL;
|
|
cmd.reason = CommandReason::TRAFFIC_LIGHT;
|
|
cmd.signalState = state;
|
|
cmd.timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
std::chrono::system_clock::now().time_since_epoch()).count();
|
|
|
|
// 添加路口信息
|
|
const Intersection* intersection = intersection_config_.findByTrafficLightId(current_signal_.trafficLightId);
|
|
if (intersection) {
|
|
cmd.intersectionId = intersection->id;
|
|
cmd.latitude = intersection->position.latitude;
|
|
cmd.longitude = intersection->position.longitude;
|
|
}
|
|
|
|
controllable_vehicles_.sendCommand(vehicleNo, cmd);
|
|
system_.broadcastVehicleCommand(cmd);
|
|
Logger::debug("发送信号灯指令到车辆: ", vehicleNo,
|
|
" 路口: ", cmd.intersectionId,
|
|
" 状态: ", state == SignalState::RED ? "红灯" : state == SignalState::GREEN ? "绿灯" : "黄灯");
|
|
} |