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 <map>
      15                 :            : #include "ad/rss/core/RelativeConstellation.hpp"
      16                 :            : #include "ad/rss/state/RssState.hpp"
      17                 :            : #include "ad/rss/world/TimeIndex.hpp"
      18                 :            : 
      19                 :            : /*!
      20                 :            :  * @brief namespace ad
      21                 :            :  */
      22                 :            : namespace ad {
      23                 :            : /*!
      24                 :            :  * @brief namespace rss
      25                 :            :  */
      26                 :            : namespace rss {
      27                 :            : /*!
      28                 :            :  * @brief namespace structured
      29                 :            :  */
      30                 :            : namespace structured {
      31                 :            : 
      32                 :            : /**
      33                 :            :  * @brief Class to check whether a non-intersection structured constellation is safe and to determine the proper
      34                 :            :  * response for
      35                 :            :  * the constellation
      36                 :            :  *
      37                 :            :  * Note: Implements the checks and responses given by the definitions 1-10 of the RSS paper (arXiv:1708.06374v6)
      38                 :            :  *
      39                 :            :  * Class performs required check to if constellation is safe
      40                 :            :  * Class will maintain the previous state of the constellation in order to provide the proper response.
      41                 :            :  */
      42                 :            : class RssNonIntersectionConstellationChecker
      43                 :            : {
      44                 :            : public:
      45                 :            :   /**
      46                 :            :    * @brief Constructor
      47                 :            :    */
      48                 :        799 :   RssNonIntersectionConstellationChecker() = default;
      49                 :            : 
      50                 :            :   /**
      51                 :            :    * @brief Destructor
      52                 :            :    */
      53                 :        799 :   ~RssNonIntersectionConstellationChecker() = default;
      54                 :            : 
      55                 :            :   /**
      56                 :            :    * @brief Calculate safety checks and determine required rssState for structured non intersection constellations
      57                 :            :    *
      58                 :            :    * @param[in]  time_index the time index of the constellation
      59                 :            :    * @param[in]  constellation constellation to analyze
      60                 :            :    * @param[out] rssState  rssState of the ego vehicle
      61                 :            :    *
      62                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      63                 :            :    */
      64                 :            :   bool calculateRssStateNonIntersection(world::TimeIndex const &time_index,
      65                 :            :                                         core::RelativeConstellation const &constellation,
      66                 :            :                                         state::RssState &rssState);
      67                 :            : 
      68                 :            : private:
      69                 :            :   /**
      70                 :            :    * @brief Calculate safety checks and determine required rssState for non intersection same direction scenario
      71                 :            :    *
      72                 :            :    * @param[in]  time_index the time index of the constellation
      73                 :            :    * @param[in]  constellation constellation to analyze
      74                 :            :    * @param[out] rssState  response state of the ego vehicle
      75                 :            :    *
      76                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      77                 :            :    */
      78                 :            :   bool calculateRssStateSameDirection(core::RelativeConstellation const &constellation, state::RssState &rssState);
      79                 :            : 
      80                 :            :   /**
      81                 :            :    * @brief Calculate safety checks and determine required rssState for non intersection opposite direction scenario
      82                 :            :    *
      83                 :            :    * @param[in]  time_index the time index of the constellation
      84                 :            :    * @param[in]  constellation constellation to analyze
      85                 :            :    * @param[out] rssState  response state of the ego vehicle
      86                 :            :    *
      87                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      88                 :            :    */
      89                 :            :   bool calculateRssStateOppositeDirection(core::RelativeConstellation const &constellation, state::RssState &rssState);
      90                 :            : 
      91                 :            :   /**
      92                 :            :    * @brief Calculate safety checks and determine required rssState for longitudinal direction for
      93                 :            :    * non intersection scenario when both vehicles are driving in same direction
      94                 :            :    *
      95                 :            :    * @param[in]  constellation constellation to analyze
      96                 :            :    * @param[out] rssState  rssState of the ego vehicle
      97                 :            :    *
      98                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      99                 :            :    *
     100                 :            :    */
     101                 :            :   bool calculateLongitudinalRssStateSameDirection(core::RelativeConstellation const &constellation,
     102                 :            :                                                   state::LongitudinalRssState &rssState);
     103                 :            : 
     104                 :            :   /**
     105                 :            :    * @brief Calculate safety checks and determine required rssState for longitudinal direction for
     106                 :            :    * non intersection scenario when vehicles are driving in opposite direction
     107                 :            :    *
     108                 :            :    * @param[in]  constellation constellation to analyze
     109                 :            :    * @param[out] rssState  rssState of the ego vehicle
     110                 :            :    *
     111                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     112                 :            :    *
     113                 :            :    */
     114                 :            :   bool calculateLongitudinalRssStateOppositeDirection(core::RelativeConstellation const &constellation,
     115                 :            :                                                       state::LongitudinalRssState &rssState);
     116                 :            : 
     117                 :            :   /**
     118                 :            :    * @brief Calculate safety checks and determine required rssState for lateral direction
     119                 :            :    *
     120                 :            :    * @param[in] constellation      constellation to analyze
     121                 :            :    * @param[out] rssStateLeft  rssState of the ego vehicle at its left side
     122                 :            :    * @param[out] rssStateRight rssState of the ego vehicle at its right side
     123                 :            :    *
     124                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     125                 :            :    *
     126                 :            :    */
     127                 :            :   bool calculateLateralRssState(core::RelativeConstellation const &constellation,
     128                 :            :                                 state::LateralRssState &rssStateLeft,
     129                 :            :                                 state::LateralRssState &rssStateRight);
     130                 :            : 
     131                 :            :   struct RssSafeState
     132                 :            :   {
     133                 :            :     bool longitudinalSafe{false};
     134                 :            :     bool lateralSafe{false};
     135                 :            :   };
     136                 :            : 
     137                 :            :   /**
     138                 :            :    * @brief typedef for the mapping of object id to the corresponding RssSafeState before the danger threshold time
     139                 :            :    */
     140                 :            :   typedef std::map<core::RelativeConstellationId, RssSafeState> RssSafeStateBeforeDangerThresholdTimeMap;
     141                 :            : 
     142                 :            :   /**
     143                 :            :    * @brief the state of each constellation before the danger threshold time of last timestep
     144                 :            :    *
     145                 :            :    * Needs to be stored to check which is the proper response required to solve an unclear constellation
     146                 :            :    */
     147                 :            :   RssSafeStateBeforeDangerThresholdTimeMap mLastStatesBeforeDangerThresholdTime;
     148                 :            :   /**
     149                 :            :    * @brief the new state of each constellation before the danger threshold time of current timestep
     150                 :            :    *
     151                 :            :    */
     152                 :            :   RssSafeStateBeforeDangerThresholdTimeMap mNewStatesBeforeDangerThresholdTime;
     153                 :            : 
     154                 :            :   /**
     155                 :            :    * @brief time index of the current processing step
     156                 :            :    * If time index increases we need to update the state maps
     157                 :            :    */
     158                 :            :   world::TimeIndex mCurrentTimeIndex{0u};
     159                 :            : };
     160                 :            : 
     161                 :            : } // namespace structured
     162                 :            : } // namespace rss
     163                 :            : } // namespace ad