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/state/RssStateOperation.hpp"
      10                 :            : 
      11                 :            : #include <ad/geometry/GeometryOperation.hpp>
      12                 :            : #include <ad/physics/RangeOperation.hpp>
      13                 :            : 
      14                 :            : #include "ad/rss/core/Logging.hpp"
      15                 :            : 
      16                 :            : namespace ad {
      17                 :            : namespace rss {
      18                 :            : namespace state {
      19                 :            : 
      20                 :            : /**
      21                 :            :  * @brief determine the resulting RSS response
      22                 :            :  *
      23                 :            :  * @param[in] previousResponse the previous RSS response
      24                 :            :  * @param[in] newResponse      the RSS response to be considered in addition
      25                 :            :  *
      26                 :            :  * The RSS responses are combined in a form that the most severe response of both becomes the resulting response.
      27                 :            :  * The responses are compared with each other based on the enumeration values.
      28                 :            :  * Therefore, these values need have to be ordered strictly ascending in respect to their severity.
      29                 :            :  *
      30                 :            :  * @returns the resulting RSS response
      31                 :            :  */
      32                 :       4581 : template <typename Response> Response combineResponse(Response const &previousResponse, Response const &newResponse)
      33                 :            : {
      34         [ +  + ]:       4581 :   if (previousResponse > newResponse)
      35                 :            :   {
      36                 :       1308 :     return previousResponse;
      37                 :            :   }
      38                 :       3273 :   return newResponse;
      39                 :            : }
      40                 :            : 
      41                 :          2 : void combineState(state::UnstructuredConstellationRssState const &state,
      42                 :            :                   physics::Acceleration &driveAwayBrakeMin,
      43                 :            :                   bool &driveAwayToBrakeTransition,
      44                 :            :                   state::UnstructuredConstellationResponse &response,
      45                 :            :                   ::ad::geometry::HeadingRangeVector &responseHeadingRanges,
      46                 :            :                   physics::AccelerationRange &accelerationRange)
      47                 :            : {
      48         [ +  - ]:          2 :   if ((response != state::UnstructuredConstellationResponse::Brake)
      49         [ -  + ]:          2 :       && (state.response == state::UnstructuredConstellationResponse::DriveAway))
      50                 :            :   {
      51                 :          0 :     driveAwayBrakeMin = std::min(driveAwayBrakeMin, state.alpha_lon.brake_min);
      52         [ #  # ]:          0 :     if (!driveAwayToBrakeTransition)
      53                 :            :     {
      54                 :          0 :       auto const overlapAvailable = ::ad::geometry::getHeadingOverlap(state.heading_range, responseHeadingRanges);
      55         [ #  # ]:          0 :       if (!overlapAvailable)
      56                 :            :       {
      57                 :          0 :         driveAwayToBrakeTransition = true;
      58                 :            :       }
      59                 :            :     }
      60                 :            :   }
      61                 :            : 
      62         [ +  - ]:          2 :   if (state.response > response)
      63                 :            :   {
      64                 :          2 :     response = state.response;
      65                 :            :   }
      66                 :            : 
      67                 :            :   // LCOV_EXCL_BR_START: unreachable exceptions due to valid input range checks
      68                 :          2 :   accelerationRange.minimum = std::max(accelerationRange.minimum, state.alpha_lon.brake_max);
      69                 :          2 :   if (state.response == state::UnstructuredConstellationResponse::Brake)
      70                 :            :   {
      71                 :          2 :     responseHeadingRanges.clear();
      72                 :          2 :     accelerationRange.maximum = std::min(accelerationRange.maximum, state.alpha_lon.brake_min);
      73                 :            :   }
      74                 :          0 :   else if (state.response == state::UnstructuredConstellationResponse::DriveAway)
      75                 :            :   {
      76                 :          0 :     bool is_inside_heading_range = ::ad::geometry::isInsideHeadingRange(
      77                 :          0 :       state.rss_state_information.considered_drive_away_steering_angle, state.heading_range);
      78                 :          0 :     if (is_inside_heading_range)
      79                 :            :     {
      80                 :          0 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alpha_lon.accel_max);
      81                 :            :     }
      82                 :            :     else
      83                 :            :     {
      84                 :            :       // brake as long as the steering angle is not within the heading range
      85                 :          0 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alpha_lon.brake_min);
      86                 :            :     }
      87                 :            :   }
      88                 :            :   else
      89                 :            :   {
      90                 :          0 :     accelerationRange.maximum = std::min(accelerationRange.maximum, state.alpha_lon.accel_max);
      91                 :            :   }
      92                 :            :   // LCOV_EXCL_BR_STOP
      93                 :          2 : }
      94                 :            : 
      95                 :       1527 : void combineState(state::LongitudinalRssState const &state,
      96                 :            :                   state::LongitudinalResponse &response,
      97                 :            :                   physics::AccelerationRange &accelerationRange)
      98                 :            : {
      99                 :       1527 :   response = combineResponse(state.response, response);
     100                 :            : 
     101                 :            :   // LCOV_EXCL_BR_START: unreachable exceptions due to valid input range checks
     102                 :       1527 :   accelerationRange.minimum = std::max(accelerationRange.minimum, state.alpha_lon.brake_max);
     103                 :       1527 :   switch (state.response)
     104                 :            :   {
     105                 :        652 :     case state::LongitudinalResponse::BrakeMin:
     106                 :        652 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alpha_lon.brake_min);
     107                 :        652 :       break;
     108                 :        399 :     case state::LongitudinalResponse::BrakeMinCorrect:
     109                 :        399 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alpha_lon.brake_min_correct);
     110                 :        399 :       break;
     111                 :        476 :     case state::LongitudinalResponse::None:
     112                 :        476 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alpha_lon.accel_max);
     113                 :        476 :       break;
     114                 :          0 :     default:
     115                 :          0 :       core::getLogger()->error(
     116                 :          0 :         "RssResponseTransformation::updateAccelerationRestriction>> Invalid longitudinal response {}", state.response);
     117                 :            :       // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     118                 :          0 :       break;
     119                 :            :   }
     120                 :            :   // LCOV_EXCL_BR_STOP
     121                 :       1527 : }
     122                 :            : 
     123                 :       3054 : void combineState(state::LateralRssState const &state,
     124                 :            :                   state::LateralResponse &response,
     125                 :            :                   physics::AccelerationRange &accelerationRange,
     126                 :            :                   bool const isLaneBoundariesObject)
     127                 :            : {
     128                 :       3054 :   response = combineResponse(state.response, response);
     129                 :            : 
     130                 :            :   // LCOV_EXCL_BR_START: unreachable exceptions due to valid input range checks
     131                 :       3054 :   accelerationRange.minimum = std::numeric_limits<physics::Acceleration>::lowest();
     132                 :       3054 :   switch (state.response)
     133                 :            :   {
     134                 :        257 :     case state::LateralResponse::BrakeMin:
     135                 :        257 :       accelerationRange.maximum = std::min(accelerationRange.maximum, state.alpha_lat.brake_min);
     136                 :        257 :       break;
     137                 :       2797 :     case state::LateralResponse::None:
     138                 :       2797 :       if (!isLaneBoundariesObject)
     139                 :            :       {
     140                 :            :         // lane boundaries are soft borders into one single direction, so don't limit the nominal driving
     141                 :       2797 :         accelerationRange.maximum = std::min(accelerationRange.maximum, state.alpha_lat.accel_max);
     142                 :            :       }
     143                 :       2797 :       break;
     144                 :          0 :     default:
     145                 :          0 :       core::getLogger()->error("RssResponseTransformation::updateAccelerationRestriction>> Invalid lateral response {}",
     146                 :          0 :                                state.response);
     147                 :            :       // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     148                 :          0 :       break;
     149                 :            :   }
     150                 :            :   // LCOV_EXCL_BR_STOP
     151                 :       3054 : }
     152                 :            : 
     153                 :          0 : state::ProperResponse combineProperResponse(state::ProperResponse const &left, state::ProperResponse const &right)
     154                 :            : {
     155                 :          0 :   state::ProperResponse combinedProperResponse;
     156                 :          0 :   combinedProperResponse.time_index = std::max(left.time_index, right.time_index);
     157   [ #  #  #  # ]:          0 :   combinedProperResponse.is_safe = left.is_safe && right.is_safe;
     158         [ #  # ]:          0 :   combinedProperResponse.dangerous_objects.reserve(left.dangerous_objects.size() + right.dangerous_objects.size());
     159         [ #  # ]:          0 :   combinedProperResponse.dangerous_objects.insert(
     160                 :          0 :     combinedProperResponse.dangerous_objects.end(), left.dangerous_objects.begin(), left.dangerous_objects.end());
     161         [ #  # ]:          0 :   combinedProperResponse.dangerous_objects.insert(
     162                 :          0 :     combinedProperResponse.dangerous_objects.end(), right.dangerous_objects.begin(), right.dangerous_objects.end());
     163                 :            :   combinedProperResponse.longitudinal_response
     164                 :          0 :     = combineResponse(left.longitudinal_response, right.longitudinal_response);
     165                 :            :   combinedProperResponse.lateral_response_right
     166                 :          0 :     = combineResponse(left.lateral_response_right, right.lateral_response_right);
     167                 :            :   combinedProperResponse.lateral_response_left
     168                 :          0 :     = combineResponse(left.lateral_response_left, right.lateral_response_left);
     169                 :            :   combinedProperResponse.unstructured_constellation_response
     170                 :          0 :     = combineResponse(left.unstructured_constellation_response, right.unstructured_constellation_response);
     171                 :          0 :   combinedProperResponse.acceleration_restrictions.lateral_left_range = ad::physics::getIntersectionRange(
     172         [ #  # ]:          0 :     left.acceleration_restrictions.lateral_left_range, right.acceleration_restrictions.lateral_left_range);
     173                 :          0 :   combinedProperResponse.acceleration_restrictions.lateral_right_range = ad::physics::getIntersectionRange(
     174         [ #  # ]:          0 :     left.acceleration_restrictions.lateral_right_range, right.acceleration_restrictions.lateral_right_range);
     175                 :          0 :   combinedProperResponse.acceleration_restrictions.longitudinal_range = ad::physics::getIntersectionRange(
     176         [ #  # ]:          0 :     left.acceleration_restrictions.longitudinal_range, right.acceleration_restrictions.longitudinal_range);
     177                 :          0 :   return combinedProperResponse;
     178                 :          0 : }
     179                 :            : 
     180                 :            : } // namespace state
     181                 :            : } // namespace rss
     182                 :            : } // namespace ad