Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2022 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/map/landmark/Types.hpp>
      15                 :            : #include <ad/map/lane/Types.hpp>
      16                 :            : #include <ad/map/route/Types.hpp>
      17                 :            : #include <ad/rss/map/RssAppendRoadBoundariesMode.hpp>
      18                 :            : #include <ad/rss/map/RssConstellationCreationMode.hpp>
      19                 :            : #include <ad/rss/map/RssObjectData.hpp>
      20                 :            : #include <ad/rss/map/RssRestrictSpeedLimitMode.hpp>
      21                 :            : #include <ad/rss/map/RssRouteList.hpp>
      22                 :            : #include <ad/rss/world/WorldModel.hpp>
      23                 :            : #include <mutex>
      24                 :            : #include <ostream>
      25                 :            : #include <string>
      26                 :            : 
      27                 :            : #include "ad/rss/map/ConnectingRoutesCache.hpp"
      28                 :            : 
      29                 :            : /*!
      30                 :            :  * @brief namespace ad
      31                 :            :  */
      32                 :            : namespace ad {
      33                 :            : /*!
      34                 :            :  * @brief namespace rss
      35                 :            :  */
      36                 :            : namespace rss {
      37                 :            : /*!
      38                 :            :  * @brief namespace map
      39                 :            :  */
      40                 :            : namespace map {
      41                 :            : 
      42                 :            : class RssWorldModelCreator;
      43                 :            : class RssObjectConversion;
      44                 :            : 
      45                 :            : /*!
      46                 :            :  * @brief class providing supporting functions to create a world model and its constellations.
      47                 :            :  *
      48                 :            :  * The appendConstellations() and appendRoadBoundaries() are multi-thread safe
      49                 :            :  */
      50                 :            : class RssWorldModelCreation
      51                 :            : {
      52                 :            : public:
      53                 :            :   /**
      54                 :            :    * @brief create a constellation creation object
      55                 :            :    *
      56                 :            :    * A world model object is created and initialized with given time_index and default ego dynamics
      57                 :            :    *
      58                 :            :    * @param[in] time_index the time index to be applied
      59                 :            :    * @param[in] defaultEgoRssDynamics the default ego vehicles' RssDynamics to be applied
      60                 :            :    */
      61                 :            :   RssWorldModelCreation(::ad::rss::world::TimeIndex const &time_index,
      62                 :            :                         ::ad::rss::world::RssDynamics const &defaultEgoRssDynamics);
      63                 :            : 
      64                 :            :   /**
      65                 :            :    * @brief create a constellation creation object
      66                 :            :    *
      67                 :            :    * A world model object is created and initialized with given time_index and default ego dynamics
      68                 :            :    *
      69                 :            :    * @param[in] time_index the time index to be applied
      70                 :            :    * @param[in] defaultEgoRssDynamics the default ego vehicles' RssDynamics to be applied
      71                 :            :    * @param[in] connecting_routes_cache a cache for connecting routes to be used
      72                 :            :    */
      73                 :            :   RssWorldModelCreation(::ad::rss::world::TimeIndex const &time_index,
      74                 :            :                         ::ad::rss::world::RssDynamics const &defaultEgoRssDynamics,
      75                 :            :                         ConnectingRoutesCache &connecting_routes_cache);
      76                 :            : 
      77                 :            :   /**
      78                 :            :    *  @brief default destructor
      79                 :            :    */
      80                 :        242 :   ~RssWorldModelCreation() = default;
      81                 :            : 
      82                 :            :   /**
      83                 :            :    * @brief Setter for a route_id provided on debug output
      84                 :            :    */
      85                 :         28 :   void setRouteId(::ad::rss::map::RssRouteId const &route_id)
      86                 :            :   {
      87                 :         28 :     mRouteId = route_id;
      88                 :         28 :   }
      89                 :            : 
      90                 :            :   /*! @brief Sets the distance below which a detailed analysis becomes necessary regardless of the objects stopping
      91                 :            :    * distance estimates.
      92                 :            :    *
      93                 :            :    *  Far away object might not be analyzed in detail, if the stopping distances allow it
      94                 :            :    */
      95                 :         28 :   void setMinimumDistanceToObjectsThatHaveToBeAnalyzed(
      96                 :            :     ad::physics::Distance const minimumDistanceToObjectsThatHaveToBeAnalyzed)
      97                 :            :   {
      98                 :         28 :     mMinimumDistanceToObjectsThatHaveToBeAnalyzed = minimumDistanceToObjectsThatHaveToBeAnalyzed;
      99                 :         28 :   }
     100                 :            : 
     101                 :            :   /**
     102                 :            :    * @brief create possible constellations between ego vehicle and the object
     103                 :            :    *
     104                 :            :    * Based on the map information a connecting route between ego vehicle and the object is created.
     105                 :            :    *
     106                 :            :    * @param[in] egoObjectData the ego vehicle's data
     107                 :            :    * @param[in] egoRoute the route the ego vehicle intends to take.
     108                 :            :    *   If the given route is empty, all potential route predictions of the ego vehicle are taken into account if
     109                 :            :    * required.
     110                 :            :    * @param[in] otherObjectData the other object's data
     111                 :            :    * @param[in] restrict_speed_limit_mode the mode to select the behavior of
     112                 :            :    * object_rss_dynamics.max_speed_on_acceleration and
     113                 :            :    * egoRssDynamics.max_speed_on_acceleration
     114                 :            :    * parameter
     115                 :            :    * @param[in] greenTrafficLights the list of known green traffic lights.
     116                 :            :    *   Required to derive the correct priority rule for the ego vehicle when approaching a traffic light intersection.
     117                 :            :    * @param[in] mode the mode of this constellation
     118                 :            :    *
     119                 :            :    * @returns \c true if the operation succeeded.
     120                 :            :    */
     121                 :            :   bool appendConstellations(RssObjectData const &egoObjectData,
     122                 :            :                             ::ad::map::route::FullRoute const &egoRoute,
     123                 :            :                             RssObjectData const &otherObjectData,
     124                 :            :                             RssRestrictSpeedLimitMode const &restrict_speed_limit_mode,
     125                 :            :                             ::ad::map::landmark::LandmarkIdSet const &greenTrafficLights,
     126                 :            :                             ::ad::rss::map::RssConstellationCreationMode const &mode,
     127                 :            :                             ::ad::map::lane::LaneIdSet const &relevantLanes = ::ad::map::lane::LaneIdSet());
     128                 :            : 
     129                 :            :   /**
     130                 :            :    * @brief create possible constellations between ego vehicle and the object
     131                 :            :    *
     132                 :            :    * Based on the map information a connecting route between ego vehicle and the object is created.
     133                 :            :    *
     134                 :            :    * @param[in] egoObjectData the ego vehicle's data
     135                 :            :    * @param[in] egoRoute the route the ego vehicle intends to take.
     136                 :            :    *   If the given route is empty, all potential route predictions of the ego vehicle are taken into account if
     137                 :            :    * required.
     138                 :            :    * @param[in] otherObjectData the other object's data
     139                 :            :    * @param[in] restrict_speed_limit_mode the mode to select the behavior of
     140                 :            :    * object_rss_dynamics.max_speed_on_acceleration and
     141                 :            :    * egoRssDynamics.max_speed_on_acceleration
     142                 :            :    * parameter
     143                 :            :    * @param[in] greenTrafficLights the list of known green traffic lights.
     144                 :            :    *   Required to derive the correct priority rule for the ego vehicle when approaching a traffic light intersection.
     145                 :            :    * @param[in] mode the mode of this constellation
     146                 :            :    * @param[in] egoObjectPredictionHints route prediction hints for ego object
     147                 :            :    * @param[in] otherObjectPredictionHints route prediction hints for other object
     148                 :            :    *
     149                 :            :    * @returns \c true if the operation succeeded.
     150                 :            :    */
     151                 :            :   bool appendConstellations(RssObjectData const &egoObjectData,
     152                 :            :                             ::ad::map::route::FullRoute const &egoRoute,
     153                 :            :                             RssObjectData const &otherObjectData,
     154                 :            :                             RssRestrictSpeedLimitMode const &restrict_speed_limit_mode,
     155                 :            :                             ::ad::map::landmark::LandmarkIdSet const &greenTrafficLights,
     156                 :            :                             ::ad::rss::map::RssConstellationCreationMode const &mode,
     157                 :            :                             RssRouteList const &egoObjectPredictionHints,
     158                 :            :                             RssRouteList const &otherObjectPredictionHints,
     159                 :            :                             ::ad::map::lane::LaneIdSet const &relevantLanes = ::ad::map::lane::LaneIdSet());
     160                 :            : 
     161                 :            :   /**
     162                 :            :    * @brief create constellations between ego vehicle and road boundaries
     163                 :            :    *
     164                 :            :    * Based on the map information static object constellations at the borders of the driveable lanes are created.
     165                 :            :    * For detailed operation modes see the operation modes at \a RssAppendRoadBoundariesMode.
     166                 :            :    *
     167                 :            :    * @param[in] egoObjectData the ego vehicle's data
     168                 :            :    * @param[in] egoRoute the route the ego vehicle intends to take.
     169                 :            :    * @param[in] operationMode the mode how the road boundaries shall be handled
     170                 :            :    *
     171                 :            :    * @returns \c true if the operation succeeded.
     172                 :            :    */
     173                 :            :   bool appendRoadBoundaries(RssObjectData const &egoObjectData,
     174                 :            :                             ::ad::map::route::FullRoute const &route,
     175                 :            :                             RssAppendRoadBoundariesMode const operationMode);
     176                 :            : 
     177                 :            :   /**
     178                 :            :    * @brief get the final world model object
     179                 :            :    *
     180                 :            :    * After the call of this function the world model is treated as finalized.
     181                 :            :    * No further operation will succeed.
     182                 :            :    * Successive calls to this will return an empty default initialized world model.
     183                 :            :    */
     184                 :            :   ::ad::rss::world::WorldModel getWorldModel();
     185                 :            : 
     186                 :            :   /**
     187                 :            :    * @brief constant defining the minimum lenght to be used for the search of connected routes
     188                 :            :    *
     189                 :            :    *  Actually the maximum of this and the conservative stopping distances of the objects (see
     190                 :            :    * RssObjectConversion::calculateConservativeMinStoppingDistance()) is used for the calculations.
     191                 :            :    */
     192                 :            :   static const ::ad::physics::Distance cMinConnectedRouteLength;
     193                 :            : 
     194                 :            : private:
     195                 :            :   friend class RssWorldModelCreator;
     196                 :            : 
     197                 :            :   bool appendStructuredConstellations(::ad::rss::map::RssWorldModelCreator &constellationCreator,
     198                 :            :                                       std::shared_ptr<RssObjectConversion const> const &egoObject,
     199                 :            :                                       ::ad::map::route::FullRoute const &egoRoute,
     200                 :            :                                       std::shared_ptr<RssObjectConversion const> const &otherObject,
     201                 :            :                                       RssRouteList const &egoObjectPredictionHints,
     202                 :            :                                       RssRouteList const &otherObjectPredictionHints,
     203                 :            :                                       ::ad::map::lane::LaneIdSet const &relevantLanes,
     204                 :            :                                       ::ad::physics::Distance const &maxConnectingRouteDistance);
     205                 :            : 
     206                 :            :   bool appendConstellationToWorldModel(::ad::rss::world::Constellation const &constellation);
     207                 :            : 
     208                 :            :   ::ad::rss::world::WorldModel mWorldModel;
     209                 :            :   bool mFinalized;
     210                 :            :   std::mutex mWorldModelLock;
     211                 :            :   ::ad::rss::map::RssRouteId mRouteId{0};
     212                 :            :   ad::physics::Distance mMinimumDistanceToObjectsThatHaveToBeAnalyzed{RssWorldModelCreation::cMinConnectedRouteLength};
     213                 :            :   ConnectingRoutesCache *mConnectingRoutesCache;
     214                 :            : };
     215                 :            : 
     216                 :            : } // namespace map
     217                 :            : } // namespace rss
     218                 :            : } // namespace ad