Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "ad/rss/core/RssSituationChecking.hpp"
      10                 :            : #include <algorithm>
      11                 :            : #include <memory>
      12                 :            : #include "../situation/RssStructuredSceneIntersectionChecker.hpp"
      13                 :            : #include "../situation/RssStructuredSceneNonIntersectionChecker.hpp"
      14                 :            : #include "../situation/RssUnstructuredSceneChecker.hpp"
      15                 :            : #include "ad/rss/situation/SituationSnapshotValidInputRange.hpp"
      16                 :            : #include "spdlog/fmt/ostr.h"
      17                 :            : #include "spdlog/spdlog.h"
      18                 :            : 
      19                 :            : namespace ad {
      20                 :            : namespace rss {
      21                 :            : namespace core {
      22                 :            : 
      23                 :            : enum class IsSafe
      24                 :            : {
      25                 :            :   Yes,
      26                 :            :   No
      27                 :            : };
      28                 :            : 
      29                 :      67198 : inline state::RssState createRssState(situation::SituationId const &situationId,
      30                 :            :                                       situation::SituationType const &situationType,
      31                 :            :                                       world::ObjectId const &objectId,
      32                 :            :                                       world::RssDynamics const &egoDynamics,
      33                 :            :                                       IsSafe const &isSafeValue)
      34                 :            : {
      35                 :      67198 :   bool const isSafe = (isSafeValue == IsSafe::Yes);
      36         [ +  - ]:      67198 :   state::RssStateInformation emptyRssStateInfo;
      37                 :      67198 :   emptyRssStateInfo.currentDistance = std::numeric_limits<physics::Distance>::max();
      38                 :      67198 :   emptyRssStateInfo.safeDistance = std::numeric_limits<physics::Distance>::max();
      39                 :      67198 :   emptyRssStateInfo.evaluator = state::RssStateEvaluator::None;
      40                 :            : 
      41         [ +  - ]:      67198 :   state::RssState resultRssState;
      42                 :      67198 :   resultRssState.situationId = situationId;
      43                 :      67198 :   resultRssState.situationType = situationType;
      44                 :      67198 :   resultRssState.objectId = objectId;
      45                 :      67198 :   resultRssState.lateralStateLeft.isSafe = isSafe;
      46                 :            :   resultRssState.lateralStateLeft.response
      47         [ +  + ]:      67198 :     = isSafe ? (::ad::rss::state::LateralResponse::None) : (::ad::rss::state::LateralResponse::BrakeMin);
      48                 :      67198 :   resultRssState.lateralStateLeft.alphaLat = egoDynamics.alphaLat;
      49                 :      67198 :   resultRssState.lateralStateLeft.rssStateInformation = emptyRssStateInfo;
      50                 :      67198 :   resultRssState.lateralStateRight.isSafe = isSafe;
      51                 :            :   resultRssState.lateralStateRight.response
      52         [ +  + ]:      67198 :     = isSafe ? (::ad::rss::state::LateralResponse::None) : (::ad::rss::state::LateralResponse::BrakeMin);
      53                 :      67198 :   resultRssState.lateralStateRight.alphaLat = egoDynamics.alphaLat;
      54                 :      67198 :   resultRssState.lateralStateRight.rssStateInformation = emptyRssStateInfo;
      55                 :      67198 :   resultRssState.longitudinalState.isSafe = isSafe;
      56                 :            :   resultRssState.longitudinalState.response
      57         [ +  + ]:      67198 :     = isSafe ? (::ad::rss::state::LongitudinalResponse::None) : (::ad::rss::state::LongitudinalResponse::BrakeMin);
      58                 :      67198 :   resultRssState.longitudinalState.alphaLon = egoDynamics.alphaLon;
      59                 :      67198 :   resultRssState.longitudinalState.rssStateInformation = emptyRssStateInfo;
      60                 :      67198 :   resultRssState.unstructuredSceneState.headingRange.begin = ad::physics::Angle(0.0);
      61                 :      67198 :   resultRssState.unstructuredSceneState.headingRange.end = ad::physics::c2PI;
      62                 :      67198 :   resultRssState.unstructuredSceneState.alphaLon = egoDynamics.alphaLon;
      63                 :      67198 :   resultRssState.unstructuredSceneState.isSafe = isSafe;
      64         [ +  + ]:      67198 :   resultRssState.unstructuredSceneState.response = isSafe ? (::ad::rss::state::UnstructuredSceneResponse::None)
      65                 :            :                                                           : (::ad::rss::state::UnstructuredSceneResponse::Brake);
      66                 :     134396 :   return resultRssState;
      67                 :            : }
      68                 :            : 
      69                 :        744 : RssSituationChecking::RssSituationChecking()
      70                 :            : {
      71                 :            :   try
      72                 :            :   {
      73                 :       1462 :     mNonIntersectionChecker = std::unique_ptr<situation::RssStructuredSceneNonIntersectionChecker>(
      74         [ +  + ]:       1475 :       new situation::RssStructuredSceneNonIntersectionChecker());
      75                 :       1436 :     mIntersectionChecker = std::unique_ptr<situation::RssStructuredSceneIntersectionChecker>(
      76         [ +  + ]:       1449 :       new situation::RssStructuredSceneIntersectionChecker());
      77                 :            :     mUnstructuredSceneChecker
      78         [ +  + ]:        718 :       = std::unique_ptr<situation::RssUnstructuredSceneChecker>(new situation::RssUnstructuredSceneChecker());
      79                 :            :   }
      80         [ +  - ]:         78 :   catch (...)
      81                 :            :   {
      82         [ +  - ]:         39 :     spdlog::critical("RssSituationChecking object initialization failed");
      83                 :         39 :     mNonIntersectionChecker = nullptr;
      84                 :         39 :     mIntersectionChecker = nullptr;
      85                 :         39 :     mUnstructuredSceneChecker = nullptr;
      86                 :            :   }
      87                 :        744 : }
      88                 :            : 
      89                 :        744 : RssSituationChecking::~RssSituationChecking()
      90                 :            : {
      91                 :        744 : }
      92                 :            : 
      93                 :      67095 : bool RssSituationChecking::checkSituationInputRangeChecked(situation::Situation const &situation,
      94                 :            :                                                            state::RssStateSnapshot &rssStateSnapshot)
      95                 :            : {
      96                 :      67095 :   bool result = false;
      97                 :            :   // global try catch block to ensure this library call doesn't throw an exception
      98                 :            :   try
      99                 :            :   {
     100         [ +  - ]:     134151 :     if ((!static_cast<bool>(mNonIntersectionChecker)) || (!static_cast<bool>(mIntersectionChecker))
     101   [ +  +  -  +  :     134151 :         || (!static_cast<bool>(mUnstructuredSceneChecker)))
                   +  + ]
     102                 :            :     {
     103         [ +  - ]:         39 :       spdlog::critical("RssSituationChecking::checkSituationInputRangeChecked>> object not properly initialized");
     104                 :         39 :       return false;
     105                 :            :     }
     106                 :            : 
     107                 :      67056 :     auto rssState = createRssState(situation.situationId,
     108                 :      67056 :                                    situation.situationType,
     109                 :      67056 :                                    situation.objectId,
     110                 :      67056 :                                    situation.egoVehicleState.dynamics,
     111         [ +  - ]:     134112 :                                    IsSafe::No);
     112                 :            : 
     113   [ +  +  +  -  :      67056 :     switch (situation.situationType)
                      + ]
     114                 :            :     {
     115                 :        142 :       case situation::SituationType::NotRelevant:
     116                 :        142 :         rssState = createRssState(situation.situationId,
     117                 :        142 :                                   situation.situationType,
     118                 :        142 :                                   situation.objectId,
     119                 :        142 :                                   situation.egoVehicleState.dynamics,
     120         [ +  - ]:        142 :                                   IsSafe::Yes);
     121                 :        142 :         result = true;
     122                 :        142 :         break;
     123                 :      66134 :       case situation::SituationType::SameDirection:
     124                 :            :       case situation::SituationType::OppositeDirection:
     125         [ +  + ]:      66134 :         result = mNonIntersectionChecker->calculateRssStateNonIntersection(mCurrentTimeIndex, situation, rssState);
     126                 :      66123 :         break;
     127                 :            : 
     128                 :        779 :       case situation::SituationType::IntersectionEgoHasPriority:
     129                 :            :       case situation::SituationType::IntersectionObjectHasPriority:
     130                 :            :       case situation::SituationType::IntersectionSamePriority:
     131         [ +  - ]:        779 :         result = mIntersectionChecker->calculateRssStateIntersection(mCurrentTimeIndex, situation, rssState);
     132                 :        779 :         break;
     133                 :          0 :       case situation::SituationType::Unstructured:
     134                 :          0 :         result = mUnstructuredSceneChecker->calculateRssStateUnstructured(
     135         [ #  # ]:          0 :           mCurrentTimeIndex, situation, rssStateSnapshot.unstructuredSceneEgoInformation, rssState);
     136                 :          0 :         break;
     137                 :          1 :       default:
     138         [ +  - ]:          1 :         spdlog::error("RssSituationChecking::checkSituationInputRangeChecked>> Invalid situation type {}", situation);
     139                 :          1 :         result = false;
     140                 :          1 :         break;
     141                 :            :     }
     142                 :            : 
     143         [ +  + ]:      67045 :     if (result)
     144                 :            :     {
     145         [ +  + ]:      55041 :       rssStateSnapshot.individualResponses.push_back(rssState);
     146                 :            :     }
     147                 :            :   }
     148                 :         26 :   catch (std::exception &e)
     149                 :            :   {
     150         [ +  - ]:         26 :     spdlog::critical(
     151                 :         26 :       "RssSituationChecking::checkSituationInputRangeChecked>> Exception caught '{}' {}", e.what(), situation);
     152                 :         26 :     result = false;
     153                 :            :   }
     154                 :          0 :   catch (...)
     155                 :            :   {
     156         [ -  - ]:          0 :     spdlog::critical("RssSituationChecking::checkSituationInputRangeChecked>> Exception caught {}", situation);
     157                 :          0 :     result = false;
     158                 :            :   }
     159                 :            : 
     160                 :      67056 :   return result;
     161                 :            : }
     162                 :            : 
     163                 :       4035 : bool RssSituationChecking::checkSituations(situation::SituationSnapshot const &situationSnapshot,
     164                 :            :                                            state::RssStateSnapshot &rssStateSnapshot)
     165                 :            : {
     166         [ +  + ]:       4035 :   if (!withinValidInputRange(situationSnapshot))
     167                 :            :   {
     168         [ +  - ]:          1 :     spdlog::error("RssSituationChecking::checkSituations>> Invalid input {}", situationSnapshot);
     169                 :          1 :     return false;
     170                 :            :   }
     171         [ +  + ]:       4034 :   if (!checkTimeIncreasingConsistently(situationSnapshot.timeIndex))
     172                 :            :   {
     173         [ +  - ]:         50 :     spdlog::error("RssSituationChecking::checkSituations>> Inconsistent time {}", situationSnapshot.timeIndex);
     174                 :         25 :     return false;
     175                 :            :   }
     176                 :       4009 :   bool result = true;
     177                 :            :   // global try catch block to ensure this library call doesn't throw an exception
     178                 :            :   try
     179                 :            :   {
     180                 :       4009 :     rssStateSnapshot.timeIndex = situationSnapshot.timeIndex;
     181                 :       4009 :     rssStateSnapshot.defaultEgoVehicleRssDynamics = situationSnapshot.defaultEgoVehicleRssDynamics;
     182                 :       4009 :     rssStateSnapshot.individualResponses.clear();
     183                 :       4009 :     rssStateSnapshot.unstructuredSceneEgoInformation.brakeTrajectorySet.clear();
     184                 :       4009 :     rssStateSnapshot.unstructuredSceneEgoInformation.continueForwardTrajectorySet.clear();
     185                 :            : 
     186   [ +  +  +  +  :      59098 :     for (auto it = situationSnapshot.situations.begin(); (it != situationSnapshot.situations.end()) && result; ++it)
                   +  + ]
     187                 :            :     {
     188         [ +  - ]:      55089 :       result = checkSituationInputRangeChecked(*it, rssStateSnapshot);
     189                 :            :     }
     190                 :            :   }
     191                 :          0 :   catch (...)
     192                 :            :   {
     193         [ -  - ]:          0 :     spdlog::critical("RssSituationChecking::checkSituations>> Exception caught {}", situationSnapshot);
     194                 :          0 :     result = false;
     195                 :            :   }
     196         [ +  + ]:       4009 :   if (!result)
     197                 :            :   {
     198                 :         68 :     rssStateSnapshot.individualResponses.clear();
     199                 :            :   }
     200                 :       4009 :   return result;
     201                 :            : }
     202                 :            : 
     203                 :       4035 : bool RssSituationChecking::checkTimeIncreasingConsistently(world::TimeIndex const &nextTimeIndex)
     204                 :            : {
     205                 :       4035 :   bool timeIsIncreasing = false;
     206         [ +  + ]:       4035 :   if (mCurrentTimeIndex != nextTimeIndex)
     207                 :            :   {
     208                 :            :     // check for overflow
     209                 :       4031 :     world::TimeIndex const deltaTimeIndex = nextTimeIndex - mCurrentTimeIndex;
     210         [ +  + ]:       4031 :     if (deltaTimeIndex < (std::numeric_limits<world::TimeIndex>::max() / 2))
     211                 :            :     {
     212                 :       4010 :       timeIsIncreasing = true;
     213                 :            :     }
     214                 :            :   }
     215                 :       4035 :   mCurrentTimeIndex = nextTimeIndex;
     216                 :       4035 :   return timeIsIncreasing;
     217                 :            : }
     218                 :            : 
     219                 :            : } // namespace core
     220                 :            : } // namespace rss
     221                 :            : } // namespace ad