Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2020-2022 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : /**
      10                 :            :  * @file
      11                 :            :  */
      12                 :            : 
      13                 :            : #pragma once
      14                 :            : 
      15                 :            : #include <ad/geometry/GeometryOperation.hpp>
      16                 :            : #include <cstdint>
      17                 :            : #include <map>
      18                 :            : #include "ad/rss/core/RelativeConstellation.hpp"
      19                 :            : #include "ad/rss/core/shared_lock_guard.hpp"
      20                 :            : #include "ad/rss/state/RssState.hpp"
      21                 :            : #include "ad/rss/world/TimeIndex.hpp"
      22                 :            : 
      23                 :            : /*!
      24                 :            :  * @brief namespace ad
      25                 :            :  */
      26                 :            : namespace ad {
      27                 :            : /*!
      28                 :            :  * @brief namespace rss
      29                 :            :  */
      30                 :            : namespace rss {
      31                 :            : /*!
      32                 :            :  * @brief namespace unstructured
      33                 :            :  */
      34                 :            : namespace unstructured {
      35                 :            : 
      36                 :            : /**
      37                 :            :  * @brief Class to check whether an unstructured constellation is safe and to determine the proper response for the
      38                 :            :  * constellation
      39                 :            :  *
      40                 :            :  * Note: Implements the checks and responses given by the definitions 19-22 of the RSS paper (arXiv:1708.06374v6)
      41                 :            :  *
      42                 :            :  * Class performs required check if constellation is safe
      43                 :            :  * Class will maintain the previous state of the constellation in order to provide the proper response.
      44                 :            :  */
      45                 :            : class RssUnstructuredConstellationChecker
      46                 :            : {
      47                 :            : public:
      48                 :            :   enum class SafeState
      49                 :            :   {
      50                 :            :     safe,
      51                 :            :     unsafeBrakeOtherHasPrio,
      52                 :            :     unsafeBrakeEgoHasPrio,
      53                 :            :     unsafeBrakeBoth
      54                 :            :   };
      55                 :            : 
      56                 :            :   enum class DrivingMode
      57                 :            :   {
      58                 :            :     DriveAway,
      59                 :            :     ContinueForward,
      60                 :            :     Brake,
      61                 :            :     Invalid
      62                 :            :   };
      63                 :            : 
      64                 :            :   /**
      65                 :            :    * @brief Constructor
      66                 :            :    */
      67                 :            :   RssUnstructuredConstellationChecker();
      68                 :            : 
      69                 :            :   /**
      70                 :            :    * @brief Copy constructor
      71                 :            :    */
      72                 :            :   RssUnstructuredConstellationChecker(RssUnstructuredConstellationChecker const &other);
      73                 :            : 
      74                 :            :   /**
      75                 :            :    * @brief Destructor
      76                 :            :    */
      77                 :        775 :   ~RssUnstructuredConstellationChecker() = default;
      78                 :            : 
      79                 :            :   /**
      80                 :            :    * @brief Assignment operator
      81                 :            :    */
      82                 :            :   RssUnstructuredConstellationChecker &operator=(RssUnstructuredConstellationChecker const &other);
      83                 :            : 
      84                 :            :   /**
      85                 :            :    * @brief Calculate safety checks and determine required rssState for unstructured constellations
      86                 :            :    *
      87                 :            :    * @param[in]  time_index the time index of the constellation
      88                 :            :    * @param[in]  constellation constellation to analyze
      89                 :            :    * @param[out] egoStateInfo  rssState of the ego vehicle (Be aware: only calculated/updated once per timestep)
      90                 :            :    * @param[out] rssState  rssState of the vehicle
      91                 :            :    *
      92                 :            :    * @returns false if a failure occurred during calculations, true otherwise
      93                 :            :    */
      94                 :            :   bool calculateRssStateUnstructured(world::TimeIndex const &time_index,
      95                 :            :                                      core::RelativeConstellation const &constellation,
      96                 :            :                                      state::UnstructuredConstellationStateInformation &egoStateInfo,
      97                 :            :                                      state::RssState &rssState);
      98                 :            : 
      99                 :            :   /*!
     100                 :            :    * @brief Function callback type for unstructured trajectory set calculation
     101                 :            :    *
     102                 :            :    * @param[in] constellation The relative constellation the calculation is based on
     103                 :            :    * @param[in] objectId The id of the object the calculation is based on (one of the two constellation objects)
     104                 :            :    * @param[in] vehicleState The relative object state of the object the calculation is based on (one of the two
     105                 :            :    * constellation vehicle states)
     106                 :            :    * @param[out] brakePolygon The calculated brake polygon
     107                 :            :    * @param[out] continueForwardPolygon The calculated continue forward polygon
     108                 :            :    *
     109                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     110                 :            :    */
     111                 :            :   using CalculateTrajectorySetsCallbackFunctionType
     112                 :            :     = std::function<bool(::ad::rss::core::RelativeConstellation const &constellation,
     113                 :            :                          ::ad::rss::world::ObjectId const &objectId,
     114                 :            :                          ::ad::rss::core::RelativeObjectState const &vehicleState,
     115                 :            :                          ::ad::geometry::Polygon &brakePolygon,
     116                 :            :                          ::ad::geometry::Polygon &continueForwardPolygon)>;
     117                 :            : 
     118                 :            :   /*!
     119                 :            :    * @brief Register a callback for unstructured trajectory set calculation
     120                 :            :    *
     121                 :            :    * @param[in] objectType The object type this trajectory set calculation should be applied
     122                 :            :    * @param[in] calculateTrajectorySetsCallback The actual callback function to perform the trajectory set calculation
     123                 :            :    */
     124                 :            :   void
     125                 :         28 :   registerCalculateTrajectorySetsCallback(world::ObjectType objectType,
     126                 :            :                                           CalculateTrajectorySetsCallbackFunctionType calculateTrajectorySetsCallback)
     127                 :            :   {
     128         [ +  - ]:         28 :     std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
     129   [ +  -  +  - ]:         28 :     mCalculateTrajectorySetsCallbackMap[objectType] = calculateTrajectorySetsCallback;
     130                 :         28 :   }
     131                 :            : 
     132                 :            : private:
     133                 :            :   /**
     134                 :            :    * @brief Calculate the unstructured constellation state info
     135                 :            :    *
     136                 :            :    * @param[in] constellation The relative constellation the calculation is based on
     137                 :            :    * @param[in] objectId The id of the object the calculation is based on (one of the two constellation objects)
     138                 :            :    * @param[in]  objectState state of the object
     139                 :            :    * @param[out] stateInfo the calculated state info
     140                 :            :    *
     141                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     142                 :            :    */
     143                 :            :   bool calculateUnstructuredConstellationStateInfo(::ad::rss::core::RelativeConstellation const &constellation,
     144                 :            :                                                    ::ad::rss::world::ObjectId const &objectId,
     145                 :            :                                                    core::RelativeObjectState const &objectState,
     146                 :            :                                                    state::UnstructuredConstellationStateInformation &stateInfo) const;
     147                 :            : 
     148                 :            :   /**
     149                 :            :    * @brief Calculate the unstructured constellation state
     150                 :            :    *
     151                 :            :    * @param[in]  constellation constellation to analyze
     152                 :            :    * @param[in]  egoStateInfo the trajectory sets of the ego vehicle
     153                 :            :    * @param[in]  otherStateInfo the trajectory sets of the other traffic participant
     154                 :            :    * @param[out] rssState the calculated rss state
     155                 :            :    *
     156                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     157                 :            :    */
     158                 :            :   bool calculateState(core::RelativeConstellation const &constellation,
     159                 :            :                       state::UnstructuredConstellationStateInformation const &egoStateInfo,
     160                 :            :                       state::UnstructuredConstellationStateInformation const &otherStateInfo,
     161                 :            :                       state::UnstructuredConstellationRssState &rssState);
     162                 :            : 
     163                 :            :   /**
     164                 :            :    * @brief calculate the angle range that is allowed to drive away
     165                 :            :    *
     166                 :            :    * @param[in]  egoVehicleLocation the location of the ego vehicle
     167                 :            :    * @param[in]  otherVehicleLocation the location of the other vehicle
     168                 :            :    * @param[in]  maxAllowedAngleWhenBothStopped the maximum angle for calculation
     169                 :            :    * @param[out] range resulting heading range
     170                 :            :    *
     171                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     172                 :            :    */
     173                 :            :   bool calculateDriveAwayAngle(::ad::geometry::Point const &egoVehicleLocation,
     174                 :            :                                ::ad::geometry::Point const &otherVehicleLocation,
     175                 :            :                                physics::Angle const &maxAllowedAngleWhenBothStopped,
     176                 :            :                                ::ad::geometry::HeadingRange &range) const;
     177                 :            : 
     178                 :            :   /**
     179                 :            :    * @brief typedef for the mapping of constellation id to the corresponding otherMustBrake value before the danger
     180                 :            :    * threshold time
     181                 :            :    */
     182                 :            :   typedef std::map<core::RelativeConstellationId, bool> OtherMustBrakeStateBeforeDangerThresholdTimeMap;
     183                 :            : 
     184                 :            :   /**
     185                 :            :    * @brief the state of each constellation before the danger threshold time
     186                 :            :    *
     187                 :            :    * Needs to be stored to check which is the required behaviour to solve the constellation
     188                 :            :    */
     189                 :            :   OtherMustBrakeStateBeforeDangerThresholdTimeMap mOtherMustBrakeStatesBeforeDangerThresholdTime;
     190                 :            : 
     191                 :            :   /**
     192                 :            :    * @brief the new states to be considered in next time step
     193                 :            :    */
     194                 :            :   OtherMustBrakeStateBeforeDangerThresholdTimeMap mNewOtherMustBrakeStatesBeforeDangerThresholdTime;
     195                 :            : 
     196                 :            :   /**
     197                 :            :    * @brief time index of the current processing step
     198                 :            :    * If time index increases we need to update the state maps
     199                 :            :    */
     200                 :            :   world::TimeIndex mCurrentTimeIndex{0u};
     201                 :            : 
     202                 :            :   /**
     203                 :            :    * @brief Store required data for drive away calculations between timesteps
     204                 :            :    */
     205                 :            :   struct DriveAwayState
     206                 :            :   {
     207                 :            :     ::ad::geometry::HeadingRange allowedHeadingRange;
     208                 :            :     physics::Distance2D otherPosition;
     209                 :            :   };
     210                 :            : 
     211                 :            :   /**
     212                 :            :    * @brief map to state drive-away data for constellations
     213                 :            :    */
     214                 :            :   std::map<core::RelativeConstellationId, DriveAwayState> mDriveAwayStateMap;
     215                 :            : 
     216                 :            :   // one writer mutex for the callbacks of the class should be sufficient
     217                 :            :   // vast majority of concurrent access is of shared nature
     218                 :            :   mutable std::shared_timed_mutex mCallbackRwLock;
     219                 :            : 
     220                 :            :   std::map<world::ObjectType, CalculateTrajectorySetsCallbackFunctionType> mCalculateTrajectorySetsCallbackMap;
     221                 :            : };
     222                 :            : 
     223                 :            : } // namespace unstructured
     224                 :            : } // namespace rss
     225                 :            : } // namespace ad