#include "EnvInfoProcessor.h"
#include <log_custom.h>
#include <fmt/format.h>
#include "lib/sd_navigation/routing_map_debug.h"
namespace cem {
namespace fusion {
namespace navigation {
EnvInfoProcessor::EnvInfoProcessor()
: env_info_(std::make_unique<EnvInfo>()),
is_switched_to_LD_(false),
last_remain_dis_(-1.0),
accumulated_distance_(0.0),
is_entered_ramp_(false),
is_accumulated_valid_(false) {
// is_entered_ramp_ = CheckIfInRamp(*INTERNAL_PARAMS.sd_map_data.GetSDRouteInfoPtr());
// if (is_entered_ramp_) {
// is_switched_to_LD_ = true; // 如果初始化时已在匝道内,设置标志位为 true
// }
}
void EnvInfoProcessor::Process(const std::vector<cem::message::env_model::StopLine> &stop_line_obj) {
auto *sensor_manager = SensorDataManager::Instance();
if (!sensor_manager) {
AWARN << "SensorDataManager instance is null";
SD_ENV_INFO_LOG << fmt::format("SensorDataManager instance is null, exiting");
return;
}
MapEventPtr map_event_ptr;
sensor_manager->GetLatestSensorFrame(map_event_ptr);
if (!map_event_ptr) {
AWARN << "Failed to get latest sensor frame";
SD_ENV_INFO_LOG << fmt::format("Failed to get latest MapEvent, exiting");
return;
}
const auto &map_event = *map_event_ptr;
// 获取 SDRouteInfo
if (INTERNAL_PARAMS.sd_map_data.GetSDRouteInfoPtr() == nullptr) {
AWARN << "[EnvInfoProcessor] SDRouteInfo is null";
SD_ENV_INFO_LOG << fmt::format("SDRouteInfo is null, exiting");
return;
}
const auto &sd_route = *INTERNAL_PARAMS.sd_map_data.GetSDRouteInfoPtr();
// SD_ENV_INFO_LOG << fmt::format("{}", sd_route);
// 清空并重新初始化 env_info_
// env_info_ = std::make_unique<EnvInfo>();
env_info_->v2_road_classes.clear();
env_info_->v2_turn_info.clear();
env_info_->traffic_flows.clear();
env_info_->v2_curvatures.clear();
env_info_->v2_non_odd_info.clear();
// 从 MapEvent 提取字段
env_info_->v2_valid = (map_event.navi_stat != NaviStatus::NO_NAVAGATION);
env_info_->v2_has_navigation = (map_event.navi_stat != NaviStatus::NO_NAVAGATION);
SD_ENV_INFO_LOG << fmt::format("Set v2_valid: {}, v2_has_navigation: {}", env_info_->v2_valid, env_info_->v2_has_navigation);
// 处理 v2_dist_to_ramp
if (CheckIfInRamp(sd_route)) {
env_info_->v2_dist_to_ramp = 0.0;
SD_ENV_INFO_LOG << fmt::format("Vehicle is in ramp, set v2_dist_to_ramp: 0.0");
} else if (map_event.control_waypoint_type == ControlWayPoint::COMING_LEFT_ONTO_THE_RAMP ||
map_event.control_waypoint_type == ControlWayPoint::COMING_RIGHT_ONTO_THE_RAMP) {
env_info_->v2_dist_to_ramp = map_event.control_waypoint_dis;
SD_ENV_INFO_LOG << fmt::format("Approaching ramp, set v2_dist_to_ramp: {}", env_info_->v2_dist_to_ramp);
} else {
env_info_->v2_dist_to_ramp = DEFAULT_LARGE_DISTANCE;
SD_ENV_INFO_LOG << fmt::format("No ramp detected, set v2_dist_to_ramp: {}", DEFAULT_LARGE_DISTANCE);
}
// 处理 v2_dist_to_toll 和 v2_dist_to_tunnel
env_info_->v2_dist_to_toll = DEFAULT_LARGE_DISTANCE;
env_info_->v2_dist_to_tunnel = DEFAULT_LARGE_DISTANCE;
for (const auto &reminder_info : map_event.reminder_way_info) {
if (reminder_info.reminder_waypoint_type == ReminderWayPoint::TOLL_BOOTH) {
env_info_->v2_dist_to_toll = reminder_info.reminder_waypoint_dis;
SD_ENV_INFO_LOG << fmt::format("Toll booth detected, set v2_dist_to_toll: {}", env_info_->v2_dist_to_toll);
} else if (reminder_info.reminder_waypoint_type == ReminderWayPoint::TUNNEL) {
env_info_->v2_dist_to_tunnel = reminder_info.reminder_waypoint_dis;
SD_ENV_INFO_LOG << fmt::format("Tunnel detected, set v2_dist_to_tunnel: {}", env_info_->v2_dist_to_tunnel);
}
}
// 处理 v2_curvatures
env_info_->v2_curvatures.clear();
for (const auto &curvature_info : map_event.curvature_info) {
V2Curvature v2_curvature;
v2_curvature.distance = curvature_info.remain_dis;
v2_curvature.curvature = curvature_info.curvature;
env_info_->v2_curvatures.emplace_back(v2_curvature);
}
SD_ENV_INFO_LOG << fmt::format("Set v2_curvatures with size: {}", env_info_->v2_curvatures.size());
// 处理 traffic_flows
env_info_->traffic_flows.clear();
V2TrafficFlow traffic_flow;
traffic_flow.start_s = map_event.traffic_info.dist_to_start_traffic_jam;
traffic_flow.end_s = traffic_flow.start_s + map_event.traffic_info.traffic_jam_dist;
switch (map_event.traffic_info.traffic_jam_status) {
case 1:
traffic_flow.type = V2TrafficFlow::V2TrafficFlowType::SMOOTH_FLOW;
break;
case 2:
traffic_flow.type = V2TrafficFlow::V2TrafficFlowType::SLOW_FLOW;
break;
case 3:
traffic_flow.type = V2TrafficFlow::V2TrafficFlowType::JAMMED_FLOW;
break;
case 4:
traffic_flow.type = V2TrafficFlow::V2TrafficFlowType::SEVERE_JAMMED_FLOW;
break;
default:
traffic_flow.type = V2TrafficFlow::V2TrafficFlowType::UNKNOWN_FLOW;
break;
}
env_info_->traffic_flows.emplace_back(traffic_flow);
SD_ENV_INFO_LOG << fmt::format("Set traffic_flows with size: {}", env_info_->traffic_flows.size());
// 从 sd_route 计算 v2_dist_to_subpath 和 v2_dist_to_split_routelanenum_dec
env_info_->v2_dist_to_subpath = CalculateDistanceToSubpath();
env_info_->v2_dist_to_split_routelanenum_dec = CalculateDistanceToSplitRouteLaneNumDec();
SD_ENV_INFO_LOG << fmt::format("Set v2_dist_to_subpath: {}, v2_dist_to_split_routelanenum_dec: {}", env_info_->v2_dist_to_subpath,
env_info_->v2_dist_to_split_routelanenum_dec);
env_info_->v2_road_classes = CalculateV2RoadClasses();
env_info_->v2_turn_info = CalculateV2TurnInfo(stop_line_obj);
env_info_->is_switched_to_LD_ = UpdateIsSwitchedToLD(map_event, sd_route);
// SD_ENV_INFO_LOG << fmt::format(
// "Exiting ProcessEnvInfo with env_info: v2_valid={}, v2_has_navigation={}, v2_dist_to_ramp={}, v2_dist_to_toll={}, "
// "v2_dist_to_tunnel={}, v2_dist_to_subpath={}, v2_dist_to_split_routelanenum_dec={}, v2_curvatures_size={}, "
// "traffic_flows_size={}, is_switched_to_LD_={}",
// env_info_->v2_valid, env_info_->v2_has_navigation, env_info_->v2_dist_to_ramp, env_info_->v2_dist_to_toll,
// env_info_->v2_dist_to_tunnel, env_info_->v2_dist_to_subpath, env_info_->v2_dist_to_split_routelanenum_dec,
// env_info_->v2_curvatures.size(), env_info_->traffic_flows.size(), env_info_->is_switched_to_LD_);
PrintEnvInfo(*env_info_);
}
bool EnvInfoProcessor::UpdateIsSwitchedToLD(const MapEvent &map_event, const SDRouteInfo &sd_route) {
//城区不更新is_switched_to_LD_高速切图标志位
const bool on_highway = (env_info_ && env_info_->is_on_highway_);
if (!on_highway) {
is_switched_to_LD_ = false;
accumulated_distance_ = 0.0;
}
ControlWayPoint control_type = map_event.control_waypoint_type;
double control_dis = map_event.control_waypoint_dis;
double remain_dis = -1.0;
for (const auto &remain_info : map_event.remain_dis_info) {
if (remain_info.remain_dis_type == RemainType::NAVI_MAP_MATCHED_END) {
remain_dis = remain_info.remain_dis;
break;
}
}
static double s_last_ego_s = std::numeric_limits<double>::quiet_NaN();
const double ego_s = GetEgoGlobalS(sd_route);
double ds = 0.0;
if (std::isfinite(s_last_ego_s)) {
ds = std::max(0.0, ego_s - s_last_ego_s); // 抗回退
}
s_last_ego_s = ego_s;
// 判断当前是否在闸道内
const bool in_ramp = CheckIfInRamp(sd_route);
// —— 兜底触发用:高速→非高速的下降沿(防止错过“出闸道”边沿)——
static bool s_last_on_highway = false;
const bool highway_fall_edge = (!on_highway && s_last_on_highway);
// —— 边沿:刚刚“出闸道” ——
const bool ramp_exit_edge = (!in_ramp && last_in_ramp_);
// 判断是否即将进入闸道(控制点为进入闸道类型且距离小于 800 米)
bool approaching_ramp =
(control_type == ControlWayPoint::COMING_LEFT_ONTO_THE_RAMP || control_type == ControlWayPoint::COMING_RIGHT_ONTO_THE_RAMP) &&
control_dis <= LD_MAP_ACTIVATION_DIST_THRESH;
// 状态1:在闸道内,切换到 LD 模式
if (in_ramp) {
is_switched_to_LD_ = true;
is_accumulated_valid_ = false;
accumulated_distance_ = 0.0;
last_in_ramp_ = in_ramp;
last_remain_dis_ = remain_dis;
SD_ENV_INFO_LOG << fmt::format("Vehicle is in ramp, set is_switched_to_LD_: true");
return is_switched_to_LD_;
}
// 状态2:不在闸道内,即将进入闸道(控制点距离 < 800 米)
if (approaching_ramp) {
is_switched_to_LD_ = true;
is_accumulated_valid_ = false;
accumulated_distance_ = 0.0;
last_in_ramp_ = in_ramp;
last_remain_dis_ = remain_dis;
SD_ENV_INFO_LOG << fmt::format("Approaching ramp (<= {} m), set is_switched_to_LD_: true", LD_MAP_ACTIVATION_DIST_THRESH);
return is_switched_to_LD_;
}
// 状态3:不在闸道内,控制点距离 > 800 米
// else if (control_type == ControlWayPoint::COMING_LEFT_ONTO_THE_RAMP || control_type == ControlWayPoint::COMING_RIGHT_ONTO_THE_RAMP) {
// is_switched_to_LD_ = false;
// SD_ENV_INFO_LOG << fmt::format("Control point distance > 800m, set is_switched_to_LD_: false");
// }
// 状态4 和 5:不在闸道内,根据离开闸道后的累积距离判断
if (ramp_exit_edge || (highway_fall_edge && is_switched_to_LD_ && !is_accumulated_valid_)) {
is_accumulated_valid_ = true;
accumulated_distance_ = 0.0;
SD_ENV_INFO_LOG << fmt::format("Just exited ramp, reset accumulated_distance_ (edge={}, onHW_fall={})", ramp_exit_edge,
highway_fall_edge);
}
if (is_accumulated_valid_) {
// 计算累积行驶距离,过滤异常跳变
// double delta = std::abs(last_remain_dis_ - remain_dis);
// if (delta <= MAX_REMAIN_DIS_JUMP) {
// accumulated_distance_ += delta;
// SD_ENV_INFO_LOG << fmt::format("Normal remain_dis change: {} meters, accumulated_distance_: {}", delta, accumulated_distance_);
// } else {
// SD_ENV_INFO_LOG << fmt::format("Detected abnormal remain_dis jump: {} meters, skip accumulation", delta);
// }
accumulated_distance_ += ds;
SD_ENV_INFO_LOG << fmt::format("Accumulating: +{:.2f}m → {:.2f}m", ds, accumulated_distance_);
// 根据累积距离判断 LD 模式
if (accumulated_distance_ >= LD_MAP_DEACTIVATION_DIST_THRESH) {
is_switched_to_LD_ = false; // 状态5:累积距离 >= 300 米
is_accumulated_valid_ = false;
SD_ENV_INFO_LOG << fmt::format("Accumulated distance >= 300m, exit LD mode");
} else {
is_switched_to_LD_ = true; // 状态4:累积距离 < 300 米
SD_ENV_INFO_LOG << fmt::format("Accumulated distance < 300m, maintain LD mode");
}
last_in_ramp_ = in_ramp;
last_remain_dis_ = remain_dis;
s_last_on_highway = on_highway;
return is_switched_to_LD_;
}
// 不在闸道、无冷却、也不临近闸道, 默认使用感知
is_switched_to_LD_ = false;
last_remain_dis_ = remain_dis;
s_last_on_highway = on_highway;
return is_switched_to_LD_;
}
const EnvInfo *EnvInfoProcessor::GetEnvInfo() const {
return env_info_.get();
}
bool EnvInfoProcessor::CheckIfInRamp(const SDRouteInfo &sd_route) {
// SD_ENV_INFO_LOG << fmt::format("Entering CheckIfInRamp");
uint64_t ego_section_id = sd_route.navi_start.section_id;
for (const auto &sec : sd_route.mpp_sections) {
if (sec.id == ego_section_id) {
bool is_ramp = IsRampSection(sec);
SD_ENV_INFO_LOG << fmt::format("Section {} is_ramp: {}", ego_section_id, is_ramp);
return is_ramp;
}
}
// SD_ENV_INFO_LOG << fmt::format("No matching section found, returning false");
return false;
}
bool EnvInfoProcessor::IsRampSection(const SDSectionInfo §ion_info) {
SD_ENV_INFO_LOG << fmt::format("Entering IsRampSection for section_id: {}", section_info.id);
// if ((section_info.link_type & static_cast<uint32_t>(SDLinkTypeMask::SDLT_RAMP)) == static_cast<uint32_t>(SDLinkTypeMask::SDLT_RAMP)) {
// SD_ENV_INFO_LOG << "Section is ramp due to link_type";
// return true;
// }
if (section_info.direction == SDDirectionType::BIDIRECTIONAL_PASSABLE) {
return false;
}
uint32_t section_type = section_info.link_type;
if ((section_type & (uint32_t)SDLinkTypeMask::SDLT_JCT) == (uint32_t)SDLinkTypeMask::SDLT_JCT ||
(section_type & (uint32_t)SDLinkTypeMask::SDLT_IC) == (uint32_t)SDLinkTypeMask::SDLT_IC ||
(section_type & (uint32_t)SDLinkTypeMask::SDLT_SERVICE) == (uint32_t)SDLinkTypeMask::SDLT_SERVICE ||
(section_type & (uint32_t)SDLinkTypeMask::SDLT_PARK) == (uint32_t)SDLinkTypeMask::SDLT_PARK ||
(section_type & (uint32_t)SDLinkTypeMask::SDLT_RAMP) == (uint32_t)SDLinkTypeMask::SDLT_RAMP ||
(section_info.road_class == SDRoadClass::SD_OTHER_ROAD &&
(section_type & (uint32_t)SDLinkTypeMask::SDLT_POICONNECTION) == (uint32_t)SDLinkTypeMask::SDLT_POICONNECTION)) {
return true;
} else {
return false;
}
//测试发现ld地图存在非闸道section里面存在lane是RAMP问题,先注销该逻辑
// for (const auto &lg_idx : section_info.lane_group_idx) {
// const auto *lane_group = INTERNAL_PARAMS.sd_map_data.GetSDLaneGroupInfoById(lg_idx.id);
// if (lane_group) {
// for (const auto &lane : lane_group->lane_info) {
// if (lane.type == LaneType::LANE_RAMP) {
// SD_ENV_INFO_LOG << "Section is ramp due to lane type";
// return true;
// }
// }
// }
// }
// SD_ENV_INFO_LOG << "Section is not ramp";
return false;
}
double EnvInfoProcessor::CalculateDistanceToSubpath() {
SD_ENV_INFO_LOG << fmt::format("Entering CalculateDistanceToSubpath");
if (INTERNAL_PARAMS.sd_map_data.GetSDRouteInfoPtr() == nullptr) {
AWARN << "[EnvInfoProcessor] SDRouteInfo is null in CalculateDistanceToSubpath";
SD_ENV_INFO_LOG << fmt::format("SDRouteInfo is null, returning default large distance");
return DEFAULT_LARGE_DISTANCE;
}
const auto &sd_route = *INTERNAL_PARAMS.sd_map_data.GetSDRouteInfoPtr();
double ego_global_s = GetEgoGlobalS(sd_route);
double min_dist = DEFAULT_LARGE_DISTANCE;
std::unordered_set<uint64_t> route_section_ids;
for (const auto &sec : sd_route.mpp_sections) {
route_section_ids.insert(sec.id);
}
for (const auto &subpath : sd_route.subpaths) {
if (subpath.path_type == SDPathType::MERGE || subpath.path_type == SDPathType::SPLIT) {
uint64_t enter_section_id = subpath.enter_section_id;
if (route_section_ids.find(enter_section_id) == route_section_ids.end()) {
SD_ENV_INFO_LOG << fmt::format("Subpath section_id {} not in route, skipping", enter_section_id);
continue;
}
auto enter_section = INTERNAL_PARAMS.sd_map_data.GetSDSectionInfoById(enter_section_id);
if (!enter_section) {
SD_ENV_INFO_LOG << fmt::format("Enter section {} not found, skipping", enter_section_id);
continue;
}
double section_start_s = GetSectionStartS(*enter_section, sd_route);
double section_end_s = section_start_s + enter_section->length;
double target_s = (subpath.path_type == SDPathType::MERGE) ? section_start_s : section_end_s;
double dist = target_s - ego_global_s;
if (dist >= 0 && dist < min_dist) {
min_dist = dist;
// SD_ENV_INFO_LOG << "Updated min_dist to " << min_dist << " for subpath section_id: " << enter_section_id;
}
}
}
// SD_ENV_INFO_LOG << "Returning min_dist: " << min_dist;
return min_dist;
}
double EnvInfoProcessor::CalculateDistanceToSplitRouteLaneNumDec() {
// SD_ENV_INFO_LOG << fmt::format("Entering CalculateDistanceToSplitRouteLaneNumDec");
if (INTERNAL_PARAMS.sd_map_data.GetSDRouteInfoPtr() == nullptr) {
AWARN << "[EnvInfoProcessor] SDRouteInfo is null in CalculateDistanceToSplitRouteLaneNumDec";
SD_ENV_INFO_LOG << fmt::format("SDRouteInfo is null, returning default large distance");
return DEFAULT_LARGE_DISTANCE;
}
const auto &sd_route = *INTERNAL_PARAMS.sd_map_data.GetSDRouteInfoPtr();
double ego_global_s = GetEgoGlobalS(sd_route);
double min_dist = DEFAULT_LARGE_DISTANCE;
for (const auto &subpath : sd_route.subpaths) {
if (subpath.path_type != SDPathType::SPLIT) {
// SD_ENV_INFO_LOG << fmt::format("Subpath is not SPLIT, skipping");
continue;
}
uint64_t enter_section_id = subpath.enter_section_id;
auto enter_section = INTERNAL_PARAMS.sd_map_data.GetSDSectionInfoById(enter_section_id);
if (!enter_section) {
SD_ENV_INFO_LOG << fmt::format("Enter section {} not found, skipping", enter_section_id);
continue;
}
// 获取 enter_section 的最后一个 lanegroup
if (enter_section->lane_group_idx.empty()) {
// SD_ENV_INFO_LOG << fmt::format("No lane groups in section {}, skipping", enter_section_id);
continue;
}
// 获取 enter_section 的车道数
int enter_lane_num = -1;
uint64_t last_lg_id = enter_section->lane_group_idx.back().id;
const auto *last_lg = INTERNAL_PARAMS.sd_map_data.GetSDLaneGroupInfoById(last_lg_id);
if (!last_lg) {
enter_lane_num = GetLaneNumWithoutEmergency(last_lg);
} else {
// SD_ENV_INFO_LOG << fmt::format("Last lane group {} not found, skipping", last_lg_id);
enter_lane_num = static_cast<int>(enter_section->lane_num);
}
// 遍历后继 section
for (uint64_t successor_id : enter_section->successor_section_id_list) {
auto successor_section = INTERNAL_PARAMS.sd_map_data.GetSDSectionInfoById(successor_id);
if (!successor_section || !successor_section->is_mpp_section) {
// SD_ENV_INFO_LOG << fmt::format("Successor section {} not found or not MPP, skipping", successor_id);
continue;
}
// 获取 successor_section 的第一个 lanegroup
if (successor_section->lane_group_idx.empty()) {
// SD_ENV_INFO_LOG << fmt::format("No lane groups in successor section {}, skipping", successor_id);
continue;
}
uint64_t first_successor_lg_id = successor_section->lane_group_idx.front().id;
const auto *first_successor_lg = INTERNAL_PARAMS.sd_map_data.GetSDLaneGroupInfoById(first_successor_lg_id);
if (!first_successor_lg) {
SD_ENV_INFO_LOG << fmt::format("First successor lane group {} not found, skipping", first_successor_lg_id);
continue;
}
// 检查车道后继
bool all_successors_in_first_lg = true;
for (const auto &lane : last_lg->lane_info) {
for (uint64_t successor_lane_id : lane.next_lane_ids) {
bool found = false;
for (const auto &successor_lane : first_successor_lg->lane_info) {
if (successor_lane.id == successor_lane_id) {
found = true;
break;
}
}
if (!found) {
all_successors_in_first_lg = false;
break;
}
}
if (!all_successors_in_first_lg) {
break;
}
}
// 获取 successor_section 的车道数
int next_lane_num = GetSDSectionMinLaneNumNOTEmergency(*successor_section);
// 判断条件:车道数减少 或 后继车道不全部在下一个 lanegroup 中
if (enter_lane_num > next_lane_num || !all_successors_in_first_lg) {
double split_s = GetSectionStartS(*enter_section, sd_route) + enter_section->length;
double dist = split_s - ego_global_s;
if (dist >= 0 && dist < min_dist) {
min_dist = dist;
// SD_ENV_INFO_LOG << fmt::format("Condition met at section {} (lane decrease: {}, successor check: {}), updated min_dist: {}",
// enter_section_id, (enter_lane_num > next_lane_num), !all_successors_in_first_lg, min_dist);
}
}
}
}
// SD_ENV_INFO_LOG << fmt::format("Returning min_dist: {}", min_dist);
return min_dist;
}
double EnvInfoProcessor::GetEgoGlobalS(const SDRouteInfo &sd_route) const {
// SD_ENV_INFO_LOG << fmt::format("Entering GetEgoGlobalS");
double ego_global_s = 0.0;
uint64_t ego_section_id = sd_route.navi_start.section_id;
double ego_s_offset = sd_route.navi_start.s_offset;
for (const auto &sec : sd_route.mpp_sections) {
if (sec.id == ego_section_id) {
ego_global_s += ego_s_offset;
break;
}
ego_global_s += sec.length;
}
// SD_ENV_INFO_LOG << fmt::format("Returning ego_global_s: {}", ego_global_s);
return ego_global_s;
}
double EnvInfoProcessor::GetSectionStartS(const SDSectionInfo §ion, const SDRouteInfo &sd_route) const {
// SD_ENV_INFO_LOG << "Entering GetSectionStartS for section_id: " << section.id;
double current_s = 0.0;
for (const auto &sec : sd_route.mpp_sections) {
if (sec.id == section.id) {
// SD_ENV_INFO_LOG << fmt::format("Returning current_s: {}", current_s);
return current_s;
}
current_s += sec.length;
}
// SD_ENV_INFO_LOG << fmt::format("Section not found, returning current_s: {}", current_s);
return current_s;
}
int EnvInfoProcessor::GetLaneNumWithoutEmergency(const SDLaneGroupInfo *lanegroup) {
SD_ENV_INFO_LOG << fmt::format("Entering GetLaneNumWithoutEmergency");
if (!lanegroup) {
SD_ENV_INFO_LOG << fmt::format("Lane group is null, returning 0");
return 0;
}
int count = 0;
for (const auto &lane : lanegroup->lane_info) {
if (lane.type != LaneType::LANE_EMERGENCY) {
count++;
}
}
SD_ENV_INFO_LOG << fmt::format("Returning non-emergency lane count: {}", count);
return count;
}
int EnvInfoProcessor::GetSDSectionMinLaneNumNOTEmergency(const SDSectionInfo §ion_info) {
// SD_ENV_INFO_LOG << fmt::format("Entering GetSDSectionMinLaneNumNOTEmergency for section_id: {}", section_info.id);
if (section_info.lane_group_idx.empty()) {
SD_ENV_INFO_LOG << fmt::format("No lane groups, returning section lane_num: {}", section_info.lane_num);
return static_cast<int>(section_info.lane_num);
}
int min_lane_num = std::numeric_limits<int>::max();
for (const auto &lane_group_id_tmp : section_info.lane_group_idx) {
auto lane_group = INTERNAL_PARAMS.sd_map_data.GetSDLaneGroupInfoById(lane_group_id_tmp.id);
if (lane_group) {
int emergency_lane_count = 0;
for (const auto &lane_info_tmp : lane_group->lane_info) {
if (lane_info_tmp.type == LaneType::LANE_EMERGENCY) {
emergency_lane_count++;
}
}
int current_lane_num = lane_group->lane_info.size() - emergency_lane_count;
if (current_lane_num < min_lane_num) {
min_lane_num = current_lane_num;
}
}
}
if (min_lane_num == std::numeric_limits<int>::max()) {
SD_ENV_INFO_LOG << fmt::format("No valid lane groups, returning section lane_num: {}", section_info.lane_num);
return section_info.lane_num != 0 ? static_cast<int>(section_info.lane_num) : 0;
}
int result = min_lane_num == 0 ? static_cast<int>(section_info.lane_num) : min_lane_num;
// SD_ENV_INFO_LOG << fmt::format("Returning min non-emergency lane count: {}", result);
return result;
}
V2RoadClassType EnvInfoProcessor::ConvertSDRoadClassToV2RoadClassType(SDRoadClass sd_road_class) const {
switch (sd_road_class) {
case SDRoadClass::SD_HIGHWAY:
return V2RoadClassType::HIGH_WAY_ROAD;
case SDRoadClass::SD_CITY_FAST_WAY:
return V2RoadClassType::EXPRESS_WAY_ROAD;
case SDRoadClass::SD_NATIONAL_ROAD:
return V2RoadClassType::NATIOANL_ROAD;
case SDRoadClass::SD_PROVINCIAL_ROAD:
return V2RoadClassType::PROVINCIAL_ROAD;
case SDRoadClass::SD_COUNTY_ROAD:
case SDRoadClass::SD_TOWNSHIP_ROAD:
return V2RoadClassType::MAIN_ROAD;
case SDRoadClass::SD_OTHER_ROAD:
case SDRoadClass::SD_LEVEL_9_ROAD:
case SDRoadClass::SD_FERRY:
case SDRoadClass::SD_WALY_WAY:
case SDRoadClass::SD_INVALID:
return V2RoadClassType::UNKNOWN_ROAD;
default:
return V2RoadClassType::UNKNOWN_ROAD;
}
}
std::vector<V2RoadClass> EnvInfoProcessor::CalculateV2RoadClasses() const {
if (INTERNAL_PARAMS.sd_map_data.GetSDRouteInfoPtr() == nullptr) {
AWARN << "[EnvInfoProcessor] SDRouteInfo is null";
SD_ENV_INFO_LOG << fmt::format("SDRouteInfo is null, exiting");
return {};
}
const auto &sd_route = *INTERNAL_PARAMS.sd_map_data.GetSDRouteInfoPtr();
if (sd_route.mpp_sections.empty()) {
return {};
}
double ego_global_s = GetEgoGlobalS(sd_route);
std::vector<V2RoadClass> v2_road_classes;
v2_road_classes.clear();
double first_start_s = GetSectionStartS(sd_route.mpp_sections[0], sd_route);
double first_end_s = first_start_s + sd_route.mpp_sections[0].length;
V2RoadClass current{first_start_s - ego_global_s, first_end_s - ego_global_s,
ConvertSDRoadClassToV2RoadClassType(sd_route.mpp_sections[0].road_class)};
// Process remaining sections
for (size_t i = 1; i < sd_route.mpp_sections.size(); ++i) {
const auto §ion = sd_route.mpp_sections[i];
double start_s = GetSectionStartS(section, sd_route);
double end_s = start_s + section.length;
auto type = ConvertSDRoadClassToV2RoadClassType(section.road_class);
// SD_ENV_INFO_LOG << "Section_id: " << section.id <<" start_s: " << start_s << " end_s: " << end_s << " type: " << static_cast<int>(section.road_class);
if (type == current.road_class) {
// Same road class type, extend the current segment
current.end = end_s - ego_global_s;
} else {
// Different road class type, save current and start a new segment
v2_road_classes.push_back(current);
current = {start_s - ego_global_s, // Start relative to ego
end_s - ego_global_s, // End relative to ego
type};
}
}
// Add the last segment
v2_road_classes.push_back(current);
return v2_road_classes;
}
UTurnPosition EnvInfoProcessor::FindUTurnLanePosition(const std::vector<TurnType> &lane_arrows) {
if (lane_arrows.empty())
return UTurnPosition::UNKNOWN;
// 查找最左侧和最右侧的 U-turn 车道
bool leftmost_has_uturn = false;
bool rightmost_has_uturn = false;
for (size_t i = 0; i < lane_arrows.size(); ++i) {
if (lane_arrows[i] == TurnType::STRAIGHT_AND_U_TURN || lane_arrows[i] == TurnType::LEFT_TURN_AND_U_TURN ||
lane_arrows[i] == TurnType::STRAIGHT_AND_LEFT_TURN_AND_U_TURN) {
if (i == 0)
leftmost_has_uturn = true;
}
if (lane_arrows[i] == TurnType::STRAIGHT_AND_U_TURN || lane_arrows[i] == TurnType::RIGHT_TURN_AND_U_TURN ||
lane_arrows[i] == TurnType::STRAIGHT_AND_RIGHT_TURN_AND_U_TURN) {
if (i == lane_arrows.size() - 1)
rightmost_has_uturn = true;
}
}
if (leftmost_has_uturn)
return UTurnPosition::LEFTMOST;
if (rightmost_has_uturn)
return UTurnPosition::RIGHTMOST;
return UTurnPosition::UNKNOWN;
}
std::pair<V2TurnInfo::V2TurnType, V2TurnInfo::V2DetailTurnType> EnvInfoProcessor::GetTurnInfoForRoadSplit(JunctionAction action) {
static const std::map<JunctionAction, std::pair<V2TurnInfo::V2TurnType, V2TurnInfo::V2DetailTurnType>> turn_map = {
{JunctionAction::TurnLeft, {V2TurnInfo::V2TurnType::LEFT, V2TurnInfo::V2DetailTurnType::SLIGHT_LEFT}},
{JunctionAction::TurnRight, {V2TurnInfo::V2TurnType::RIGHT, V2TurnInfo::V2DetailTurnType::SLIGHT_RIGHT}},
{JunctionAction::GoStraight, {V2TurnInfo::V2TurnType::STRAIGHT, V2TurnInfo::V2DetailTurnType::CONTINUE}}};
auto it = turn_map.find(action);
if (it != turn_map.end()) {
return it->second;
}
return {V2TurnInfo::V2TurnType::UNKNOWN, V2TurnInfo::V2DetailTurnType::NONE};
}
std::vector<V2TurnInfo> EnvInfoProcessor::CalculateV2TurnInfo(const std::vector<cem::message::env_model::StopLine> &stop_line_obj) {
if (!env_info_) {
SD_ENV_INFO_LOG << fmt::format("EnvInfo is null, returning empty turn info");
return {};
}
// 根据Master层计算的is_on_highway_ 区分处理高速还是城区逻辑
if (env_info_->is_on_highway_) {
SD_ENV_INFO_LOG << fmt::format("is_on_highway_ is: {}, Processing highway logic.", env_info_->is_on_highway_);
auto highway_junctions_ptr = INTERNAL_PARAMS.navigation_info_data.GetJunctionInfoPtr();
if (highway_junctions_ptr) {
return ProcessHighwayJunctions(stop_line_obj, *highway_junctions_ptr);
} else {
SD_ENV_INFO_LOG << fmt::format("Highway junctions pointer is null, returning empty turn info");
return {};
}
} else {
SD_ENV_INFO_LOG << fmt::format("is_on_highway_ is: {}, Processing city logic.", env_info_->is_on_highway_);
auto city_junctions_ptr = INTERNAL_PARAMS.navigation_info_data.GetJunctionInfoCityPtr();
if (city_junctions_ptr) {
return ProcessCityJunctions(stop_line_obj, *city_junctions_ptr);
} else {
SD_ENV_INFO_LOG << fmt::format("City junctions pointer is null, returning empty turn info");
return {};
}
}
}
std::vector<V2TurnInfo> EnvInfoProcessor::ProcessCityJunctions(const std::vector<cem::message::env_model::StopLine> &stop_line_obj,
const std::vector<navigation::JunctionInfoCity> &city_junctions) {
if (city_junctions.empty()) {
return {};
}
std::vector<V2TurnInfo> turn_infos;
turn_infos.clear();
std::vector<uint64_t> v2_junction_ids = INTERNAL_PARAMS.navigation_info_data.get_v2_junction_ids();
for (const auto &junction : city_junctions) {
auto find_v2 = std::find(v2_junction_ids.begin(), v2_junction_ids.end(), junction.junction_id);
if (find_v2 == v2_junction_ids.end()) {
continue;
}
SD_ENV_INFO_LOG << fmt::format("{}", junction);
V2TurnInfo turn_info;
turn_info.id = junction.junction_id;
turn_info.is_valid = true;
// 处理 RoadSplit 路口的特殊逻辑
if (junction.junction_type_city == JunctionTypeCity::RoadSplit) {
// 检查是否为右转专用道
if (junction.is_dedicated_right_turn_lane && junction.split_merge_direction == DirectionSplitMerge::Right) {
turn_info.turn_type = V2TurnInfo::V2TurnType::RIGHT;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::TURN_RIGHT_ONLY;
} else {
// 非右转专用道的RoadSplit路口,使用原默认逻辑
auto [turn_type, detail_turn_type] = GetTurnInfoForRoadSplit(junction.junction_action);
turn_info.turn_type = turn_type;
turn_info.detail_turn_type = detail_turn_type;
}
} else {
// 其他路口类型的原有逻辑
switch (junction.junction_action) {
case JunctionAction::TurnLeft:
turn_info.turn_type = V2TurnInfo::V2TurnType::LEFT;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::TURN_LEFT;
break;
case JunctionAction::TurnRight:
turn_info.turn_type = V2TurnInfo::V2TurnType::RIGHT;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::TURN_RIGHT;
break;
case JunctionAction::GoStraight:
turn_info.turn_type = V2TurnInfo::V2TurnType::STRAIGHT;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::CONTINUE;
break;
case JunctionAction::UTurn: {
UTurnPosition uturn_pos = FindUTurnLanePosition(junction.map_lane_arrows_plan);
if (uturn_pos == UTurnPosition::LEFTMOST) {
turn_info.turn_type = V2TurnInfo::V2TurnType::U_TURN_LEFT;
} else if (uturn_pos == UTurnPosition::RIGHTMOST) {
turn_info.turn_type = V2TurnInfo::V2TurnType::U_TURN_RIGHT;
}
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::UTURN;
break;
}
default:
turn_info.turn_type = V2TurnInfo::V2TurnType::UNKNOWN;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::NONE;
break;
}
}
turn_info.v2_dist = junction.offset;
if (!stop_line_obj.empty() && (std::fabs(stop_line_obj.at(0).distance_to_stopline - turn_info.v2_dist) < 30.0)) {
SD_ENV_INFO_LOG << fmt::format("Using stopline dist to calib dist_v2");
turn_info.dist = stop_line_obj.at(0).distance_to_stopline;
} else {
turn_info.dist = turn_info.v2_dist;
}
turn_info.before_turn.lane_num = junction.main_road_lane_nums;
turn_info.after_turn.lane_num = junction.target_road_lane_nums;
if (!junction.junction_ids.empty()) {
uint64_t before_section_id = junction.junction_ids.front();
auto before_section = INTERNAL_PARAMS.sd_map_data.GetSDSectionInfoById(before_section_id);
if (before_section) {
turn_info.before_turn.road_class = ConvertSDRoadClassToV2RoadClassType(before_section->road_class);
} else {
turn_info.before_turn.road_class = V2RoadClassType::UNKNOWN_ROAD;
SD_ENV_INFO_LOG << fmt::format("Before section {} not found for junction {}", before_section_id, junction.junction_id);
}
uint64_t after_section_id = junction.junction_ids.back();
auto after_section = INTERNAL_PARAMS.sd_map_data.GetSDSectionInfoById(after_section_id);
if (after_section) {
turn_info.after_turn.road_class = ConvertSDRoadClassToV2RoadClassType(after_section->road_class);
} else {
turn_info.after_turn.road_class = V2RoadClassType::UNKNOWN_ROAD;
SD_ENV_INFO_LOG << fmt::format("After section {} not found for junction {}", after_section_id, junction.junction_id);
}
} else {
turn_info.before_turn.road_class = V2RoadClassType::UNKNOWN_ROAD;
turn_info.after_turn.road_class = V2RoadClassType::UNKNOWN_ROAD;
SD_ENV_INFO_LOG << fmt::format("Junction {} has empty junction_ids", junction.junction_id);
}
turn_infos.push_back(turn_info);
}
return turn_infos;
}
std::vector<V2TurnInfo> EnvInfoProcessor::ProcessHighwayJunctions(const std::vector<cem::message::env_model::StopLine> &stop_line_obj,
const std::vector<navigation::JunctionInfo> &highway_junctions) {
std::vector<V2TurnInfo> turn_infos;
std::vector<uint64_t> v2_junction_ids = INTERNAL_PARAMS.navigation_info_data.get_v2_junction_ids();
// 定义闸道里分叉路口类型集合
std::unordered_set<JunctionType> split_types = {JunctionType::RampSplitLeft, JunctionType::RampSplitRight, JunctionType::RampSplitMiddle};
for (const auto &junction : highway_junctions) {
auto find_v2 = std::find(v2_junction_ids.begin(), v2_junction_ids.end(), junction.junction_id);
if (find_v2 == v2_junction_ids.end()) {
continue;
}
SD_ENV_INFO_LOG << fmt::format("Evaluating highway junction ID: {}, type: {}, SplitDirection: {}, offset: {}", junction.junction_id,
static_cast<int>(junction.junction_type), static_cast<int>(junction.split_direction), junction.offset);
V2TurnInfo turn_info;
turn_info.id = junction.junction_id;
turn_info.is_valid = true;
// 检查是否为闸道里分叉路口类型
if (split_types.count(junction.junction_type)) {
// 对于分叉路口,使用 split_direction 设置转向信息
switch (junction.split_direction) {
case SplitDirection::Straight:
turn_info.turn_type = V2TurnInfo::V2TurnType::STRAIGHT;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::CONTINUE;
break;
case SplitDirection::SplitLeft:
turn_info.turn_type = V2TurnInfo::V2TurnType::LEFT;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::SLIGHT_LEFT;
break;
case SplitDirection::SplitRight:
turn_info.turn_type = V2TurnInfo::V2TurnType::RIGHT;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::SLIGHT_RIGHT;
break;
default:
turn_info.turn_type = V2TurnInfo::V2TurnType::UNKNOWN;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::NONE;
break;
}
} else {
// 对于非分叉路口,保留原有 junction_type 逻辑
switch (junction.junction_type) {
case JunctionType::RampInto:
turn_info.turn_type = V2TurnInfo::V2TurnType::RIGHT;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::SLIGHT_RIGHT;
break;
case JunctionType::ApproachRampInto:
turn_info.turn_type = V2TurnInfo::V2TurnType::STRAIGHT;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::CONTINUE;
break;
default:
turn_info.turn_type = V2TurnInfo::V2TurnType::UNKNOWN;
turn_info.detail_turn_type = V2TurnInfo::V2DetailTurnType::NONE;
break;
}
}
turn_info.v2_dist = junction.offset;
turn_info.dist = turn_info.v2_dist;
turn_info.before_turn.lane_num = junction.main_road_lane_nums;
turn_info.after_turn.lane_num = junction.target_road_lane_nums;
if (!junction.junction_ids.empty()) {
uint64_t before_section_id = junction.junction_ids.front();
auto before_section = INTERNAL_PARAMS.sd_map_data.GetSDSectionInfoById(before_section_id);
if (before_section) {
turn_info.before_turn.road_class = ConvertSDRoadClassToV2RoadClassType(before_section->road_class);
} else {
turn_info.before_turn.road_class = V2RoadClassType::UNKNOWN_ROAD;
SD_ENV_INFO_LOG << fmt::format("Before section {} not found for junction {}", before_section_id, junction.junction_id);
}
uint64_t after_section_id = junction.junction_ids.back();
auto after_section = INTERNAL_PARAMS.sd_map_data.GetSDSectionInfoById(after_section_id);
if (after_section) {
turn_info.after_turn.road_class = ConvertSDRoadClassToV2RoadClassType(after_section->road_class);
} else {
turn_info.after_turn.road_class = V2RoadClassType::UNKNOWN_ROAD;
SD_ENV_INFO_LOG << fmt::format("After section {} not found for junction {}", after_section_id, junction.junction_id);
}
} else {
turn_info.before_turn.road_class = V2RoadClassType::UNKNOWN_ROAD;
turn_info.after_turn.road_class = V2RoadClassType::UNKNOWN_ROAD;
SD_ENV_INFO_LOG << fmt::format("Junction {} has empty junction_ids", junction.junction_id);
}
turn_infos.push_back(turn_info);
}
return turn_infos;
}
void EnvInfoProcessor::SetIsOnHighway(bool is_on_highway) {
if (env_info_) {
env_info_->is_on_highway_ = is_on_highway;
// SD_ENV_INFO_LOG << fmt::format("Set value is_on_highway:{}", env_info_->is_on_highway_);
}
}
void EnvInfoProcessor::PrintEnvInfo(const EnvInfo &env_info) const {
SD_ENV_INFO_LOG << fmt::format("{}", env_info);
}
} // namespace navigation
} // namespace fusion
} // namespace cem详细解释下代码
最新发布