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 "RssStructuredSceneNonIntersectionChecker.hpp"
      10                 :            : #include "ad/rss/situation/RssFormulas.hpp"
      11                 :            : #include "ad/rss/state/RssStateOperation.hpp"
      12                 :            : 
      13                 :            : namespace ad {
      14                 :            : namespace rss {
      15                 :            : namespace situation {
      16                 :            : 
      17                 :      66188 : bool RssStructuredSceneNonIntersectionChecker::calculateRssStateNonIntersection(world::TimeIndex const &timeIndex,
      18                 :            :                                                                                 Situation const &situation,
      19                 :            :                                                                                 state::RssState &rssState)
      20                 :            : {
      21         [ +  + ]:      66188 :   if (timeIndex != mCurrentTimeIndex)
      22                 :            :   {
      23                 :       3102 :     mLastStatesBeforeDangerThresholdTime.swap(mNewStatesBeforeDangerThresholdTime);
      24                 :       3102 :     mNewStatesBeforeDangerThresholdTime.clear();
      25                 :       3102 :     mCurrentTimeIndex = timeIndex;
      26                 :            :   }
      27                 :            : 
      28                 :      66188 :   rssState.situationId = situation.situationId;
      29                 :      66188 :   rssState.situationType = situation.situationType;
      30                 :      66188 :   rssState.objectId = situation.objectId;
      31                 :            : 
      32                 :      66188 :   rssState.longitudinalState.isSafe = false;
      33                 :      66188 :   rssState.longitudinalState.response = state::LongitudinalResponse::BrakeMin;
      34                 :      66188 :   rssState.longitudinalState.alphaLon = situation.egoVehicleState.dynamics.alphaLon;
      35                 :            : 
      36                 :      66188 :   rssState.lateralStateLeft.isSafe = false;
      37                 :      66188 :   rssState.lateralStateLeft.response = state::LateralResponse::BrakeMin;
      38                 :      66188 :   rssState.lateralStateLeft.alphaLat = situation.egoVehicleState.dynamics.alphaLat;
      39                 :            : 
      40                 :      66188 :   rssState.lateralStateRight.isSafe = false;
      41                 :      66188 :   rssState.lateralStateRight.response = state::LateralResponse::BrakeMin;
      42                 :      66188 :   rssState.lateralStateRight.alphaLat = situation.egoVehicleState.dynamics.alphaLat;
      43                 :            : 
      44                 :      66188 :   bool result = false;
      45                 :            :   // first calculate the current state
      46         [ +  + ]:      66188 :   if (situation.situationType == situation::SituationType::SameDirection)
      47                 :            :   {
      48         [ +  - ]:      65120 :     result = calculateRssStateSameDirection(situation, rssState);
      49                 :            :   }
      50         [ +  - ]:       1068 :   else if (situation.situationType == situation::SituationType::OppositeDirection)
      51                 :            :   {
      52         [ +  - ]:       1068 :     result = calculateRssStateOppositeDirection(situation, rssState);
      53                 :            :   }
      54                 :            :   else
      55                 :            :   {
      56         [ #  # ]:          0 :     spdlog::error(
      57                 :            :       "RssStructuredSceneNonIntersectionChecker::calculateRssStateNonIntersection>> situation type invalid {}",
      58                 :            :       situation);
      59                 :            :   }
      60                 :            : 
      61                 :            :   // second calculate proper response in respect to the state before danger threshold according to definition 10 of the
      62                 :            :   // RSS paper v6
      63                 :      66188 :   RssSafeState nonDangerousStateToRemember;
      64         [ +  + ]:      66188 :   if (isDangerous(rssState))
      65                 :            :   {
      66         [ +  - ]:      13301 :     auto const previousNonDangerousState = mLastStatesBeforeDangerThresholdTime.find(rssState.situationId);
      67         [ +  + ]:      13301 :     if (previousNonDangerousState != mLastStatesBeforeDangerThresholdTime.end())
      68                 :            :     {
      69                 :       1228 :       nonDangerousStateToRemember = previousNonDangerousState->second;
      70                 :            :     }
      71                 :            :   }
      72                 :            :   else
      73                 :            :   {
      74                 :      52887 :     nonDangerousStateToRemember.longitudinalSafe = isLongitudinalSafe(rssState);
      75                 :      52887 :     nonDangerousStateToRemember.lateralSafe = isLateralSafe(rssState);
      76                 :            :   }
      77                 :            : 
      78   [ +  +  +  + ]:      66188 :   if ((nonDangerousStateToRemember.lateralSafe) && (nonDangerousStateToRemember.longitudinalSafe))
      79                 :            :   {
      80                 :            :     // Both longitudinal and lateral distances became dangerous at the same time
      81         [ -  + ]:        321 :     if (isDangerous(rssState))
      82                 :            :     {
      83         [ #  # ]:          0 :       spdlog::info("RssStructuredSceneNonIntersectionChecker>> State is dangerous (t_b == t_b,lon == t_b,lat) {}",
      84                 :            :                    rssState);
      85                 :            :     }
      86                 :            :   }
      87         [ +  + ]:      65867 :   else if (nonDangerousStateToRemember.lateralSafe)
      88                 :            :   {
      89                 :            :     // @todo: Handling of a cut-in by a leading vehicle as stated in definitions 11-13 of the RSS paper v6
      90                 :            :     //        will be handled outside of this function. As a consequence.
      91                 :            :     //        There is currently no response for a cut-in of a leading vehicle
      92                 :       1232 :     rssState.longitudinalState.response = state::LongitudinalResponse::None;
      93         [ +  + ]:       1232 :     if (isDangerous(rssState))
      94                 :            :     {
      95         [ +  - ]:        198 :       spdlog::info(
      96                 :            :         "RssStructuredSceneNonIntersectionChecker>> State is dangerous (t_b == t_b,lat) No longitudinal response: {}",
      97                 :            :         rssState);
      98                 :            :     }
      99                 :            :   }
     100         [ +  + ]:      64635 :   else if (nonDangerousStateToRemember.longitudinalSafe)
     101                 :            :   {
     102                 :      52562 :     rssState.lateralStateLeft.response = state::LateralResponse::None;
     103                 :      52562 :     rssState.lateralStateRight.response = state::LateralResponse::None;
     104         [ +  + ]:      52562 :     if (isDangerous(rssState))
     105                 :            :     {
     106         [ +  - ]:       1030 :       spdlog::info(
     107                 :            :         "RssStructuredSceneNonIntersectionChecker>> State is dangerous (t_b == t_b,lon) No lateral response: {}",
     108                 :            :         rssState);
     109                 :            :     }
     110                 :            :   }
     111                 :            :   else
     112                 :            :   {
     113                 :            :     // no non dangerous state available
     114         [ +  - ]:      12073 :     if (isDangerous(rssState))
     115                 :            :     {
     116         [ +  - ]:      12073 :       spdlog::info("RssStructuredSceneNonIntersectionChecker>> State is dangerous, no non dangerous state available {}",
     117                 :            :                    rssState);
     118                 :            :     }
     119                 :            :   }
     120                 :            : 
     121                 :            :   // store state for the next time step
     122   [ +  +  +  + ]:      66188 :   if (nonDangerousStateToRemember.longitudinalSafe || nonDangerousStateToRemember.lateralSafe)
     123                 :            :   {
     124                 :            :     auto const insertResult = mNewStatesBeforeDangerThresholdTime.insert(
     125         [ +  + ]:      54115 :       RssSafeStateBeforeDangerThresholdTimeMap::value_type(situation.situationId, nonDangerousStateToRemember));
     126                 :            : 
     127         [ -  + ]:      54104 :     if (!insertResult.second)
     128                 :            :     {
     129                 :          0 :       result = false;
     130         [ #  # ]:          0 :       spdlog::error("RssStructuredSceneNonIntersectionChecker>> map insertion failed unexpectedly");
     131                 :            :     }
     132                 :            :   }
     133                 :            : 
     134                 :      66177 :   return result;
     135                 :            : }
     136                 :            : 
     137                 :      65120 : bool RssStructuredSceneNonIntersectionChecker::calculateRssStateSameDirection(Situation const &situation,
     138                 :            :                                                                               state::RssState &rssState)
     139                 :            : {
     140                 :      65120 :   bool result = calculateLongitudinalRssStateSameDirection(situation, rssState.longitudinalState);
     141         [ +  + ]:      65120 :   if (result)
     142                 :            :   {
     143                 :      53119 :     result = calculateLateralRssState(situation, rssState.lateralStateLeft, rssState.lateralStateRight);
     144                 :            :   }
     145                 :      65120 :   return result;
     146                 :            : }
     147                 :            : 
     148                 :       1068 : bool RssStructuredSceneNonIntersectionChecker::calculateRssStateOppositeDirection(Situation const &situation,
     149                 :            :                                                                                   state::RssState &rssState)
     150                 :            : {
     151                 :       1068 :   bool result = calculateLongitudinalRssStateOppositeDirection(situation, rssState.longitudinalState);
     152         [ +  + ]:       1068 :   if (result)
     153                 :            :   {
     154                 :       1066 :     result = calculateLateralRssState(situation, rssState.lateralStateLeft, rssState.lateralStateRight);
     155                 :            :   }
     156                 :       1068 :   return result;
     157                 :            : }
     158                 :            : 
     159                 :      65120 : bool RssStructuredSceneNonIntersectionChecker::calculateLongitudinalRssStateSameDirection(
     160                 :            :   Situation const &situation, state::LongitudinalRssState &rssState)
     161                 :            : {
     162                 :      65120 :   bool result = false;
     163                 :            : 
     164                 :      65120 :   rssState.response = state::LongitudinalResponse::BrakeMin;
     165                 :      65120 :   rssState.rssStateInformation.currentDistance = situation.relativePosition.longitudinalDistance;
     166                 :            : 
     167                 :      65120 :   bool isSafe = false;
     168                 :            : 
     169         [ +  + ]:      65120 :   if ((LongitudinalRelativePosition::InFront == situation.relativePosition.longitudinalPosition)
     170         [ +  + ]:       2082 :       || (LongitudinalRelativePosition::OverlapFront == situation.relativePosition.longitudinalPosition))
     171                 :            :   {
     172                 :      63138 :     rssState.rssStateInformation.evaluator = state::RssStateEvaluator::LongitudinalDistanceSameDirectionEgoFront;
     173                 :            : 
     174                 :            :     // The ego vehicle is leading in this situation so we don't need to break longitudinal
     175                 :      63138 :     rssState.response = state::LongitudinalResponse::None;
     176                 :            : 
     177                 :      63138 :     result = checkSafeLongitudinalDistanceSameDirection(situation.egoVehicleState,
     178                 :      63138 :                                                         situation.otherVehicleState,
     179                 :      63138 :                                                         situation.relativePosition.longitudinalDistance,
     180         [ +  - ]:      63138 :                                                         rssState.rssStateInformation.safeDistance,
     181                 :            :                                                         isSafe);
     182                 :            :   }
     183                 :            :   else
     184                 :            :   {
     185                 :       1982 :     rssState.rssStateInformation.evaluator = state::RssStateEvaluator::LongitudinalDistanceSameDirectionOtherInFront;
     186                 :            : 
     187                 :       1982 :     result = checkSafeLongitudinalDistanceSameDirection(situation.otherVehicleState,
     188                 :       1982 :                                                         situation.egoVehicleState,
     189                 :       1982 :                                                         situation.relativePosition.longitudinalDistance,
     190         [ +  - ]:       1982 :                                                         rssState.rssStateInformation.safeDistance,
     191                 :            :                                                         isSafe);
     192                 :            :   }
     193                 :            : 
     194                 :      65120 :   rssState.isSafe = isSafe;
     195         [ +  + ]:      65120 :   if (isSafe)
     196                 :            :   {
     197                 :      51462 :     rssState.response = state::LongitudinalResponse::None;
     198                 :            :   }
     199                 :            : 
     200                 :      65120 :   return result;
     201                 :            : }
     202                 :            : 
     203                 :       1068 : bool RssStructuredSceneNonIntersectionChecker::calculateLongitudinalRssStateOppositeDirection(
     204                 :            :   Situation const &situation, state::LongitudinalRssState &rssState)
     205                 :            : {
     206                 :       1068 :   bool result = false;
     207                 :            : 
     208                 :       1068 :   bool isSafe = false;
     209                 :       1068 :   rssState.response = state::LongitudinalResponse::BrakeMin;
     210                 :       1068 :   rssState.rssStateInformation.currentDistance = situation.relativePosition.longitudinalDistance;
     211                 :            : 
     212         [ +  + ]:       1068 :   if (situation.egoVehicleState.isInCorrectLane)
     213                 :            :   {
     214                 :            :     rssState.rssStateInformation.evaluator
     215                 :        748 :       = state::RssStateEvaluator::LongitudinalDistanceOppositeDirectionEgoCorrectLane;
     216                 :            : 
     217                 :       1496 :     result = checkSafeLongitudinalDistanceOppositeDirection(situation.egoVehicleState,
     218                 :        748 :                                                             situation.otherVehicleState,
     219                 :        748 :                                                             situation.relativePosition.longitudinalDistance,
     220         [ +  - ]:        748 :                                                             rssState.rssStateInformation.safeDistance,
     221                 :            :                                                             isSafe);
     222                 :        748 :     rssState.response = state::LongitudinalResponse::BrakeMinCorrect;
     223                 :            :   }
     224                 :            :   else
     225                 :            :   {
     226                 :        320 :     rssState.rssStateInformation.evaluator = state::RssStateEvaluator::LongitudinalDistanceOppositeDirection;
     227                 :            : 
     228                 :        320 :     result = checkSafeLongitudinalDistanceOppositeDirection(situation.otherVehicleState,
     229                 :        320 :                                                             situation.egoVehicleState,
     230                 :        320 :                                                             situation.relativePosition.longitudinalDistance,
     231         [ +  - ]:        320 :                                                             rssState.rssStateInformation.safeDistance,
     232                 :            :                                                             isSafe);
     233                 :            :   }
     234                 :            : 
     235                 :       1068 :   rssState.isSafe = isSafe;
     236         [ +  + ]:       1068 :   if (rssState.isSafe)
     237                 :            :   {
     238                 :        391 :     rssState.response = state::LongitudinalResponse::None;
     239                 :            :   }
     240                 :            : 
     241                 :       1068 :   return result;
     242                 :            : }
     243                 :            : 
     244                 :      54185 : bool RssStructuredSceneNonIntersectionChecker::calculateLateralRssState(Situation const &situation,
     245                 :            :                                                                         state::LateralRssState &rssStateLeft,
     246                 :            :                                                                         state::LateralRssState &rssStateRight)
     247                 :            : {
     248                 :      54185 :   rssStateLeft.isSafe = false;
     249                 :      54185 :   rssStateLeft.response = state::LateralResponse::BrakeMin;
     250                 :      54185 :   rssStateRight.isSafe = false;
     251                 :      54185 :   rssStateRight.response = state::LateralResponse::BrakeMin;
     252                 :            : 
     253                 :      54185 :   bool isDistanceSafe = false;
     254                 :            : 
     255                 :      54185 :   bool result = false;
     256         [ +  + ]:      54185 :   if (LateralRelativePosition::AtLeft == situation.relativePosition.lateralPosition)
     257                 :            :   {
     258                 :        913 :     rssStateLeft.rssStateInformation.evaluator = state::RssStateEvaluator::None;
     259                 :        913 :     rssStateLeft.rssStateInformation.currentDistance = std::numeric_limits<physics::Distance>::max();
     260                 :        913 :     rssStateLeft.rssStateInformation.safeDistance = std::numeric_limits<physics::Distance>::max();
     261                 :            : 
     262                 :            :     // ego is the left vehicle, so right side has to be checked
     263                 :        913 :     rssStateRight.rssStateInformation.evaluator = state::RssStateEvaluator::LateralDistance;
     264                 :        913 :     rssStateRight.rssStateInformation.currentDistance = situation.relativePosition.lateralDistance;
     265                 :        913 :     result = checkSafeLateralDistance(situation.egoVehicleState,
     266                 :        913 :                                       situation.otherVehicleState,
     267                 :        913 :                                       situation.relativePosition.lateralDistance,
     268         [ +  - ]:        913 :                                       rssStateRight.rssStateInformation.safeDistance,
     269                 :            :                                       isDistanceSafe);
     270                 :            :   }
     271         [ +  + ]:      53272 :   else if (LateralRelativePosition::AtRight == situation.relativePosition.lateralPosition)
     272                 :            :   {
     273                 :       1094 :     rssStateRight.rssStateInformation.evaluator = state::RssStateEvaluator::None;
     274                 :       1094 :     rssStateRight.rssStateInformation.currentDistance = std::numeric_limits<physics::Distance>::max();
     275                 :       1094 :     rssStateRight.rssStateInformation.safeDistance = std::numeric_limits<physics::Distance>::max();
     276                 :            : 
     277                 :            :     // ego is the right vehicle, so left side has to be checked
     278                 :       1094 :     rssStateLeft.rssStateInformation.evaluator = state::RssStateEvaluator::LateralDistance;
     279                 :       1094 :     rssStateLeft.rssStateInformation.currentDistance = situation.relativePosition.lateralDistance;
     280                 :       1094 :     result = checkSafeLateralDistance(situation.otherVehicleState,
     281                 :       1094 :                                       situation.egoVehicleState,
     282                 :       1094 :                                       situation.relativePosition.lateralDistance,
     283         [ +  - ]:       1094 :                                       rssStateLeft.rssStateInformation.safeDistance,
     284                 :            :                                       isDistanceSafe);
     285                 :            :   }
     286                 :            :   else
     287                 :            :   {
     288                 :      52178 :     rssStateLeft.rssStateInformation.evaluator = state::RssStateEvaluator::LateralDistance;
     289                 :      52178 :     rssStateLeft.rssStateInformation.currentDistance = physics::Distance(0);
     290                 :      52178 :     rssStateLeft.rssStateInformation.safeDistance = physics::Distance(0);
     291                 :      52178 :     rssStateRight.rssStateInformation.evaluator = state::RssStateEvaluator::LateralDistance;
     292                 :      52178 :     rssStateRight.rssStateInformation.currentDistance = physics::Distance(0);
     293                 :      52178 :     rssStateRight.rssStateInformation.safeDistance = physics::Distance(0);
     294                 :            : 
     295                 :            :     // lateral distance is zero, never safe
     296                 :      52178 :     result = true;
     297                 :            :   }
     298                 :            : 
     299         [ +  + ]:      54185 :   if (isDistanceSafe)
     300                 :            :   {
     301                 :       1355 :     rssStateLeft.isSafe = true;
     302                 :       1355 :     rssStateLeft.response = state::LateralResponse::None;
     303                 :       1355 :     rssStateRight.isSafe = true;
     304                 :       1355 :     rssStateRight.response = state::LateralResponse::None;
     305                 :            :   }
     306         [ +  + ]:      52830 :   else if ((LateralRelativePosition::AtLeft == situation.relativePosition.lateralPosition)
     307         [ +  + ]:      52673 :            || (LateralRelativePosition::OverlapLeft == situation.relativePosition.lateralPosition))
     308                 :            :   {
     309                 :            :     // ego is the left vehicle, so the collision is on the right side
     310                 :        260 :     rssStateLeft.isSafe = true;
     311                 :        260 :     rssStateLeft.response = state::LateralResponse::None;
     312                 :            :   }
     313         [ +  + ]:      52570 :   else if ((LateralRelativePosition::AtRight == situation.relativePosition.lateralPosition)
     314         [ +  + ]:      52075 :            || (LateralRelativePosition::OverlapRight == situation.relativePosition.lateralPosition))
     315                 :            :   {
     316                 :            :     // ego is the right vehicle, so the collision is on the left side
     317                 :        597 :     rssStateRight.isSafe = true;
     318                 :        597 :     rssStateRight.response = state::LateralResponse::None;
     319                 :            :   }
     320                 :            : 
     321                 :      54185 :   return result;
     322                 :            : }
     323                 :            : 
     324                 :            : } // namespace situation
     325                 :            : } // namespace rss
     326                 :            : } // namespace ad