diff --git a/modules/prediction/scenario/prioritization/obstacles_prioritizer.cc b/modules/prediction/scenario/prioritization/obstacles_prioritizer.cc index 02739e45c50a66503177e71b58d5fa32d8af9193..34392698d86d1fa5e607665c60a844cb9a4bcc41 100644 --- a/modules/prediction/scenario/prioritization/obstacles_prioritizer.cc +++ b/modules/prediction/scenario/prioritization/obstacles_prioritizer.cc @@ -183,11 +183,37 @@ void ObstaclesPrioritizer::AssignIgnoreLevel() { } void ObstaclesPrioritizer::AssignCautionLevel() { + AssignCautionLevelInJunction(); AssignCautionLevelCruiseKeepLane(); AssignCautionLevelCruiseChangeLane(); AssignCautionLevelByEgoReferenceLine(); } +void ObstaclesPrioritizer::AssignCautionLevelInJunction() { + auto obstacles_container = + ContainerManager::Instance()->GetContainer( + AdapterConfig::PERCEPTION_OBSTACLES); + if (obstacles_container == nullptr) { + AERROR << "Obstacles container pointer is a null pointer."; + return; + } + + // TODO(Hongyi): get current junction_id from Storytelling + std::string curr_junction_id; + const auto& obstacle_ids = + obstacles_container->curr_frame_movable_obstacle_ids(); + for (const int obstacle_id : obstacle_ids) { + Obstacle* obstacle_ptr = obstacles_container->GetObstacle(obstacle_id); + if (obstacle_ptr == nullptr) { + AERROR << "Null obstacle pointer found."; + continue; + } + if (obstacle_ptr->IsInJunction(curr_junction_id)) { + obstacle_ptr->SetCaution(); + } + } +} + void ObstaclesPrioritizer::AssignCautionLevelCruiseKeepLane() { ObstaclesContainer* obstacles_container = ContainerManager::Instance()->GetContainer( diff --git a/modules/prediction/scenario/prioritization/obstacles_prioritizer.h b/modules/prediction/scenario/prioritization/obstacles_prioritizer.h index 5f4ff59f908894e79e27385158ac9fc04d6a743f..bbe110d8a24dc54b507de77cc2c48ffe2ae49e42 100644 --- a/modules/prediction/scenario/prioritization/obstacles_prioritizer.h +++ b/modules/prediction/scenario/prioritization/obstacles_prioritizer.h @@ -37,6 +37,8 @@ class ObstaclesPrioritizer { void AssignCautionLevel(); private: + void AssignCautionLevelInJunction(); + void AssignCautionLevelCruiseKeepLane(); void AssignCautionLevelCruiseChangeLane();