LCOV - code coverage report
Current view: top level - src/situation - RssStructuredSceneIntersectionChecker.cpp (source / functions) Hit Total Coverage
Test: ad_rss Lines: 112 113 99.1 %
Date: 2024-03-25 17:12:56 Functions: 3 3 100.0 %
Branches: 69 94 73.4 %

           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 "RssStructuredSceneIntersectionChecker.hpp"
      10                 :            : #include <cmath>
      11                 :            : #include <limits>
      12                 :            : #include "ad/rss/situation/Physics.hpp"
      13                 :            : #include "ad/rss/situation/RssFormulas.hpp"
      14                 :            : 
      15                 :            : namespace ad {
      16                 :            : 
      17                 :            : using physics::Duration;
      18                 :            : 
      19                 :            : namespace rss {
      20                 :            : namespace situation {
      21                 :            : 
      22                 :            : using situation::calculateTimeToCoverDistance;
      23                 :            : 
      24                 :        241 : bool RssStructuredSceneIntersectionChecker::checkLateralIntersect(Situation const &situation, bool &isSafe)
      25                 :            : {
      26                 :        241 :   isSafe = false;
      27                 :            : 
      28                 :            :   /**
      29                 :            :    * Check if in any case the first vehicle has passed the intersection before the other
      30                 :            :    * vehicle arrives at that point
      31                 :            :    */
      32                 :            : 
      33                 :        241 :   Duration timeToReachEgo;
      34                 :        241 :   Duration timeToReachOther;
      35                 :        241 :   Duration timeToLeaveEgo;
      36                 :        241 :   Duration timeToLeaveOther;
      37                 :            : 
      38         [ +  - ]:        241 :   bool result = calculateTimeToCoverDistance(situation.egoVehicleState.velocity.speedLon.maximum,
      39                 :            :                                              situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
      40                 :            :                                              situation.egoVehicleState.dynamics.responseTime,
      41                 :            :                                              situation.egoVehicleState.dynamics.alphaLon.accelMax,
      42                 :            :                                              situation.egoVehicleState.dynamics.alphaLon.brakeMin,
      43                 :            :                                              situation.egoVehicleState.distanceToEnterIntersection,
      44                 :            :                                              timeToReachEgo);
      45                 :            : 
      46                 :        241 :   result = result
      47   [ +  -  +  -  :        241 :     && calculateTimeToCoverDistance(situation.otherVehicleState.velocity.speedLon.maximum,
                   +  - ]
      48                 :            :                                     situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
      49                 :            :                                     situation.otherVehicleState.dynamics.responseTime,
      50                 :            :                                     situation.otherVehicleState.dynamics.alphaLon.accelMax,
      51                 :            :                                     situation.otherVehicleState.dynamics.alphaLon.brakeMin,
      52                 :            :                                     situation.otherVehicleState.distanceToEnterIntersection,
      53                 :            :                                     timeToReachOther);
      54                 :            : 
      55                 :        241 :   result = result
      56   [ +  -  +  -  :        241 :     && calculateTimeToCoverDistance(situation.egoVehicleState.velocity.speedLon.minimum,
                   +  - ]
      57                 :            :                                     situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
      58                 :            :                                     situation.egoVehicleState.dynamics.responseTime,
      59                 :            :                                     situation.egoVehicleState.dynamics.alphaLon.brakeMax,
      60                 :            :                                     situation.egoVehicleState.dynamics.alphaLon.brakeMax,
      61                 :            :                                     situation.egoVehicleState.distanceToLeaveIntersection,
      62                 :            :                                     timeToLeaveEgo);
      63                 :            : 
      64                 :        241 :   result = result
      65   [ +  -  +  -  :        241 :     && calculateTimeToCoverDistance(situation.otherVehicleState.velocity.speedLon.minimum,
                   +  - ]
      66                 :            :                                     situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
      67                 :            :                                     situation.otherVehicleState.dynamics.responseTime,
      68                 :            :                                     situation.otherVehicleState.dynamics.alphaLon.brakeMax,
      69                 :            :                                     situation.otherVehicleState.dynamics.alphaLon.brakeMax,
      70                 :            :                                     situation.otherVehicleState.distanceToLeaveIntersection,
      71                 :            :                                     timeToLeaveOther);
      72                 :            : 
      73         [ +  - ]:        241 :   if (result)
      74                 :            :   {
      75   [ +  -  +  -  :        482 :     if ((timeToReachEgo > timeToLeaveOther) || (timeToReachOther > timeToLeaveEgo)
                   +  + ]
      76   [ +  -  +  -  :        571 :         || ((timeToReachEgo == std::numeric_limits<Duration>::max())
                   +  + ]
      77   [ +  -  -  +  :        330 :             && (timeToReachOther == std::numeric_limits<Duration>::max())))
                   +  + ]
      78                 :            :     {
      79                 :            :       /**
      80                 :            :        * Currently we assume lateral overlap when there is a vehicle in an intersection
      81                 :            :        * @todo This could be relaxed by calculating lateral distance here as well
      82                 :            :        */
      83                 :          1 :       isSafe = true;
      84                 :            :     }
      85                 :            :   }
      86                 :            : 
      87                 :        241 :   return result;
      88                 :            : }
      89                 :            : 
      90                 :        804 : bool RssStructuredSceneIntersectionChecker::checkIntersectionSafe(Situation const &situation,
      91                 :            :                                                                   state::RssStateInformation &rssStateInformation,
      92                 :            :                                                                   bool &isSafe,
      93                 :            :                                                                   IntersectionState &intersectionState)
      94                 :            : {
      95                 :        804 :   if ((situation.egoVehicleState.distanceToLeaveIntersection < situation.egoVehicleState.distanceToEnterIntersection)
      96   [ +  +  +  + ]:       1607 :       || (situation.otherVehicleState.distanceToLeaveIntersection
      97         [ +  + ]:        803 :           < situation.otherVehicleState.distanceToEnterIntersection))
      98                 :            :   {
      99                 :          2 :     return false;
     100                 :            :   }
     101                 :            : 
     102                 :        802 :   bool result = true;
     103                 :        802 :   isSafe = false;
     104                 :            : 
     105                 :            :   /**
     106                 :            :    * Check if a non prio vehicle has safe distance to the intersection
     107                 :            :    */
     108         [ +  + ]:        802 :   if (!situation.egoVehicleState.hasPriority)
     109                 :            :   {
     110                 :        417 :     rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionOtherPriorityEgoAbleToStop;
     111                 :        417 :     rssStateInformation.currentDistance = situation.egoVehicleState.distanceToEnterIntersection;
     112                 :        417 :     result = checkStopInFrontIntersection(situation.egoVehicleState, rssStateInformation.safeDistance, isSafe);
     113                 :            :   }
     114   [ +  -  +  +  :        802 :   if (result && !isSafe && !situation.otherVehicleState.hasPriority)
                   +  + ]
     115                 :            :   {
     116                 :        430 :     rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionEgoPriorityOtherAbleToStop;
     117                 :        430 :     rssStateInformation.currentDistance = situation.otherVehicleState.distanceToEnterIntersection;
     118                 :        430 :     result = checkStopInFrontIntersection(situation.otherVehicleState, rssStateInformation.safeDistance, isSafe);
     119                 :            :   }
     120                 :            : 
     121         [ +  + ]:        802 :   if (isSafe)
     122                 :            :   {
     123                 :        333 :     intersectionState = IntersectionState::NonPrioAbleToBreak;
     124                 :            :   }
     125         [ +  - ]:        469 :   else if (result)
     126                 :            :   {
     127                 :        469 :     rssStateInformation.currentDistance = situation.relativePosition.longitudinalDistance;
     128                 :            :     /**
     129                 :            :      * Check if there is a safe longitudinal distance between the vehicles
     130                 :            :      */
     131         [ +  + ]:        469 :     if (situation.relativePosition.longitudinalPosition == LongitudinalRelativePosition::InFront)
     132                 :            :     {
     133                 :        114 :       rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionEgoInFront;
     134                 :        114 :       result = checkSafeLongitudinalDistanceSameDirection(situation.egoVehicleState,
     135                 :        114 :                                                           situation.otherVehicleState,
     136                 :        114 :                                                           situation.relativePosition.longitudinalDistance,
     137                 :        114 :                                                           rssStateInformation.safeDistance,
     138                 :            :                                                           isSafe);
     139                 :            :     }
     140                 :            :     else
     141                 :            :     {
     142                 :        355 :       rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionOtherInFront;
     143                 :        355 :       result = checkSafeLongitudinalDistanceSameDirection(situation.otherVehicleState,
     144                 :        355 :                                                           situation.egoVehicleState,
     145                 :        355 :                                                           situation.relativePosition.longitudinalDistance,
     146                 :        355 :                                                           rssStateInformation.safeDistance,
     147                 :            :                                                           isSafe);
     148                 :            :     }
     149         [ +  + ]:        469 :     if (isSafe)
     150                 :            :     {
     151                 :        228 :       intersectionState = IntersectionState::SafeLongitudinalDistance;
     152                 :            :     }
     153         [ +  - ]:        241 :     else if (result)
     154                 :            :     {
     155                 :        241 :       rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionOverlap;
     156                 :        241 :       rssStateInformation.currentDistance = physics::Distance(0.);
     157                 :        241 :       rssStateInformation.safeDistance = physics::Distance(0.);
     158                 :        241 :       result = checkLateralIntersect(situation, isSafe);
     159                 :            : 
     160         [ +  + ]:        241 :       if (isSafe)
     161                 :            :       {
     162                 :          1 :         intersectionState = IntersectionState::NoTimeOverlap;
     163                 :            :       }
     164                 :            :     }
     165                 :            :   }
     166                 :            : 
     167                 :        802 :   return result;
     168                 :            : }
     169                 :            : 
     170                 :        805 : bool RssStructuredSceneIntersectionChecker::calculateRssStateIntersection(world::TimeIndex const &timeIndex,
     171                 :            :                                                                           Situation const &situation,
     172                 :            :                                                                           state::RssState &rssState)
     173                 :            : {
     174   [ +  +  +  + ]:        805 :   if (situation.egoVehicleState.hasPriority && situation.otherVehicleState.hasPriority)
     175                 :            :   {
     176                 :            :     // both cannot have priority over the other at the same time
     177                 :          1 :     return false;
     178                 :            :   }
     179                 :        804 :   bool result = false;
     180                 :            :   try
     181                 :            :   {
     182         [ +  - ]:        804 :     if (timeIndex != mCurrentTimeIndex)
     183                 :            :     {
     184                 :            :       /**
     185                 :            :        * next time step: current safe state map becomes last state now
     186                 :            :        */
     187                 :        804 :       mLastSafeStateMap.swap(mNewSafeStateMap);
     188                 :        804 :       mNewSafeStateMap.clear();
     189                 :        804 :       mCurrentTimeIndex = timeIndex;
     190                 :            :     }
     191                 :            : 
     192                 :        804 :     rssState.situationId = situation.situationId;
     193                 :        804 :     rssState.situationType = situation.situationType;
     194                 :        804 :     rssState.objectId = situation.objectId;
     195                 :            : 
     196                 :        804 :     rssState.longitudinalState.isSafe = false;
     197                 :        804 :     rssState.longitudinalState.response = state::LongitudinalResponse::BrakeMin;
     198                 :        804 :     rssState.longitudinalState.alphaLon = situation.egoVehicleState.dynamics.alphaLon;
     199                 :            : 
     200                 :            :     /**
     201                 :            :      * An intersection situation is lateral unsafe but usually doesn't require a lateral brake
     202                 :            :      * @todo: if taking lateral intersection handling into account, this also has to be updated
     203                 :            :      */
     204                 :        804 :     rssState.lateralStateLeft.isSafe = false;
     205                 :        804 :     rssState.lateralStateLeft.response = state::LateralResponse::None;
     206                 :        804 :     rssState.lateralStateLeft.alphaLat = situation.egoVehicleState.dynamics.alphaLat;
     207                 :        804 :     rssState.lateralStateLeft.rssStateInformation.evaluator = state::RssStateEvaluator::LateralDistance;
     208                 :        804 :     rssState.lateralStateLeft.rssStateInformation.currentDistance = physics::Distance(0);
     209                 :        804 :     rssState.lateralStateLeft.rssStateInformation.safeDistance = physics::Distance(0);
     210                 :        804 :     rssState.lateralStateRight.isSafe = false;
     211                 :        804 :     rssState.lateralStateRight.response = state::LateralResponse::None;
     212                 :        804 :     rssState.lateralStateRight.alphaLat = situation.egoVehicleState.dynamics.alphaLat;
     213                 :        804 :     rssState.lateralStateRight.rssStateInformation.evaluator = state::RssStateEvaluator::LateralDistance;
     214                 :        804 :     rssState.lateralStateRight.rssStateInformation.currentDistance = physics::Distance(0);
     215                 :        804 :     rssState.lateralStateRight.rssStateInformation.safeDistance = physics::Distance(0);
     216                 :            : 
     217                 :        804 :     bool isSafe = false;
     218                 :        804 :     IntersectionState intersectionState = IntersectionState::NonPrioAbleToBreak;
     219                 :            : 
     220                 :            :     /**
     221                 :            :      * Check if the intersection is safe and determine the intersection state of the situation
     222                 :            :      */
     223                 :            :     result
     224         [ +  - ]:        804 :       = checkIntersectionSafe(situation, rssState.longitudinalState.rssStateInformation, isSafe, intersectionState);
     225                 :            : 
     226         [ +  + ]:        804 :     if (result)
     227                 :            :     {
     228                 :        802 :       rssState.longitudinalState.isSafe = isSafe;
     229                 :            : 
     230         [ +  - ]:        802 :       auto const previousIntersectionState = mLastSafeStateMap.find(situation.situationId);
     231                 :            : 
     232         [ +  + ]:        802 :       if (!isSafe)
     233                 :            :       {
     234                 :            :         /**
     235                 :            :          * Situation is unsafe determine proper response
     236                 :            :          */
     237         [ +  + ]:        240 :         if (previousIntersectionState != mLastSafeStateMap.end())
     238                 :            :         {
     239   [ +  +  +  - ]:        238 :           switch (previousIntersectionState->second)
     240                 :            :           {
     241                 :         77 :             case IntersectionState::NonPrioAbleToBreak:
     242                 :            :             {
     243         [ +  + ]:         77 :               if (situation.egoVehicleState.hasPriority)
     244                 :            :               {
     245                 :          1 :                 rssState.longitudinalState.response = state::LongitudinalResponse::None;
     246                 :            :               }
     247                 :         77 :               break;
     248                 :            :             }
     249                 :        160 :             case IntersectionState::SafeLongitudinalDistance:
     250                 :            :             {
     251         [ +  + ]:        160 :               if (situation.relativePosition.longitudinalPosition == LongitudinalRelativePosition::InFront)
     252                 :            :               {
     253                 :         52 :                 rssState.longitudinalState.response = state::LongitudinalResponse::None;
     254                 :            :               }
     255                 :        160 :               break;
     256                 :            :             }
     257                 :          1 :             case IntersectionState::NoTimeOverlap:
     258                 :            :             {
     259                 :            :               // @todo If we don't assume always lateral overlap we might need to brake laterally as well
     260                 :          1 :               rssState.longitudinalState.response = state::LongitudinalResponse::BrakeMin;
     261                 :          1 :               break;
     262                 :            :             }
     263                 :          0 :             default:
     264                 :            :             {
     265                 :            :               // LCOV_EXCL_START: unreachable code, keep to be on the safe side
     266                 :            :               result = false;
     267                 :            :               break;
     268                 :            :               // LCOV_EXCL_STOP: unreachable code, keep to be on the safe side
     269                 :            :             }
     270                 :            :           }
     271                 :            : 
     272                 :            :           /**
     273                 :            :            * Store the last safe intersection state for next time step
     274                 :            :            */
     275                 :            :           mNewSafeStateMap.insert(
     276         [ +  - ]:        238 :             RssIntersectionStateMap::value_type(situation.situationId, previousIntersectionState->second));
     277                 :            :         }
     278                 :            :         else
     279                 :            :         {
     280                 :            :           /**
     281                 :            :            * Situation is unsafe but there is no previous safe state. Therefore we can only decide to brake.
     282                 :            :            */
     283                 :          2 :           rssState.longitudinalState.response = state::LongitudinalResponse::BrakeMin;
     284                 :            :         }
     285                 :            :       }
     286                 :            :       else
     287                 :            :       {
     288                 :            :         /**
     289                 :            :          * Situation is safe
     290                 :            :          */
     291                 :        562 :         rssState.longitudinalState.response = state::LongitudinalResponse::None;
     292                 :            : 
     293                 :            :         // store the new safe state
     294         [ +  + ]:        562 :         mNewSafeStateMap.insert(RssIntersectionStateMap::value_type(situation.situationId, intersectionState));
     295                 :            :       }
     296                 :            :     }
     297                 :            :   }
     298                 :          3 :   catch (...)
     299                 :            :   {
     300                 :          3 :     result = false;
     301                 :            :   }
     302                 :        804 :   return result;
     303                 :            : }
     304                 :            : 
     305                 :            : } // namespace situation
     306                 :            : } // namespace rss
     307                 :            : } // namespace ad

Generated by: LCOV version 1.14