#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cyber/common/log.h>
#include <localmap_construction/traffic_light_map_topo.h>
#include <message/internal_message.h>
#include <message/sensor/vision/tsrmobject.h>
#include <perception_and_ld_map_fusion/data_fusion/geometry_match_info.h>
#include "modules/common/math/vec2d.h"
#include "modules/perception/env/src/lib/base/params_manager/params_manager.h"
namespace cem {
namespace fusion {
return {seg1.start() + translation, seg1.end() + translation};
}
void TrafficLightMapping::StayPrevTrafficLightSet() {
auto SaveObj = [&](const TrafficLight &light) {
if (IsValid(light)) {
}
void TrafficLightMapping::StayPreviousState(const TrafficLight &previous_light, TrafficLight &traffic_light) {
if (previous_light.is_valid && !traffic_light.is_valid && previous_light.stay_prev_counter < max_stay_frames_no_info) {
traffic_light = previous_light;
traffic_light.stay_prev_counter++;
traffic_light.traffic_reason = byd::msg::orin::routing_map::LaneInfo::STAY_PREV;
}
void TrafficLightMapping::Process(const RoutingMapPtr &routing_map_input, const RoutingMapPtr &routing_map_output,
const BevMapInfoPtr &bev_map, const std::shared_ptr<navigation::SdNavigationCity> &sd_navigation_city,
BevMapInfoConstPtr &bev_raw) {
// TRAFFIC_LOG << "----------------------------\n\n";
TRAFFIC_REC_LOG << "--------------------------------\n\n";
double timestamp = GetMwTimeNowSec();
info_traffic_lights_ = "";
traffic_light_e2e_ = std::make_shared<cem::message::env_model::TrafficLightsE2EInfo>();
{
Reset();
sd_navigation_city_ = sd_navigation_city;
routing_map_ptr_ = routing_map_input;
FillPerceptionTrafficLights();
/*
SetTrafficLightColor();
bool valid_transform = TransformPostionOfObjs(timestamp);
FilterLightsByPosition();
StayPreviousState(traffic_lights_previous_.right, traffic_lights_.right);
StayPrevTrafficLightSet();
}
*/
traffic_light_descern_.Proc(routing_map_input, bev_raw);
traffic_lights_ = traffic_light_descern_.GetTrafficLights();
deal_per_traffic_light_objects_ = traffic_light_descern_.GetDealObjMap();
IsRightExit();
if ((traffic_lights_previous_.straight.is_valid && !traffic_lights_.straight.is_valid) ||
(traffic_lights_previous_.left.is_valid && !traffic_lights_.left.is_valid) ||
(traffic_lights_previous_.u_turn.is_valid && !traffic_lights_.u_turn.is_valid) ||
(traffic_lights_previous_.right.is_valid && !traffic_lights_.right.is_valid)) {
NEW_LOG << fmt::format("stay_prev_state.");
StayPreviousState(traffic_lights_previous_.straight, traffic_lights_.straight);
StayPreviousState(traffic_lights_previous_.left, traffic_lights_.left);
StayPreviousState(traffic_lights_previous_.u_turn, traffic_lights_.u_turn);
StayPreviousState(traffic_lights_previous_.right, traffic_lights_.right);
StayPrevTrafficLightSet();
}
NEW_LOG << fmt::format("prev_dis_to_junction:{:.2f}", distance_to_junction_prev_);
IsLightBlockFailed(traffic_lights_previous_.left, traffic_lights_.left);
IsLightBlockFailed(traffic_lights_previous_.u_turn, traffic_lights_.u_turn);
IsLightBlockFailed(traffic_lights_previous_.right, traffic_lights_.right);
IsLightBlockFailed(traffic_lights_previous_.straight, traffic_lights_.straight);
traffic_lights_previous_ = traffic_lights_;
NEW_LOG << fmt::format("traffic_straight {}", traffic_lights_.straight);
NEW_LOG << fmt::format("traffic_left {}", traffic_lights_.left);
NEW_LOG << fmt::format("traffic_uturn {}", traffic_lights_.u_turn);
NEW_LOG << fmt::format("traffic_right {}", traffic_lights_.right);
NEW_LOG << "------------------------------end-----------\n\n";
}
SetTrafficLightInfo();
SetTrafficLight(routing_map_input, routing_map_output);
SetSrTrafficLight(routing_map_output);
BindingTrafficLightToBev(bev_map);
time_previous_ = timestamp;
}
bool TrafficLightMapping::IsLightBlockFailed(const TrafficLight &light_prev, TrafficLight &light) const {
light.stay_prev_counter = 0;
light.traffic_light_num = 1000;
light.traffic_obj_info.reset();
NEW_LOG << fmt::format("BLOCK_FAILED_check sucessful turn:{} oringin_counter:{} invalid_counter:{} dis:{:.2f}",
magic_enum::enum_name(light.turn_type), light_prev.invalid_counter, light.invalid_counter,
distance_to_junction_prev_);
return true;
}
NEW_LOG << fmt::format("BLOCK_FAILED_check failed counter_less_3. turn:{} oringin_counter:{} invalid_counter:{} dis:{:.2f}",
magic_enum::enum_name(light.turn_type), light_prev.invalid_counter, light.invalid_counter,
distance_to_junction_prev_);
return false;
}
NEW_LOG << fmt::format("BLOCK_FAILED_check failed not in 50m->ego_junction turn:{} dis:{:.2f} current_invalid:{:d}",
magic_enum::enum_name(light.turn_type), distance_to_junction_prev_, !is_current_invalid);
light.invalid_counter = 0;
return false;
}
void TrafficLightMapping::IsRightExit() {
if (distance_to_junction_prev_ < k_epsilon) {
is_right_exist = false;
} else if (traffic_lights_.right.is_valid && traffic_lights_.right.traffic_obj_info) {
is_right_exist = true;
}
if (!is_right_exist) {
if (!traffic_lights_.right.is_valid) {
traffic_lights_.right.is_valid = true;
traffic_lights_.right.color = message::sensor::TLC_GREEN;
traffic_lights_.right.traffic_reason = byd::msg::orin::routing_map::LaneInfo::SET_DEFAULT_OBJ;
NEW_LOG << "";
}
} else if (!traffic_lights_.right.is_valid || (traffic_lights_.right.color == message::sensor::TLC_UNKNOWN)) {
traffic_lights_.right.is_valid = false;
traffic_lights_.right.color = message::sensor::TLC_UNKNOWN;
}
NEW_LOG << fmt::format("is_right_exist:{:d}", is_right_exist);
}
void TrafficLightMapping::SetTrafficLightInfo() {
auto AppendLightInfo = [&](const TrafficLight &light, const char *prefix) {
if (light.is_valid) {
info_traffic_lights_ += fmt::format(" {}:{}-{}-{}; ", prefix, light.traffic_obj_info ? light.traffic_obj_info->id : 0, light.color,
light.traffic_reason);
}
};
info_traffic_lights_ += " TRF";
AppendLightInfo(traffic_lights_.u_turn, "U");
AppendLightInfo(traffic_lights_.left, "L");
AppendLightInfo(traffic_lights_.right, "R");
AppendLightInfo(traffic_lights_.straight, "S");
}
}
}
double TrafficLightMapping::LaneNearDisToJunction(const LaneInfo &ld_lane) {
if (ld_lane.points.size() < 2 || deal_per_traffic_light_objects_.empty()) {
return std::numeric_limits<double>::infinity();
present_ids.insert(lane.id);
}
TRAFFIC_LOG << fmt::format("raw_lanes. size:{}", lanes_traffic_all.size());
// for (const LaneInfo &lane : lanes_traffic_all) {
// TRAFFIC_LOG << fmt::format(
// "lane_id:{} prev_lanes:{} left_lanes:{} right_lanes:{} turn:{} "
// "light_status:{}",
// lane.id, lane.previous_lane_ids, lane.left_lane_id, lane.right_lane_id, lane.turn_type, lane.light_status);
// }
// remove the lanes in lanes_traffic_all whose prev-lane has been in lanes_traffic_all.
// lanes_traffic_all.erase(
if (lanes_traffic_all.empty()) {
return;
}
// for (const auto &lane : lanes_traffic_all) {
// TRAFFIC_LOG << fmt::format("lane_id:{} turn:{} light_status:{}", lane.id, magic_enum::enum_name(lane.turn_type),
// magic_enum::enum_name(lane.light_status));
// }
TrafficLightStatus traffic_light_status_t;
size_t idx = 0;
traffic_light_status_t.light_status = lanes_traffic_all[idx].light_status;
SetE2EInfo(routing_map_output, lanes_traffic_all);
}
bool TrafficLightMapping::IsValid(const TrafficLight &light) {
return light.is_valid && light.traffic_obj_info;
};
void TrafficLightMapping::ValidateComparePrevLight(const TrafficLight &light_previous,
std::vector<std::vector<TrfObjectInfo>::iterator> &valid_objs) {
if (!IsValid(light_previous)) {
TRAFFIC_REC_LOG << "light_previous_is_invalid.";
return;
return std::sqrt(delta_x * delta_x + delta_y * delta_y);
};
auto DisTooFar = [DisPoints, dis_max](const TrafficLight &light_previous, const TrfObjectInfo &light_current_obj) {
double dis_temp = DisPoints(light_current_obj.position, light_previous.traffic_obj_info->position);
TRAFFIC_REC_LOG << fmt::format("prev_obj:{} pos:{:.2f}-{:.2f} cur_obj:{} pos:{:.2f}-{:.2f} dis_temp:{:.2f} dis_max:{:.2f}",
light_previous.traffic_obj_info->id, light_previous.traffic_obj_info->position.x,
LocalizationPtr target_pose = nullptr;
SensorDataManager::Instance()->GetLatestSensorFrame(timestamp, 0.05, target_pose);
LocalizationPtr measurement_pose = nullptr;
SensorDataManager::Instance()->GetLatestSensorFrame(time_previous_, 0.05, measurement_pose);
if (!(target_pose && measurement_pose)) {
return std::nullopt;
}
return CalcRotateTranslateMatrix(measurement_pose, target_pose);
}
void TrafficLightMapping::SetSrTrafficLight(const RoutingMapPtr &routing_map_output) {
if (routing_map_output == nullptr) {
return;
}
routing_map_output->traffic_lights.clear();
for (const auto &obj : perception_traffic_lights_) {
if (obj.position.x < 10) {
continue;
}
}
}
void TrafficLightMapping::TransSrTrafficLight(const TrfObjectInfo &ori_light, cem::message::env_model::TrafficLightMap &out_light,
uint64_t id, uint32_t shape) {
out_light.id = id;
out_light.center_position = ori_light.position;
out_light.light_countdown = ori_light.attributes.traffic_light_num;
out_light.light_status = LightColorConvert(ori_light.attributes.traffic_light_color);
}
void TrafficLightMapping::SetLightsTurn(const std::vector<TrafficLightShapeType> &shapes, LightTurn turn) {
for (auto &obj : perception_traffic_lights_) {
auto it = std::find(shapes.begin(), shapes.end(), obj.attributes.traffic_light_shape);
constexpr double max_x_gap = 80.0;
double base_x_dis = perception_traffic_lights_.begin()->position.x;
perception_traffic_lights_.erase(
std::remove_if(perception_traffic_lights_.begin(), perception_traffic_lights_.end(),
[&](const TrfObjectInfo &traffic_light) { return traffic_light.position.x - base_x_dis > max_x_gap; }),
perception_traffic_lights_.end());
}
std::vector<std::vector<TrfObjectInfo>::iterator> TrafficLightMapping::SatisfyTrafficLight(const std::vector<TrafficLightShapeType> &shapes,
const std::vector<TrafficLightColorType> &colors,
LightTurn turn,
const TrafficLight &previous_light) {
std::vector<std::vector<TrfObjectInfo>::iterator> res;
for (TrafficLightShapeType type_val : shapes) {
for (TrafficLightColorType color_val : colors) {
for (auto it_light = perception_traffic_lights_.begin(); it_light != perception_traffic_lights_.end(); ++it_light) {
return res;
}
bool TrafficLightMapping::TrafficLightPositionTrack(std::vector<std::vector<TrfObjectInfo>::iterator> &objs,
const TrafficLight &previous_light) {
if (objs.empty()) {
return false;
};
}
constexpr double max_dis_between_prev = 25.0;
objs.erase(std::remove_if(objs.begin(), objs.end(),
[&](const std::vector<TrfObjectInfo>::iterator &it) {
double delta_x = it->position.x - previous_light.traffic_obj_info->position.x;
double delta_y = it->position.y - previous_light.traffic_obj_info->position.y;
double dis_prev = std::sqrt(delta_x * delta_x + delta_y * delta_y);
return true;
}
TrfObjectInfo TrafficLightMapping::FindOptObj(const TrafficLight &previous_light,
std::vector<std::vector<TrfObjectInfo>::iterator> &valid_objs) {
if (valid_objs.empty()) {
return {};
}
return;
}
auto SetLightT = [&](const TrfObjectInfo &obj, TrafficLight &traffic_light_t) {
traffic_light_t.traffic_light_flashing = obj.attributes.traffic_light_flashing;
traffic_light_t.traffic_light_num = obj.attributes.traffic_light_num;
traffic_light_t.color = obj.attributes.traffic_light_color;
traffic_light_t.traffic_obj_info = std::make_shared<TrfObjectInfo>(obj);
};
traffic_light.is_valid = true;
traffic_light.perception_seq_num = perception_traffic_light_info_->header.cycle_counter;
return light.attributes.traffic_light_shape == TrafficLightShapeType::TLS_UNKNOWN ||
light.attributes.traffic_light_shape == TrafficLightShapeType::TLS_OTHER_SHAPE;
});
bool is_exist_flash = std::find_if(perception_traffic_lights_.begin(), perception_traffic_lights_.end(), [](const TrfObjectInfo &light) {
return light.attributes.traffic_light_flashing;
}) != perception_traffic_lights_.end();
auto valid_objs = SatisfyTrafficLight(
shapes,
traffic_lights_.header.cycle_counter = perception_traffic_light_info_->header.cycle_counter;
}
bool TrafficLightMapping::IsUnreliableRoadCrossJunction() {
return first_section_over_juntion_ != nullptr && first_section_over_juntion_->road_class >= cem::message::env_model::RoadClass::TOWN_ROAD;
解读这段代码,给出流程图