#include "master
.h"
#include <log_custom
.h>
#include <cstddef>
#include <cstdint>
#include <map>
#include <string>
#include <base/params_manager/params_manager
.h>
#include <perception_and_ld_map_fusion/data_fusion/geometry_match_info
.h>
#include "fmt/core
.h"
#include "lib/sd_navigation/SDMapElementExtract
.h"
#include "message/env_model/routing_map/routing_map
.h"
#include "message/env_model/occ/occ
.h"
#include "message/sensor/camera/bev_lane/bev_lane
.h"
#include "cyber/event/trace
.h"
#include "cyber/time/time
.h"
#include "modules/perception/env/src/lib/base/localmap_conf
.h"
#include "modules/simulator/deterministic_scheduler/deterministic_scheduler
.h"
using byd::simulator::deterministic_scheduler::SchedulerUnitManager;
using byd::simulator::deterministic_scheduler::TaskType;
using namespace cem::fusion;
using namespace cem::fusion::extendLane;
namespace cem {
namespace fusion {
std::shared_ptr<cem::fusion::Master> MasterPtr = nullptr;
std::shared_ptr<MasterInterface> GetMasterInstance
() {
if
(!MasterPtr
) {
MasterPtr = std::shared_ptr<Master>
(new Master
("modules/perception/env", "local_map"
));
}
return MasterPtr;
}
Master::Master
(const std::string &confdir, const std::string &name
) {
conf_abs_path_ = confdir + std::string
("/"
);
VLOG
(1
) << "conf_abs_path_ = " << conf_abs_path_;
Init
();
}
void Master::Init
() {
LoadConfig
();
InitMember
();
}
void Master::LoadConfig
() {
ParamsManager::Instance
()->ReadJsonParams
(conf_abs_path_ + "conf/TrafficRoadFusion/E2_Conf_TrafficRoadFusion
.json",
conf_abs_path_ + "conf/TrafficRoadFusion/E2_Conf_MeasurementEvaluation
.json"
);
}
void Master::InitMember
() {
routing_map_
.reset
(new RoutingMap
());
pre_
processor_
.reset
(new
PreProcessor
());
post_
processor_
.reset
(new Post
Processor
());
messenger_
.reset
(new Messenger
());
stopline_mapping_
.reset
(new StoplineMapping
);
traffic_light_mapping_
.reset
(new TrafficLightMapping
);
bev_map_
processor_
.reset
(new BevMap
PreProcessor
());
fusion_manager_
.reset
(new FusionManager
());
fusion_manager_
->Init
();
map_fusion_
.reset
(new MapFusion
);
lane_guidance_
.reset
(new LaneGuidance
);
lane_guidance_city_
.reset
(new LaneGuidanceCity
);
env_info_
processor_
.reset
(new navigation::EnvInfo
Processor
());
traffic_flow_mapping_
.reset
(new TrafficFlowMapping
);
toggle_to_bev_info_
.push
(MapType::BEV_MAP
);
cross_road_construction_
.reset
(new CrossRoadConstruction
);
cross_road_construction_
->Init
();
extend_lane_
.reset
(new ExtendLane
);
debug_infos_
.reset
(new DebugInfos
);
sd_navigation_base_ = std::make_shared<navigation::SdNavigationBase>
();
sd_navigation_highway_ = std::make_shared<navigation::SdNavigationHighway>
();
sd_navigation_city_ = std::make_shared<navigation::SdNavigationCity>
();
if
(FLAGS_init_map_source == "ld" || FLAGS_init_map_source == "LD"
) {
toggle_to_bev_info_
.push
(MapType::HD_MAP
);
} else {
toggle_to_bev_info_
.push
(MapType::BEV_MAP
);
}
}
#define MapLessFlag
void Master::Proc
() {
env_status_ = std::make_shared<byd::msg::orin::routing_map::EnvStatus>
();
//get
previous cross_road status and cross road distance
pre_
processor_
->is_cross_road_status = cross_road_construction_
->IsCrossRoad
();
pre_
processor_
->cross_road_distance = cross_road_construction_
->CrossRoadDis
();
pre_
processor_
->
Process();
//get
previous cross_road status
pre_
processor_
->is_cross_road_status = cross_road_construction_
->IsCrossRoad
();
RoutingMapPtr routing_map_raw{nullptr};
SensorDataManager::Instance
()->GetLatestSensorFrame
(routing_map_raw
);
auto raw_bev_map_ptr = INTERNAL_PARAMS
.raw_bev_data
.GetRawBevMapPtr
();
///高速和城区的切换
bool IS_ON_HIGHWAY_FLAG = false;
{
IS_ON_HIGHWAY_FLAG = GetIsOnHighwayFlag
(routing_map_raw
);
if
(IS_ON_HIGHWAY_FLAG
) {
sd_navigation_base_ = sd_navigation_highway_;
} else {
sd_navigation_base_ = sd_navigation_city_;
}
}
env_status_
->set_is_on_freeway
(IS_ON_HIGHWAY_FLAG == true
);
DetectBevMap bev_map =
pre_
processor_
->GetGlobalBevMapPtr
();
///停止线处理
stopline_mapping_
->
Process();
const auto &bev_stopline_objs = stopline_mapping_
->GetStopLines
();
///导航推荐
std::vector<std::pair<uint64_t, std::pair<int, int>>> guide_lane_result = {};
std::vector<uint64_t> sd_guide_lane_ids_ref = {};
sd_navigation_base_
->Proc
(routing_map_raw, raw_bev_map_ptr, bev_map
.bev_map_ptr, guide_lane_result, sd_guide_lane_ids_ref
);
auto junctions_ptr = INTERNAL_PARAMS
.navigation_info_data
.GetJunctionInfoCityPtr
();
if
((!IS_ON_HIGHWAY_FLAG
) &&
(routing_map_raw != nullptr
)) {
auto trajectory = sd_navigation_city_
->GetNextRightPoints
(routing_map_raw
);
pre_
processor_
->SetRightTurnTrajectory
(routing_map_raw
->header
.timestamp, trajectory
);
}
///env_info
env_info_
processor_
->SetIsOnHighway
(IS_ON_HIGHWAY_FLAG
);
env_info_
processor_
->
Process(bev_stopline_objs
);
const EnvInfo *env_info = env_info_
processor_
->GetEnvInfo
();
if
(bev_map
.bev_map_ptr != nullptr
) {
bev_map
.bev_map_ptr
->env_info = *env_info;
}
bev_map_
processor_
->
Process(bev_map, sd_guide_lane_ids_ref
);
fusion_manager_
->
Process();
const auto routing_map =
pre_
processor_
->routing_map_
pre_
processor_
.GetRoutingMap
();
if
(routing_map != nullptr
) {
routing_map
->env_info = *env_info;
}
extend_lane_
->
Process(bev_map
);
bev_ld_map_geometry_match_info_
.CalculateMapGeometryMatchInfo
(fusion_manager_
->GetBevMapInfo
(), fusion_manager_
->GetLdMapInfo
());
// cross road part
cross_road_construction_
->SetGeometryMatchInfo
(&bev_ld_map_geometry_match_info_
);
cross_road_construction_
->
Process(bev_map
.bev_map_ptr, bev_map
.Twb, raw_bev_map_ptr, junctions_ptr
);
// Post
-processing of the bev map and intergration the guide lane, virtual
// lane, traffic light status, etc
.
lane_guidance_city_
->
Process(routing_map_raw, bev_map, bev_stopline_objs, guide_lane_result
);
traffic_light_mapping_
->
Process(routing_map_raw, routing_map, lane_guidance_city_
->GetMapInfo
(), sd_navigation_city_
);
GeometryMatchResult match_detail;
auto map_convert_type = MapToggleMode
(match_detail
);
messenger_
->PulishDiagno
();
messenger_
->PulishTrafficLightE2E
(traffic_light_mapping_
->GetTrafficLightsE2E
());
bool used_ld_map = toggle_to_bev_info_
.back
() == MapType::HD_MAP;
uint32_t map_counter = 0;
// std::string map_type_str = used_ld_map ? "ld" : "bev";
map_counter = used_ld_map ?
(routing_map ? routing_map
->header
.cycle_counter : 2
)
:
(lane_guidance_city_
->GetMapInfo
() ? lane_guidance_city_
->GetMapInfo
()->header
.cycle_counter : 1
);
const auto &map_convert_params = ParamsManager::Instance
()->GetMapConvertParams
();
if
(IS_ON_HIGHWAY_FLAG
) {
used_ld_map = false;
env_status_
->clear_hd_map_sup
pression_reason
();
env_status_
->clear_perception_map_sup
pression_reason
();
if
(env_info
->is_switched_to_LD_ && match_detail
.fail_reason != kLostHdMapLane
) {
env_status_
->set_perception_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_EGO_IN_THE_RAMP
);
used_ld_map = true;
} else {
if
(match_detail
.fail_reason == kLostHdMapLane
) {
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_EGO_OUTSIDE_MAP
);
} else {
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_EGO_NOT_IN_THE_RAMP
);
}
used_ld_map = false;
}
env_status_
->clear_distance_to_next_junction
();
env_status_
->clear_distance_to_passed_junction
();
env_status_
->clear_left_laneline_match_info
();
env_status_
->clear_right_laneline_match_info
();
// todo: add env status here
} else {
if
(map_convert_params
.map_stay_ld
) {
used_ld_map = true;
} else if
(map_convert_params
.map_stay_bev
) {
used_ld_map = false;
}
}
std::string map_type_str = used_ld_map ? "ld" : "bev";
if
(!map_convert_params
.map_stay_ld && used_ld_map
) {
const bool ld_has{false};
// const bool ld_has = bev_ld_map_geometry_match_info_
.LDHasEnoughPoints
();
// const bool bev_has = bev_ld_map_geometry_match_info_
.BEVHasEnoughPoints
();
if
(!routing_map || routing_map
->route
.navi_start
.section_id == 0U
) {
used_ld_map = false;
if
(!toggle_to_bev_info_
.empty
()) {
toggle_to_bev_info_
.pop
();
}
toggle_to_bev_info_
.emplace
(MapType::BEV_MAP
);
if
(!routing_map
) {
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_NO_HD_MAP_FRAME
);
} else {
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_NO_NAVIGATION_START_SECTION
);
}
env_status_
->clear_perception_map_sup
pression_reason
();
map_type_str = fmt::format
("bev ld_less_points
.[{:d}
-{}
-{:d}]", static_cast<bool>
(routing_map
),
routing_map ? routing_map
->route
.navi_start
.section_id : 0, ld_has
);
}
}
RoutingMapPtr routing_map_copy_ptr = nullptr;
if
(nullptr != routing_map
) {
bool use_ld_tmp = used_ld_map;
if
(true == used_ld_map
) {
routing_map
->sensor_status_info
.sensor_status = 2;
}
post_
processor_
->run
(routing_map, used_ld_map
);
if
(use_ld_tmp && !used_ld_map
) {
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_CLOSE_TO_HIGH_LEVEL_ROAD
);
env_status_
->clear_perception_map_sup
pression_reason
();
env_status_
->clear_distance_to_next_junction
();
env_status_
->clear_distance_to_passed_junction
();
env_status_
->clear_left_laneline_match_info
();
env_status_
->clear_right_laneline_match_info
();
}
map_fusion_
->SetGeometryMatchInfo
(&bev_ld_map_geometry_match_info_
);
routing_map_copy_ptr = std::make_shared<RoutingMap>
(*routing_map
);
map_fusion_
->
Process(bev_map, routing_map_copy_ptr
);
}
if
(used_ld_map
) {
env_status_
->set_map_source
(byd::msg::orin::routing_map::EnvStatus::EnvSource::EnvStatus_EnvSource_HD_MAP
);
} else {
env_status_
->set_map_source
(byd::msg::orin::routing_map::EnvStatus::EnvSource::EnvStatus_EnvSource_PERCEPTION_MAP
);
}
#ifdef MapLessFlag
used_ld_map = false;
#endif
debug_infos_
->SetDebugInfo
(used_ld_map, lane_guidance_city_
->GetMapInfo
(), lane_guidance_city_
->GetTwb
(), routing_map_copy_ptr
);
messenger_
->PulishFusionMap
(!used_ld_map, lane_guidance_city_
->GetMapInfo
(), routing_map_copy_ptr, bev_stopline_objs, IS_ON_HIGHWAY_FLAG,
sd_navigation_city_
->GetInfo
());
if
(auto map_output = messenger_
->GetFusionMapInfo
(); map_output
) {
double adc_to_junction = traffic_light_mapping_
->DistanceToJunction
();
uint64_t ld_counter = routing_map == nullptr ? 0 : routing_map
->header
.cycle_counter;
uint64_t bev_counter = lane_guidance_city_
->GetMapInfo
() == nullptr ? 0 : lane_guidance_city_
->GetMapInfo
()->header
.cycle_counter;
map_output
->mutable_map_info
()->mutable_env_status
()->CopyFrom
(*env_status_
);
map_output
->mutable_map_info
()->set_map_version
(
fmt::format
("map_type:{} ld_counter:{} bev_counter:{} vision_counter:{} "
"adc_to_junction:{:
.2f} map_change_type:{} "
"stay_ld:{:d} stay_bev:{:d} IS_ON_HIGHWAY_FLAG:{:d} "
"used_ld_map:{:d} is_switched:{:d}",
map_type_str, ld_counter, bev_counter, traffic_light_mapping_
->GetPerceptionCyberCounter
(), adc_to_junction,
to_string
(map_convert_type
), map_convert_params
.map_stay_ld, map_convert_params
.map_stay_bev, IS_ON_HIGHWAY_FLAG,
used_ld_map, env_info
->is_switched_to_LD_
) +
sd_navigation_city_
->GetInfo
());
map_output
->mutable_map_info
()->set_engine_version
(
bev_ld_map_geometry_match_info_
.GetDebugInfo
() +
fmt::format
(" is_keep_lane:{:d} is_right:{:d} ", is_lane_keeping_, sd_navigation_city_
->IsEgoInDedicatedRightLane
()) +
traffic_light_mapping_
->TrafficLightLaneDebug
());
GEOMETRY_LOG << map_output
->map_info
().map_version
();
TRAFFIC_LOG << map_output
->map_info
().engine_version
();
}
}
bool Master::GetIsOnHighwayFlag
(const RoutingMapPtr &routing_map_raw
) {
auto is_on_highway_flag = []
(const SDSectionInfo &sd_section_info
) {
auto ¤t_road_class = sd_section_info
.road_class;
uint32_t current_section_type = sd_section_info
.link_type;
uint64_t sd_section_id = sd_section_info
.id / 10;
if
((sd_section_id == 1678534592
) ||
(sd_section_id == 1699361336
) ||
(sd_section_id == 1699374097
) ||
(sd_section_id == 1691246158
) ||
(sd_section_id == 1607693456
) ||
(sd_section_id == 1607693601
) ||
(sd_section_id == 1648672436
) ||
(sd_section_id == 1648672824
) ||
(sd_section_id == 1678890528
) ||
(sd_section_id == 1705771743
) ||
(sd_section_id == 1705771555
) ||
(sd_section_id == 1699402002
) ||
(sd_section_id == 1699402004
) ||
(sd_section_id == 1613352349
) ||
(sd_section_id == 1678533134
) ||
(sd_section_id == 16993
72997
) ||
(sd_section_id == 1699374094
) ||
(sd_section_id == 1607690036
) ||
(sd_section_id == 1607690332
) ||
(sd_section_id == 1664789284
) ||
(sd_section_id == 1664789271
) ||
(sd_section_id == 1648672814
) ||
(sd_section_id == 1678890452
) ||
(sd_section_id == 1699443498
) ||
(sd_section_id == 1705771744
) ||
(sd_section_id == 1705771880
) ||
(sd_section_id == 1705771278
) ||
(sd_section_id == 1705771745
) ||
(sd_section_id == 1699402006
) ||
(sd_section_id == 1699402007
) ||
(sd_section_id == 1705771745
) ||
(sd_section_id == 1663998523
) ||
(sd_section_id == 1582251492
)) {
return false;
}
if
(current_road_class == SDRoadClass::SD_HIGHWAY || current_road_class == SDRoadClass::SD_CITY_FAST_WAY ||
(current_section_type &
(uint32_t
)SDLinkTypeMask::SDLT_IC
) ==
(uint32_t
)SDLinkTypeMask::SDLT_IC ||
(current_section_type &
(uint32_t
)SDLinkTypeMask::SDLT_JCT
) ==
(uint32_t
)SDLinkTypeMask::SDLT_JCT ||
(current_section_type &
(uint32_t
)SDLinkTypeMask::SDLT_SERVICE
) ==
(uint32_t
)SDLinkTypeMask::SDLT_SERVICE
) {
return true;
}
return false;
};
auto is_highway_ramp = []
(const SDSectionInfo &sd_section_info
) {
uint32_t section_type = sd_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 ||
(sd_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;
}
};
auto is_highway_city_connection =
[is_on_highway_flag, is_highway_ramp]
(
const SDSectionInfo &
pre_sd_section_info,
const SDSectionInfo &cur_sd_section_info,
const SDSectionInfo &suc_sd_section_info
) {
uint32_t cur_section_type = cur_sd_section_info
.link_type;
if
((pre_sd_section_info
.successor_section_id_list
.size
() == 1
) &&
(is_on_highway_flag
(pre_sd_section_info
)) &&
((cur_section_type &
(uint32_t
)SDLinkTypeMask::SDLT_IC
) ==
(uint32_t
)SDLinkTypeMask::SDLT_IC
) &&
(!is_on_highway_flag
(suc_sd_section_info
) &&
(!is_highway_ramp
(suc_sd_section_info
)))) {
return true;
} else {
return false;
}
};
if
(routing_map_raw == nullptr
) {
return false;
}
auto &sd_mpp_sections = routing_map_raw
->sd_route
.mpp_sections;
uint64_t sd_ego_sectionid = routing_map_raw
->sd_route
.navi_start
.section_id;
int ego_section_index =
-1;
for
(unsigned int i = 0; i < sd_mpp_sections
.size
(); i++
) {
if
(sd_ego_sectionid == sd_mpp_sections[i]
.id
) {
ego_section_index = i;
break;
}
}
if
(ego_section_index ==
-1
) {
return routing_map_raw
->is_on_highway;
}
auto ¤t_section = sd_mpp_sections[ego_section_index];
bool IS_ON_HIGHWAY_FLAG = routing_map_raw
->is_on_highway;
if
(IS_ON_HIGHWAY_FLAG
) {
if
(!is_on_highway_flag
(current_section
)) {
return false;
}
if
(ego_section_index >= 1 &&
ego_section_index < sd_mpp_sections
.size
() - 1
) {
if
(is_highway_city_connection
(sd_mpp_sections[ego_section_index
- 1],
current_section,
sd_mpp_sections[ego_section_index + 1]
)) {
return false;
}
}
if
(is_highway_ramp
(current_section
)) {
return true;
}
double length_tmp = fabs
(current_section
.length
- routing_map_raw
->sd_route
.navi_start
.s_offset
);
double max_range = 1500;
for
(unsigned int i = ego_section_index + 1; i < sd_mpp_sections
.size
(); i++
) {
if
(max_range < length_tmp
) {
break;
}
if
(i >= 1 && i < sd_mpp_sections
.size
() - 1
) {
if
(is_highway_city_connection
(sd_mpp_sections[i
- 1],
sd_mpp_sections[i],
sd_mpp_sections[i + 1]
)) {
IS_ON_HIGHWAY_FLAG = false;
break;
}
}
if
(is_on_highway_flag
(sd_mpp_sections[i]
) && is_highway_ramp
(sd_mpp_sections[i]
)) {
break;
}
if
(!is_on_highway_flag
(sd_mpp_sections[i]
)) {
IS_ON_HIGHWAY_FLAG = false;
break;
}
length_tmp += sd_mpp_sections[i]
.length;
}
return IS_ON_HIGHWAY_FLAG;
} else {
return false;
}
}
MapConvertType Master::MapToggleMode
(GeometryMatchResult &match_detail
) {
static constexpr size_t queue_max_size = 30;
if
(bev_ld_matched_info_
.size
() > queue_max_size
) {
bev_ld_matched_info_
.pop
();
}
if
(toggle_to_bev_info_
.size
() > queue_max_size
) {
toggle_to_bev_info_
.pop
();
}
constexpr double distance_bev_to_ld_max = 150;
constexpr double distance_bev_to_ld_min = 0
.5;
constexpr double distance_ld_to_bev_max = 150;
constexpr double distance_threshold = 100;
const bool bev_ld_boundary_matched = bev_ld_map_geometry_match_info_
.IsLaneBoundaryMatch
(match_detail
);
double adc_to_junction = traffic_light_mapping_
->DistanceToJunction
();
bool is_drive_into_unreliable_road = traffic_light_mapping_
->IsUnreliableRoadCrossJunction
();
is_drive_into_unreliable_road = false;
bool distance_bev_to_ld = adc_to_junction < distance_bev_to_ld_max && adc_to_junction > distance_bev_to_ld_min;
bool bev_ld_matched_current_period = distance_bev_to_ld && bev_ld_boundary_matched;
double dis_ego_to_passed_junction = traffic_light_mapping_
->Get
PreviousJunctionEgoPassed
();
LaneInfo ego_lane_ld = bev_ld_map_geometry_match_info_
.GetLdBevLane
();
bool ego_in_junction = ego_lane_ld
.id != 0 && ego_lane_ld
.type == cem::message::env_model::LaneType::LANE_VIRTUAL_JUNCTION;
PLanningResultPtr planning_result{nullptr};
SensorDataManager::Instance
()->GetLatestSensorFrame
(planning_result
);
if
(planning_result
) {
if
(planning_result
->lc_state == PLanningResultData::Keeping
) {
is_lane_keeping_ = true;
} else {
is_lane_keeping_ = false;
}
}
if
(!env_status_
->is_on_freeway
() &&
((match_detail
.fail_reason == MatchFailReason::kNotFailed
) ||
(match_detail
.fail_reason == MatchFailReason::kMatchFailed
))) {
env_status_
->set_distance_to_next_junction
(adc_to_junction
);
env_status_
->set_distance_to_passed_junction
(dis_ego_to_passed_junction
);
env_status_
->mutable_left_laneline_match_info
()->set_average_offset
(match_detail
.left_offset_average
);
env_status_
->mutable_left_laneline_match_info
()->set_max_offset
(match_detail
.left_offset_max
);
env_status_
->mutable_right_laneline_match_info
()->set_average_offset
(match_detail
.right_offset_average
);
env_status_
->mutable_right_laneline_match_info
()->set_max_offset
(match_detail
.right_offset_max
);
}
if
(env_status_
->is_on_freeway
() &&
match_detail
.fail_reason == MatchFailReason::kLostHdMapLane &&
(!is_lane_keeping_
)) {
match_detail
.fail_reason = MatchFailReason::kNotFailed;
}
GEOMETRY_LOG << fmt::format
(
" bev_ld_boundary_matched:{:d} distance_to_junction:{:
.2f} ego_in_junction:{:d} vector_type:{:d} size:{:d} "
"ego_dis_to_last_junction:{:
.2f} is_lane_keeping:{:d} bev_ld_matched_current_period:{:d}",
bev_ld_boundary_matched, adc_to_junction, ego_in_junction,
(toggle_to_bev_info_
.empty
() ? MapType::UNKOWN_MAP : toggle_to_bev_info_
.back
()), toggle_to_bev_info_
.size
(),
dis_ego_to_passed_junction, is_lane_keeping_, bev_ld_matched_current_period
);
switch
(toggle_to_bev_info_
.back
()) {
case MapType::HD_MAP: {
if
(adc_to_junction > distance_ld_to_bev_max && bev_ld_boundary_matched && dis_ego_to_passed_junction > distance_threshold &&
is_lane_keeping_
) {
toggle_to_bev_info_
.emplace
(MapType::BEV_MAP
);
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_EGO_PASSED_JUNCTION
);
return MapConvertType::LD_2_BEV;
}
if
(!is_lane_keeping_
) {
env_status_
->set_perception_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_EGO_IS_CHANGING_LANE
);
} else if
(adc_to_junction <= distance_ld_to_bev_max || dis_ego_to_passed_junction <= distance_threshold
) {
env_status_
->set_perception_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_EGO_IN_JUNCTION
);
} else {
env_status_
->set_perception_map_sup
pression_reason
(byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::
EnvStatus_MapSup
pressionReason_UNABLE_TO_MATCH_PERCEPTION_WITH_HD_MAP
);
}
toggle_to_bev_info_
.emplace
(MapType::HD_MAP
);
return MapConvertType::LD_2_LD;
}
case MapType::BEV_MAP: {
if
(bev_ld_matched_current_period && is_lane_keeping_ &&
(!is_drive_into_unreliable_road
)) {
toggle_to_bev_info_
.emplace
(MapType::HD_MAP
);
env_status_
->set_perception_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_EGO_IN_JUNCTION
);
return MapConvertType::BEV_2_LD;
}
if
(adc_to_junction < distance_bev_to_ld_min
) {
toggle_to_bev_info_
.emplace
(MapType::UNKOWN_MAP
);
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_UNKNOWN_REASON
);
return MapConvertType::BEV_2_UNKNOWN;
}
toggle_to_bev_info_
.emplace
(MapType::BEV_MAP
);
if
(!is_lane_keeping_
) {
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_EGO_IS_CHANGING_LANE
);
} else if
(is_drive_into_unreliable_road
) {
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_CLOSE_TO_HIGH_LEVEL_ROAD
);
} else {
if
(adc_to_junction > distance_bev_to_ld_max
) {
env_status_
->set_hd_map_sup
pression_reason
(byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::
EnvStatus_MapSup
pressionReason_EGO_NOT_CLOSE_ENOUGH_TO_JUNCTION
);
} else {
env_status_
->set_hd_map_sup
pression_reason
(byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::
EnvStatus_MapSup
pressionReason_UNABLE_TO_MATCH_PERCEPTION_WITH_HD_MAP
);
}
}
return MapConvertType::BEV_2_BEV;
}
case MapType::UNKOWN_MAP: {
if
(bev_ld_matched_current_period && is_lane_keeping_
) {
toggle_to_bev_info_
.emplace
(MapType::HD_MAP
);
env_status_
->set_perception_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_EGO_IN_JUNCTION
);
return MapConvertType::UNKNOWN_2_LD;
}
if
(adc_to_junction > distance_bev_to_ld_min * 2 && is_lane_keeping_
) {
toggle_to_bev_info_
.emplace
(MapType::BEV_MAP
);
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_UNKNOWN_REASON
);
return MapConvertType::UNKNOWN_2_BEV;
}
env_status_
->set_hd_map_sup
pression_reason
(
byd::msg::orin::routing_map::EnvStatus::MapSup
pressionReason::EnvStatus_MapSup
pressionReason_UNKNOWN_REASON
);
toggle_to_bev_info_
.emplace
(MapType::UNKOWN_MAP
);
return MapConvertType::UNKNOWN_2_UNKNOWN;
}
default: {
toggle_to_bev_info_
.emplace
(MapType::BEV_MAP
);
return MapConvertType::DEFAULT_BEV;
}
}
toggle_to_bev_info_
.emplace
(MapType::BEV_MAP
);
return MapConvertType::DEFAULT_BEV;
}
/**********************************发送消息的数据结构接口************************************/
std::shared_ptr<byd::msg::basic::ModuleStatus> Master::GetModuleStatusPtr
() {
return this
->messenger_
->GetDiagnoInfo
();
}
std::shared_ptr<byd::msg::orin::routing_map::RoutingMap> Master::GetENVRoutingMapPtr
() {
return this
->messenger_
->GetRoutingMapInfo
();
}
std::shared_ptr<byd::msg::orin::routing_map::TrafficLightsE2EInfo> Master::GetTrafficLightsE2EPtr
() {
return this
->messenger_
->GetTrafficLightsE2EInfo
();
}
/***************************接收消息callback函数
* 在此处实现************************************/
void Master::OnVehInfoCallback
(const std::shared_ptr<byd::msg::drivers::VehInfo> &msg
) {
this
->messenger_
->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::VehicleSignal>
(msg
);
this
->messenger_
->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Bcm50msPdu02>
(msg
);
this
->messenger_
->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Ibs20msPdu08>
(msg
);
this
->messenger_
->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Imcu20msPdu05>
(msg
);
this
->messenger_
->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Eps010msPdu00>
(msg
);
this
->messenger_
->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Ibs20msPdu15>
(msg
);
this
->messenger_
->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Sas10msPdu00>
(msg
);
this
->messenger_
->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Mpd100msPdu04>
(msg
);
this
->messenger_
->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Icm100msPdu29>
(msg
);
this
->messenger_
->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::AdasIpd100msPdu12>
(msg
);
}
void Master::OnEnvModelLocalMapInfoCallback
(const std::shared_ptr<byd::msg::env_model::LocalMapInfo> &msg
) {
this
->messenger_
->Callback<byd::msg::env_model::LocalMapInfo, cem::message::sensor::BevMapInfo>
(msg
);
}
void Master::OnRoutingMapCallBack
(const std::shared_ptr<byd::msg::orin::routing_map::RoutingMap> &msg
) {
this
->messenger_
->Callback<byd::msg::orin::routing_map::RoutingMap, cem::message::env_model::RoutingMap>
(msg
);
}
void Master::OnMsgMapEventCallback
(const std::shared_ptr<byd::msg::orin::routing_map::MapEvent> &msg
) {
this
->messenger_
->Callback<byd::msg::orin::routing_map::MapEvent, cem::message::env_model::MapEvent>
(msg
);
} /* OnMsgMapEventCallback */
void Master::OnPercepTrfInfoCallback
(const std::shared_ptr<byd::msg::perception::PerceptionTrafficInfo> &msg
) {
this
->messenger_
->Callback<byd::msg::perception::PerceptionTrafficInfo, cem::message::sensor::PercepTrfInfo>
(msg
);
}
void Master::OnPerceptionObstaclesInfoCallback
(const std::shared_ptr<byd::msg::perception::PerceptionObstacles> &msg
) {
this
->messenger_
->Callback<byd::msg::perception::PerceptionObstacles, cem::message::sensor::FusObjInfo>
(msg
);
}
void Master::OnMsgDriverInsCallback
(const std::shared_ptr<byd::msg::drivers::Ins> &msg
) {
this
->messenger_
->Callback<byd::msg::drivers::Ins, cem::message::sensor::LocalizationData>
(msg
);
}
void Master::OnMsgLocationDrCallback
(const std::shared_ptr<byd::msg::localization::LocalizationEstimate> &msg
) {
this
->messenger_
->Callback<byd::msg::localization::LocalizationEstimate, cem::message::sensor::LocalizationData>
(msg
);
}
// void Master::OncanoutCallback
(
// const std::shared_ptr<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output> &msg
)
// {
// this
->messenger_
->Callback<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output,
// cem::message::sensor::CAN1>
(msg
);
// }
void Master::OnEnvModelLidaRoadedgeCallback
(const std::shared_ptr<byd::msg::env_model::LocalMapInfo> &msg
) {
this
->messenger_
->Callback<byd::msg::env_model::LocalMapInfo, cem::message::sensor::LidarRoadEdgeInfo>
(msg
);
}
void Master::OnPlanResultCallback
(const std::shared_ptr<byd::msg::planning::PLanningResultProto> &msg
) {
this
->messenger_
->Callback<byd::msg::planning::PLanningResultProto, cem::message::env_model::PLanningResultData>
(msg
);
}
void Master::OnPlanFuncStateCallback
(const std::shared_ptr<byd::msg::pnc::PlanFuncState> &msg
) {
this
->messenger_
->Callback<byd::msg::pnc::PlanFuncState, cem::message::env_model::PLanFuncState>
(msg
);
}
void Master::OncanoutCallback
(const std::shared_ptr<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output> &msg
) {
this
->messenger_
->Callback<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output, cem::message::env_model::CAN1>
(msg
);
}
void Master::OnOccCallback
(const std::shared_ptr<byd::msg::perception::OCCInfo>& msg
){
this
->messenger_
->Callback<byd::msg::perception::OCCInfo,cem::env_model::occ::OCCInfo>
(msg
);
return;
}
/***************************************************************************************************/
} // namespace fusion
} // namespace cem
解读下代码
本文介绍了G.729A编码过程中的预处理步骤，主要包括对语音数据进行除2定标以及应用140Hz高通滤波器。通过滤波器系数a140和b进行计算，更新预处理后的语音数据y存放在new speech中，而原始数据x则用于滤波操作。
最低0.47元/天 解锁文章
扫一扫
1166
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
