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                 :            :  * @file
      10                 :            :  */
      11                 :            : 
      12                 :            : #pragma once
      13                 :            : 
      14                 :            : #include "ad/rss/state/ProperResponse.hpp"
      15                 :            : #include "ad/rss/state/RssState.hpp"
      16                 :            : 
      17                 :            : /*!
      18                 :            :  * @brief namespace ad
      19                 :            :  */
      20                 :            : namespace ad {
      21                 :            : /*!
      22                 :            :  * @brief namespace rss
      23                 :            :  */
      24                 :            : namespace rss {
      25                 :            : /*!
      26                 :            :  * @brief namespace for RSS state datatypes and operations
      27                 :            :  */
      28                 :            : namespace state {
      29                 :            : 
      30                 :            : /**
      31                 :            :  * @brief is rss state longitudinal safe
      32                 :            :  *
      33                 :            :  * @param[in] rssState to check
      34                 :            :  *
      35                 :            :  * true if safe, false if not
      36                 :            :  */
      37                 :     189740 : inline bool isLongitudinalSafe(RssState const &rssState)
      38                 :            : {
      39                 :     189740 :   return rssState.longitudinal_state.is_safe;
      40                 :            : }
      41                 :            : 
      42                 :            : /**
      43                 :            :  * @brief is rss state lateral safe
      44                 :            :  *
      45                 :            :  * @param[in] rssState to check
      46                 :            :  *
      47                 :            :  * true if safe, false if not
      48                 :            :  */
      49                 :      84018 : inline bool isLateralSafe(RssState const &rssState)
      50                 :            : {
      51   [ +  +  +  + ]:      84018 :   return rssState.lateral_state_right.is_safe && rssState.lateral_state_left.is_safe;
      52                 :            : }
      53                 :            : 
      54                 :            : /**
      55                 :            :  * @brief is rss unstructured constellation state safe
      56                 :            :  *
      57                 :            :  * @param[in] rssState to check
      58                 :            :  *
      59                 :            :  * true if safe, false if not
      60                 :            :  */
      61                 :          0 : inline bool isUnstructuredConstellationSafe(RssState const &rssState)
      62                 :            : {
      63                 :          0 :   return rssState.unstructured_constellation_state.is_safe;
      64                 :            : }
      65                 :            : 
      66                 :            : /**
      67                 :            :  * @brief is rss state dangerous
      68                 :            :  *
      69                 :            :  * @param[in] rssState to check
      70                 :            :  *
      71                 :            :  * true if dangerous, false if not
      72                 :            :  */
      73                 :     136873 : inline bool isDangerous(RssState const &rssState)
      74                 :            : {
      75         [ -  + ]:     136873 :   if (rssState.constellation_type == ad::rss::world::ConstellationType::Unstructured)
      76                 :            :   {
      77                 :          0 :     return !isUnstructuredConstellationSafe(rssState);
      78                 :            :   }
      79                 :            :   else
      80                 :            :   {
      81   [ +  +  +  + ]:     136873 :     return !isLongitudinalSafe(rssState) && !isLateralSafe(rssState);
      82                 :            :   }
      83                 :            : }
      84                 :            : 
      85                 :            : /*!
      86                 :            :  * @brief combineState and update AccelerationRestriction
      87                 :            :  *
      88                 :            :  * Updates the longitudinal accelerationRestriction
      89                 :            :  *
      90                 :            :  * @param[in] state - The unstructured constellation state to update the acceleration restrictions with
      91                 :            :  *  @param[inout] driveAwayBrakeMin -- the minimal brake_min of all drive-away states
      92                 :            :  *  @param[inout] driveAwayToBrakeTransition -- a transition from drive-Away to Brake happened
      93                 :            :  *  @param[inout] response -- the combined RSS response to become RSS safe.
      94                 :            :  *  @param[inout] responseHeadingRanges -- the combined RSS response heading ranges (for driving away)
      95                 :            :  * @param[inout] accelerationRange - The combined restrictions on the vehicle acceleration to become RSS safe.
      96                 :            :  *
      97                 :            :  */
      98                 :            : void combineState(state::UnstructuredConstellationRssState const &state,
      99                 :            :                   physics::Acceleration &driveAwayBrakeMin,
     100                 :            :                   bool &driveAwayToBrakeTransition,
     101                 :            :                   state::UnstructuredConstellationResponse &response,
     102                 :            :                   ::ad::geometry::HeadingRangeVector &responseHeadingRanges,
     103                 :            :                   physics::AccelerationRange &accelerationRange);
     104                 :            : 
     105                 :            : /*!
     106                 :            :  * @brief combineState and update AccelerationRestriction
     107                 :            :  *
     108                 :            :  * Updates the longitudinal accelerationRestriction
     109                 :            :  *
     110                 :            :  * @param[in] state - The longitudinal state to update the acceleration restrictions with
     111                 :            :  * @param[inout] response -- the combined RSS response to become RSS safe.
     112                 :            :  * @param[inout] accelerationRange - The combined restrictions on the vehicle acceleration to become RSS safe.
     113                 :            :  *
     114                 :            :  */
     115                 :            : void combineState(state::LongitudinalRssState const &state,
     116                 :            :                   state::LongitudinalResponse &response,
     117                 :            :                   physics::AccelerationRange &accelerationRange);
     118                 :            : 
     119                 :            : /*!
     120                 :            :  * @brief combineState and update AccelerationRestriction
     121                 :            :  *
     122                 :            :  * Updates the lateral acceleration_restrictions
     123                 :            :  *
     124                 :            :  * @param[in] state - The lateral state to update the acceleration restrictions with
     125                 :            :  * @param[inout] response -- the combined RSS response to become RSS safe.
     126                 :            :  * @param[inout] accelerationRange - The restrictions on the vehicle acceleration to become RSS safe.
     127                 :            :  * @param[in] isLaneBoundariesObject is the object a lane boundaries object (which is ignored if none response)
     128                 :            :  *
     129                 :            :  */
     130                 :            : void combineState(state::LateralRssState const &state,
     131                 :            :                   state::LateralResponse &response,
     132                 :            :                   physics::AccelerationRange &accelerationRange,
     133                 :            :                   bool const isLaneBoundariesObject);
     134                 :            : 
     135                 :            : /*!
     136                 :            :  * @brief Combine two proper responses
     137                 :            :  *
     138                 :            :  * @param[in] left -- a RSS response to become RSS safe.
     139                 :            :  * @param[in] right -- another RSS response to become RSS safe.
     140                 :            :  *
     141                 :            :  * @returns The combined proper response considering the worst-case
     142                 :            :  */
     143                 :            : state::ProperResponse combineProperResponse(state::ProperResponse const &left, state::ProperResponse const &right);
     144                 :            : 
     145                 :            : } // namespace state
     146                 :            : } // namespace rss
     147                 :            : } // namespace ad