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/RssResponseResolving.hpp"
      10                 :            : #include <algorithm>
      11                 :            : #include "ad/rss/state/RssStateOperation.hpp"
      12                 :            : #include "ad/rss/state/RssStateSnapshotValidInputRange.hpp"
      13                 :            : #include "ad/rss/unstructured/Geometry.hpp"
      14                 :            : #include "spdlog/fmt/ostr.h"
      15                 :            : #include "spdlog/spdlog.h"
      16                 :            : 
      17                 :            : namespace ad {
      18                 :            : namespace rss {
      19                 :            : namespace core {
      20                 :            : 
      21                 :        735 : RssResponseResolving::RssResponseResolving()
      22                 :            : {
      23                 :        735 : }
      24                 :            : 
      25                 :       3867 : bool RssResponseResolving::provideProperResponse(state::RssStateSnapshot const &currentStateSnapshot,
      26                 :            :                                                  state::ProperResponse &response)
      27                 :            : {
      28         [ +  + ]:       3867 :   if (!withinValidInputRange(currentStateSnapshot))
      29                 :            :   {
      30                 :          1 :     spdlog::error("RssResponseResolving::provideProperResponse>> Invalid input");
      31                 :          1 :     return false;
      32                 :            :   }
      33                 :            : 
      34                 :       3866 :   bool result = true;
      35                 :            :   // global try catch block to ensure this library call doesn't throw an exception
      36                 :            :   try
      37                 :            :   {
      38                 :       3866 :     response.timeIndex = currentStateSnapshot.timeIndex;
      39                 :       3866 :     response.isSafe = true;
      40                 :       3866 :     response.dangerousObjects.clear();
      41                 :       3866 :     response.longitudinalResponse = state::LongitudinalResponse::None;
      42                 :       3866 :     response.lateralResponseLeft = state::LateralResponse::None;
      43                 :       3866 :     response.lateralResponseRight = state::LateralResponse::None;
      44                 :       3866 :     response.unstructuredSceneResponse = state::UnstructuredSceneResponse::None;
      45                 :            : 
      46                 :            :     // absolute maxima are given by the default dynamics
      47                 :            :     response.accelerationRestrictions.longitudinalRange.maximum
      48                 :       3866 :       = currentStateSnapshot.defaultEgoVehicleRssDynamics.alphaLon.accelMax;
      49                 :            :     response.accelerationRestrictions.lateralLeftRange.maximum
      50                 :       3866 :       = currentStateSnapshot.defaultEgoVehicleRssDynamics.alphaLat.accelMax;
      51                 :            :     response.accelerationRestrictions.lateralRightRange.maximum
      52                 :       3866 :       = currentStateSnapshot.defaultEgoVehicleRssDynamics.alphaLat.accelMax;
      53                 :            : 
      54                 :            :     // absolute minimum in longitudinal direction is given by brakeMax
      55                 :            :     response.accelerationRestrictions.longitudinalRange.minimum
      56                 :       3866 :       = currentStateSnapshot.defaultEgoVehicleRssDynamics.alphaLon.brakeMax;
      57                 :            : 
      58                 :            :     // in lateral dimension, this is handled differently
      59                 :            :     // because of the explicit split into left and right direction
      60                 :            :     //
      61                 :            :     // lowest() value used for minimum because, if the vehicle is driving to the left
      62                 :            :     // it can decelerate the movement to the left in an unbounded manner.
      63                 :            :     // At the point the vehicle direction is turning, the right restrictions are becoming valid.
      64                 :            :     //
      65                 :            :     // @todo: ideally we should try to come to a closed description in here, so that
      66                 :            :     // we only have ONE lateral acceleration range combining the whole result
      67                 :            :     // This will make it easier outside
      68                 :            :     // But: in this case we have to ensure that the orientation of the situation
      69                 :            :     // is respected accordingly, as especially within intersections this might differ depending on the driven route!
      70                 :            :     // Furthermore, we might need to have some knowledge on the expected cycle time,
      71                 :            :     // to be able to restrict counter-steering at the right point in time BEFORE the turn over
      72                 :            :     // the minimum therefore, would then change dynamically while counter steering...
      73                 :            :     //
      74                 :            :     // Currently the final transformation back has to be performed outside which requires
      75                 :            :     // deeper understanding there; so the above is more or less up to the system integrator
      76                 :            :     // to calculate while transforming back from vehicle coordinates to control data
      77                 :            :     //
      78                 :            :     // => Would make it much easier to integrate in the outside if we can solve this internally in
      79                 :            :     // a robust and correct manner
      80                 :       3866 :     response.accelerationRestrictions.lateralLeftRange.minimum = std::numeric_limits<physics::Acceleration>::lowest();
      81                 :       3866 :     response.accelerationRestrictions.lateralRightRange.minimum = std::numeric_limits<physics::Acceleration>::lowest();
      82                 :       3866 :     response.headingRanges.clear();
      83                 :       3866 :     physics::Acceleration driveAwayBrakeMin = currentStateSnapshot.defaultEgoVehicleRssDynamics.alphaLon.accelMax;
      84                 :       3866 :     bool unstructuredDriveAwayToBrakeTransitionOccured = false;
      85                 :            : 
      86         [ +  + ]:       8435 :     for (auto const &currentState : currentStateSnapshot.individualResponses)
      87                 :            :     {
      88         [ +  + ]:       4569 :       if (isDangerous(currentState))
      89                 :            :       {
      90                 :       1516 :         response.isSafe = false;
      91         [ +  - ]:       1516 :         if (std::find(response.dangerousObjects.begin(), response.dangerousObjects.end(), currentState.objectId)
      92         [ +  - ]:       3032 :             == response.dangerousObjects.end())
      93                 :            :         {
      94         [ +  - ]:       1516 :           response.dangerousObjects.push_back(currentState.objectId);
      95                 :            :         }
      96                 :            : 
      97         [ -  + ]:       1516 :         if (currentState.situationType == situation::SituationType::Unstructured)
      98                 :            :         {
      99         [ #  # ]:          0 :           spdlog::info("RssResponseResolving::provideProperResponse>> Unstructured state is dangerous: {}",
     100                 :            :                        currentState);
     101                 :          0 :           combineState(currentState.unstructuredSceneState,
     102                 :            :                        driveAwayBrakeMin,
     103                 :            :                        unstructuredDriveAwayToBrakeTransitionOccured,
     104                 :          0 :                        response.unstructuredSceneResponse,
     105                 :          0 :                        response.headingRanges,
     106         [ #  # ]:          0 :                        response.accelerationRestrictions.longitudinalRange);
     107                 :            :         }
     108                 :            :         else // structured
     109                 :            :         {
     110         [ +  - ]:       1516 :           spdlog::info("RssResponseResolving::provideProperResponse>> Structured state is dangerous: {}", currentState);
     111                 :            : 
     112                 :       1516 :           combineState(currentState.longitudinalState,
     113                 :       1516 :                        response.longitudinalResponse,
     114         [ +  - ]:       1516 :                        response.accelerationRestrictions.longitudinalRange);
     115                 :            : 
     116                 :            :           // we might need to check here if left or right is the dangerous side
     117                 :            :           // but for the combineLateralResponse will only respect the more severe response
     118                 :            :           // omitting the check should have the same result
     119                 :       1516 :           combineState(currentState.lateralStateLeft,
     120                 :       1516 :                        response.lateralResponseLeft,
     121         [ +  - ]:       1516 :                        response.accelerationRestrictions.lateralLeftRange);
     122                 :            : 
     123                 :       1516 :           combineState(currentState.lateralStateRight,
     124                 :       1516 :                        response.lateralResponseRight,
     125         [ +  - ]:       1516 :                        response.accelerationRestrictions.lateralRightRange);
     126                 :            :         }
     127                 :            :       }
     128                 :            :     }
     129                 :            : 
     130         [ -  + ]:       3866 :     if (unstructuredDriveAwayToBrakeTransitionOccured
     131         [ #  # ]:          0 :         && (response.unstructuredSceneResponse == state::UnstructuredSceneResponse::DriveAway))
     132                 :            :     {
     133                 :          0 :       response.unstructuredSceneResponse = state::UnstructuredSceneResponse::Brake;
     134                 :            :       response.accelerationRestrictions.longitudinalRange.maximum
     135         [ #  # ]:          0 :         = std::min(response.accelerationRestrictions.longitudinalRange.maximum, driveAwayBrakeMin);
     136                 :            :     }
     137                 :            :   }
     138                 :          0 :   catch (std::exception &e)
     139                 :            :   {
     140         [ -  - ]:          0 :     spdlog::critical(
     141                 :          0 :       "RssResponseResolving::provideProperResponse>> Exception caught'{}' {}", e.what(), currentStateSnapshot);
     142                 :          0 :     result = false;
     143                 :            :   }
     144                 :          0 :   catch (...)
     145                 :            :   {
     146         [ -  - ]:          0 :     spdlog::critical("RssResponseResolving::provideProperResponse>> Exception caught {}", currentStateSnapshot);
     147                 :          0 :     result = false;
     148                 :            :   }
     149                 :            : 
     150                 :       3866 :   return result;
     151                 :            : }
     152                 :            : 
     153                 :            : /**
     154                 :            :  * @brief determine the resulting RSS response
     155                 :            :  *
     156                 :            :  * @param[in] previousResponse the previous RSS response
     157                 :            :  * @param[in] newResponse      the RSS response to be considered in addition
     158                 :            :  *
     159                 :            :  * The RSS responses are combined in a form that the most severe response of both becomes the resulting response.
     160                 :            :  * The responses are compared with each other based on the enumeration values.
     161                 :            :  * Therefore, these values need have to be ordered strictly ascending in respect to their severity.
     162                 :            :  *
     163                 :            :  * @returns the resulting RSS response
     164                 :            :  */
     165                 :       4548 : template <typename Response> Response combineResponse(Response const &previousResponse, Response const &newResponse)
     166                 :            : {
     167         [ +  + ]:       4548 :   if (previousResponse > newResponse)
     168                 :            :   {
     169                 :       1302 :     return previousResponse;
     170                 :            :   }
     171                 :       3246 :   return newResponse;
     172                 :            : }
     173                 :            : 
     174                 :          0 : void RssResponseResolving::combineState(state::UnstructuredSceneRssState const &state,
     175                 :            :                                         physics::Acceleration &driveAwayBrakeMin,
     176                 :            :                                         bool &driveAwayToBrakeTransition,
     177                 :            :                                         state::UnstructuredSceneResponse &response,
     178                 :            :                                         state::HeadingRangeVector &responseHeadingRanges,
     179                 :            :                                         physics::AccelerationRange &accelerationRange)
     180                 :            : {
     181         [ #  # ]:          0 :   if ((response != state::UnstructuredSceneResponse::Brake)
     182         [ #  # ]:          0 :       && (state.response == state::UnstructuredSceneResponse::DriveAway))
     183                 :            :   {
     184                 :          0 :     driveAwayBrakeMin = std::min(driveAwayBrakeMin, state.alphaLon.brakeMin);
     185         [ #  # ]:          0 :     if (!driveAwayToBrakeTransition)
     186                 :            :     {
     187                 :          0 :       auto const overlapAvailable = unstructured::getHeadingOverlap(state.headingRange, responseHeadingRanges);
     188         [ #  # ]:          0 :       if (!overlapAvailable)
     189                 :            :       {
     190                 :          0 :         driveAwayToBrakeTransition = true;
     191                 :            :       }
     192                 :            :     }
     193                 :            :   }
     194                 :            : 
     195         [ #  # ]:          0 :   if (state.response > response)
     196                 :            :   {
     197                 :          0 :     response = state.response;
     198                 :            :   }
     199                 :            : 
     200                 :            :   // LCOV_EXCL_BR_START: unreachable exceptions due to valid input range checks
     201                 :          0 :   accelerationRange.minimum = std::max(accelerationRange.minimum, state.alphaLon.brakeMax);
     202                 :          0 :   if (state.response == state::UnstructuredSceneResponse::Brake)
     203                 :            :   {
     204                 :          0 :     responseHeadingRanges.clear();
     205                 :          0 :     accelerationRange.maximum = std::min(accelerationRange.maximum, state.alphaLon.brakeMin);
     206                 :            :   }
     207                 :            :   else
     208                 :            :   {
     209                 :          0 :     accelerationRange.maximum = std::min(accelerationRange.maximum, state.alphaLon.accelMax);
     210                 :            :   }
     211                 :            :   // LCOV_EXCL_BR_STOP
     212                 :          0 : }
     213                 :            : 
     214                 :       1516 : void RssResponseResolving::combineState(state::LongitudinalRssState const &state,
     215                 :            :                                         state::LongitudinalResponse &response,
     216                 :            :                                         physics::AccelerationRange &accelerationRange)
     217                 :            : {
     218                 :       1516 :   response = combineResponse(state.response, response);
     219                 :            : 
     220                 :            :   // LCOV_EXCL_BR_START: unreachable exceptions due to valid input range checks
     221                 :       1516 :   accelerationRange.minimum = std::max(accelerationRange.minimum, state.alphaLon.brakeMax);
     222                 :       1516 :   switch (state.response)
     223                 :            :   {
     224                 :        645 :     case state::LongitudinalResponse::BrakeMin:
     225                 :        645 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alphaLon.brakeMin);
     226                 :        645 :       break;
     227                 :        399 :     case state::LongitudinalResponse::BrakeMinCorrect:
     228                 :        399 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alphaLon.brakeMinCorrect);
     229                 :        399 :       break;
     230                 :        472 :     case state::LongitudinalResponse::None:
     231                 :        472 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alphaLon.accelMax);
     232                 :        472 :       break;
     233                 :          0 :     default:
     234                 :          0 :       spdlog::error("RssResponseTransformation::updateAccelerationRestriction>> Invalid longitudinal response {}",
     235                 :          0 :                     state.response);
     236                 :            :       // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     237                 :          0 :       break;
     238                 :            :   }
     239                 :            :   // LCOV_EXCL_BR_STOP
     240                 :       1516 : }
     241                 :            : 
     242                 :       3032 : void RssResponseResolving::combineState(state::LateralRssState const &state,
     243                 :            :                                         state::LateralResponse &response,
     244                 :            :                                         physics::AccelerationRange &accelerationRange)
     245                 :            : {
     246                 :       3032 :   response = combineResponse(state.response, response);
     247                 :            : 
     248                 :            :   // LCOV_EXCL_BR_START: unreachable exceptions due to valid input range checks
     249                 :       3032 :   accelerationRange.minimum = std::numeric_limits<physics::Acceleration>::lowest();
     250                 :       3032 :   switch (state.response)
     251                 :            :   {
     252                 :        258 :     case state::LateralResponse::BrakeMin:
     253                 :        258 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alphaLat.brakeMin);
     254                 :        258 :       break;
     255                 :       2774 :     case state::LateralResponse::None:
     256                 :       2774 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alphaLat.accelMax);
     257                 :       2774 :       break;
     258                 :          0 :     default:
     259                 :          0 :       spdlog::error("RssResponseTransformation::updateAccelerationRestriction>> Invalid lateral response {}",
     260                 :          0 :                     state.response);
     261                 :            :       // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     262                 :          0 :       break;
     263                 :            :   }
     264                 :            :   // LCOV_EXCL_BR_STOP
     265                 :       3032 : }
     266                 :            : 
     267                 :            : } // namespace core
     268                 :            : } // namespace rss
     269                 :            : } // namespace ad