Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2019-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/lane/MapAreaOperation.hpp"
      15                 :            : #include "ad/map/route/Planning.hpp"
      16                 :            : #include "ad/rss/map/ConnectingRoutesCache.hpp"
      17                 :            : #include "ad/rss/map/RssObjectResultList.hpp"
      18                 :            : #include "ad/rss/map/RssRouteCheckerData.hpp"
      19                 :            : 
      20                 :            : /*!
      21                 :            :  * @brief namespace rss
      22                 :            :  */
      23                 :            : namespace ad {
      24                 :            : /*!
      25                 :            :  * @brief namespace rss
      26                 :            :  */
      27                 :            : namespace rss {
      28                 :            : /*!
      29                 :            :  * @brief namespace map
      30                 :            :  */
      31                 :            : namespace map {
      32                 :            : 
      33                 :            : /// @brief supporting class for implementing the RSS checks
      34                 :            : template <class OBJECT_INSTANCE_TYPE> class RssRouteChecker
      35                 :            : {
      36                 :            : public:
      37                 :            :   /*!
      38                 :            :    * \brief Smart pointer on RssVehicleAdapter
      39                 :            :    */
      40                 :            :   typedef std::shared_ptr<RssRouteChecker> Ptr;
      41                 :            : 
      42                 :            :   /*!
      43                 :            :    * \brief Smart pointer on constant RssObjectAdapter
      44                 :            :    */
      45                 :            :   typedef std::shared_ptr<RssRouteChecker const> ConstPtr;
      46                 :            : 
      47                 :            :   /// @brief default constructor
      48                 :          5 :   RssRouteChecker(::ad::physics::Duration const dropOutdatedObjecstDuration)
      49                 :          5 :     : mDropOutdatedObjectsDuration(dropOutdatedObjecstDuration)
      50                 :          5 :     , mTimeIndex(0u)
      51                 :          5 :     , mArtificialObjectSamplingDistance(1.)
      52                 :         10 :     , mArtificialObjectsInjectionCallback(nullptr)
      53                 :            :   {
      54                 :          5 :   }
      55                 :            : 
      56                 :            :   /// @brief destructor
      57                 :          5 :   virtual ~RssRouteChecker() = default;
      58                 :            : 
      59                 :            :   /*!
      60                 :            :    *  @brief defines the type for callback to inject artificial objects
      61                 :            :    *
      62                 :            :    *  The provided parameters to the function can be used to re-use already pre-proccessed data on
      63                 :            :    *  vehicles and pedestrians at the current point in time.
      64                 :            :    */
      65                 :            :   using ArtificialObjectInjectionCallbackFunctionType
      66                 :            :     = std::function<RssArtificialObjectAdapterList<OBJECT_INSTANCE_TYPE>(
      67                 :            :       RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE> const &egoVehicleMap,
      68                 :            :       RssRouteCheckerVehicleDataMap<OBJECT_INSTANCE_TYPE> const &vehicleMap,
      69                 :            :       RssRouteCheckerPedestrianDataMap<OBJECT_INSTANCE_TYPE> const &pedestrianMap)>;
      70                 :            : 
      71                 :            :   /*!
      72                 :            :    *  @brief register a callback to inject artificial objects
      73                 :            :    *
      74                 :            :    * This function allows to register a callback to inject artificial objects into the procssing of the checkObjects()
      75                 :            :    * function. The callback is triggered after the object and route data of the vehicles and pedestrians is updated to
      76                 :            :    * allow the injection function the reuse of already available data like map matched positions and future routes.
      77                 :            :    *
      78                 :            :    * @param[in] artificialObjectSamplingDistance the sampling distance used for map matching of artificial objects
      79                 :            :    * (see ad::map::match::AdMapMatching::getMapMatchedBoundingBox() for a detailed description of this
      80                 :            :    * @param[in] artificialObjectsInjectionCallback the actual callback function to register (use nullptr to unregister)
      81                 :            :    *
      82                 :            :    */
      83                 :            :   void registerArtificialObjectInjectionCallback(
      84                 :            :     ::ad::physics::Distance const artificialObjectSamplingDistance,
      85                 :            :     ArtificialObjectInjectionCallbackFunctionType artificialObjectsInjectionCallback);
      86                 :            : 
      87                 :            :   /*!
      88                 :            :    * @brief Function callback type for unstructured trajectory set calculation
      89                 :            :    *
      90                 :            :    * @param[in] egoVehicle The ego-vehicle data the calculation is performed for
      91                 :            :    * @param[in] routedId The route id of the ego-vehicle the calculation is performed for
      92                 :            :    * @param[in] constellation The relative constellation the calculation is based on
      93                 :            :    * @param[in] objectId The id of the object the calculation is based on (one of the two constellation objects)
      94                 :            :    * @param[in] vehicleState The relative object state of the object the calculation is based on (one of the two
      95                 :            :    * constellation vehicle states)
      96                 :            :    * @param[out] brakePolygon The calculated brake polygon
      97                 :            :    * @param[out] continueForwardPolygon The calculated continue forward polygon
      98                 :            :    *
      99                 :            :    * @returns false if a failure occurred during calculations, true otherwise
     100                 :            :    */
     101                 :            :   using CalculateTrajectorySetsCallbackFunctionType
     102                 :            :     = std::function<bool(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &egoVehicle,
     103                 :            :                          ::ad::rss::map::RssRouteId const &routeId,
     104                 :            :                          ::ad::rss::core::RelativeConstellation const &constellation,
     105                 :            :                          ::ad::rss::world::ObjectId const &objectId,
     106                 :            :                          ::ad::rss::core::RelativeObjectState const &vehicleState,
     107                 :            :                          ::ad::geometry::Polygon &brakePolygon,
     108                 :            :                          ::ad::geometry::Polygon &continueForwardPolygon)>;
     109                 :            : 
     110                 :            :   /*!
     111                 :            :    * @brief Register a callback for unstructured trajectory set calculation
     112                 :            :    *
     113                 :            :    * @param[in] objectType The object type this trajectory set calculation should be applied
     114                 :            :    * @param[in] calculateTrajectorySetsCallback The actual callback function to perform the trajectory set calculation
     115                 :            :    */
     116                 :            :   void
     117                 :            :   registerCalculateTrajectorySetsCallback(world::ObjectType objectType,
     118                 :            :                                           CalculateTrajectorySetsCallbackFunctionType calculateTrajectorySetsCallback);
     119                 :            : 
     120                 :            :   /*!
     121                 :            :    *  @brief perform the RSS check of the objects
     122                 :            :    *
     123                 :            :    *  This function performs the following steps:
     124                 :            :    *
     125                 :            :    *  - update the object data of egoVehicles, vehicles, pedestrians (i.e. data of RssObjectAdapter class including
     126                 :            :    *    map matching)
     127                 :            :    *  - update the route data of the egoVehicles and vehicles
     128                 :            :    *  - trigger artificial object injection callback if registered (see also
     129                 :            :    * registerArtificialObjectInjectionCallback()) and update
     130                 :            :    *  - keep track of the ::ad::rss::core::RssCheck objects for each route of each ego vehicle (which defines the input
     131                 :            :    * situation to be checked; one checker for one situation)
     132                 :            :    *  - create the world model for each situation (each route of each ego vehicle)
     133                 :            :    *  - extract the situation for each situation (each route of each ego vehicle)
     134                 :            :    *  - check the constellation for each situation (each route of each ego vehicle)
     135                 :            :    *  - provide a proper response for each situation (each route of each ego vehicle)
     136                 :            :    *
     137                 :            :    *  The protected callback functions allow to interact with the calculation process and e.g. allow to override
     138                 :            :    *  intermediate results.
     139                 :            :    *
     140                 :            :    *  @returns a RSS result list of all considered egoVehicles, vehicles, pedestrians and artificialObjects within this
     141                 :            :    *    calculations
     142                 :            :    */
     143                 :            :   virtual RssObjectResultList
     144                 :            :   checkObjects(std::chrono::system_clock::time_point const &currentTime,
     145                 :            :                ::ad::physics::Distance const vehicleSamplingDistance,
     146                 :            :                RssEgoVehicleAdapterList<OBJECT_INSTANCE_TYPE> &egoVehicles,
     147                 :            :                RssVehicleAdapterList<OBJECT_INSTANCE_TYPE> &vehicles,
     148                 :            :                ::ad::physics::Distance const pedestrianSamplingDistance,
     149                 :            :                RssPedestrianAdapterList<OBJECT_INSTANCE_TYPE> &pedestrians,
     150                 :            :                typename RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::RouteExtensionMode routeExtensionMode);
     151                 :            : 
     152                 :            :   /*!
     153                 :            :    * @brief These group of functions restrict the operation of this class to a certain map area
     154                 :            :    *
     155                 :            :    * The computations within this class are heavily depending on the size and complexity of the used ad map.
     156                 :            :    * Especially use cases with many ego vehicles suffer from this.
     157                 :            :    * On the other hand, the use-cases with many vehicles are often restricted to specific locations
     158                 :            :    * on a map with a certain extend.
     159                 :            :    *
     160                 :            :    * The following functions provide different means of restricting the lanes of the map that actually
     161                 :            :    * are being considered for internal computations like:
     162                 :            :    * - ::ad::map::match::AdMapMatching()
     163                 :            :    * - ::ad::map::route::extendRouteToDistance()
     164                 :            :    * - ::ad::map::route::planning::predictRoutesOnDistance()
     165                 :            :    * If the map area is restricted by one of the following functions, all the map based operations will
     166                 :            :    * be performed as if the rest of the map would not exist at all.
     167                 :            :    *
     168                 :            :    * @{
     169                 :            :    */
     170                 :            :   /*!
     171                 :            :    * @brief restriction of the map area to an explicit list of lanes
     172                 :            :    *
     173                 :            :    * This function sets the lane to consider for all map operations.
     174                 :            :    *
     175                 :            :    * @param[in] lanesToConsider the lanes of the map to be considered
     176                 :            :    *
     177                 :            :    */
     178                 :            :   void restrictOperationToMapArea(::ad::map::lane::LaneIdSet const &lanesToConsider);
     179                 :            : 
     180                 :            :   /*!
     181                 :            :    * @brief restriction of the map area to a bounding sphere
     182                 :            :    *
     183                 :            :    * This function collects all lane within the given bounding sphere in a LaneIdSet
     184                 :            :    * to consider for all map operations.
     185                 :            :    * For collecting the lanes ::ad::map::lane::isNear() is called that checks for overlapping
     186                 :            :    * of the bounding sphere of the lane and the given bounding sphere.
     187                 :            :    *
     188                 :            :    * @param[in] bounding_sphere the bounding sphere of the map area to be considered
     189                 :            :    *
     190                 :            :    * @returns \c true if the map area is filled
     191                 :            :    */
     192                 :            :   bool restrictOperationToMapArea(::ad::map::point::BoundingSphere const &bounding_sphere);
     193                 :            : 
     194                 :            :   /*!
     195                 :            :    * @brief restriction of the map area to a set of reachable lanes
     196                 :            :    *
     197                 :            :    * This function collects all reachable lanes from the given list of input MapAreaPredictionStartPoint
     198                 :            :    * to consider for all map operations.
     199                 :            :    *
     200                 :            :    * @param[in] predictionStartPoints the start points used to perform route prediction
     201                 :            :    * with respective prediction distance and route creation mode
     202                 :            :    *
     203                 :            :    * @returns \c true if the map area is filled
     204                 :            :    */
     205                 :            :   bool restrictOperationToMapArea(std::vector<ad::map::lane::MapAreaPredictionStartPoint> const &predictionStartPoints);
     206                 :            : 
     207                 :            :   /**
     208                 :            :    * @brief restriction of the map area around an intersection to a set of reachable lanes
     209                 :            :    *
     210                 :            :    * This function collects all reachable lanes from the intersection at the given intersectionPoint
     211                 :            :    * to consider for all map operations.
     212                 :            :    * Reachable in the sense of all inner intersection lanes plus all entry/exit arms of the intersection
     213                 :            :    * until the given intersectionDistance is reached.
     214                 :            :    *
     215                 :            :    * @param[in] intersectionPoint the start points used to check for an intersection to be used
     216                 :            :    * @param[in] intersectionDistance distance to be used for a directionless prediction from
     217                 :            :    *            all intersection border points
     218                 :            :    *
     219                 :            :    * @returns \c true if the map area is filled
     220                 :            :    */
     221                 :            :   bool restrictOperationToIntersectionArea(::ad::map::point::ENUPoint const &intersectionPoint,
     222                 :            :                                            ::ad::physics::Distance const &intersectionDistance);
     223                 :            : 
     224                 :            :   /*!
     225                 :            :    * @brief clear the restriction to a certain map area
     226                 :            :    *
     227                 :            :    * This function clears the lanes to consider for all map operations.
     228                 :            :    */
     229                 :            :   void clearOperationToMapAreaRestriction();
     230                 :            :   /*!
     231                 :            :    *  @}
     232                 :            :    */
     233                 :            : 
     234                 :            :   /*!
     235                 :            :    * @brief check if the ego vehicle object is still present.
     236                 :            :    *
     237                 :            :    * ego vehicles are dropped from the internal list if there is not update on it for longer than
     238                 :            :    * the constructor parameter \c dropOutdatedObjecstDuration.
     239                 :            :    */
     240                 :            :   bool isEgoVehicleAvailable(::ad::rss::world::ObjectId ego_object_id)
     241                 :            :   {
     242                 :            :     return mEgoVehicleMap.find(ego_object_id) != mEgoVehicleMap.end();
     243                 :            :   }
     244                 :            : 
     245                 :            :   /*!
     246                 :            :    * @brief check if the ego vehicle object is still present and currently active.
     247                 :            :    *
     248                 :            :    * Check if the ego vehicle is available and still listed as active. The vehicles are active if they have been part of
     249                 :            :    * the latest check.
     250                 :            :    */
     251                 :            :   bool isEgoVehicleActive(::ad::rss::world::ObjectId ego_object_id)
     252                 :            :   {
     253                 :            :     auto findResult = mEgoVehicleMap.find(ego_object_id);
     254                 :            :     if (findResult != mEgoVehicleMap.end())
     255                 :            :     {
     256                 :            :       return findResult->second.mActive;
     257                 :            :     }
     258                 :            :     else
     259                 :            :     {
     260                 :            :       return false;
     261                 :            :     }
     262                 :            :   }
     263                 :            : 
     264                 :            : protected:
     265                 :            :   /*! Callback functions called after the respective functionality is processed
     266                 :            :    *
     267                 :            :    *  The following set of callback functions are intentionally non const to allow derived classes to adapt intermediate
     268                 :            :    * results.
     269                 :            :    *  Depending on the research activities conducted, this might be useful to quickly log and/or test out the effects of
     270                 :            :    * smaller
     271                 :            :    *  changes to the complete proceessing chain.
     272                 :            :    *
     273                 :            :    *  @{
     274                 :            :    */
     275                 :            :   /*!
     276                 :            :    * @brief callback after object data of the ego vehicles has been updated
     277                 :            :    */
     278                 :         11 :   virtual void objectDataUpdated(RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE> &egoVehicleMap)
     279                 :            :   {
     280                 :            :     (void)egoVehicleMap;
     281                 :         11 :   }
     282                 :            :   /*!
     283                 :            :    * @brief callback after object data of the vehicles has been updated
     284                 :            :    */
     285                 :         11 :   virtual void objectDataUpdated(RssRouteCheckerVehicleDataMap<OBJECT_INSTANCE_TYPE> &vehicleMap)
     286                 :            :   {
     287                 :            :     (void)vehicleMap;
     288                 :         11 :   }
     289                 :            :   /*!
     290                 :            :    * @brief callback after object data of the pedestrians has been updated
     291                 :            :    */
     292                 :         11 :   virtual void objectDataUpdated(RssRouteCheckerPedestrianDataMap<OBJECT_INSTANCE_TYPE> &pedestrianMap)
     293                 :            :   {
     294                 :            :     (void)pedestrianMap;
     295                 :         11 :   }
     296                 :            :   /*!
     297                 :            :    * @brief callback after object data of the artificial objects has been updated
     298                 :            :    */
     299                 :         11 :   virtual void objectDataUpdated(RssRouteCheckerArtificialObjectDataMap<OBJECT_INSTANCE_TYPE> &artificialObjectMap)
     300                 :            :   {
     301                 :            :     (void)artificialObjectMap;
     302                 :         11 :   }
     303                 :            :   /*!
     304                 :            :    * @brief callback after the routes of the ego vehicles have been updated
     305                 :            :    */
     306                 :         11 :   virtual void routesUpdated(RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE> &egoVehicleMap)
     307                 :            :   {
     308                 :            :     (void)egoVehicleMap;
     309                 :         11 :   }
     310                 :            :   /*!
     311                 :            :    * @brief callback after the routes of the vehicles have been updated
     312                 :            :    */
     313                 :         11 :   virtual void routesUpdated(RssRouteCheckerVehicleDataMap<OBJECT_INSTANCE_TYPE> &vehicleMap)
     314                 :            :   {
     315                 :            :     (void)vehicleMap;
     316                 :         11 :   }
     317                 :            :   /*!
     318                 :            :    * @brief callback after the rssRouteCheckResult situations has been created for the respective ego vehicle
     319                 :            :    *
     320                 :            :    *  Filled members of rssRouteCheckResult:
     321                 :            :    *  - rss_route
     322                 :            :    *  - world_model
     323                 :            :    */
     324                 :         28 :   virtual void situationCreated(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &ego_vehicle,
     325                 :            :                                 RssRouteCheckResult &rssRouteCheckResult)
     326                 :            :   {
     327                 :            :     (void)ego_vehicle;
     328                 :            :     (void)rssRouteCheckResult;
     329                 :         28 :   }
     330                 :            :   /*!
     331                 :            :    * @brief callback after all rssRouteCheckResult situations have been created for the respective ego vehicle
     332                 :            :    *
     333                 :            :    *  Filled members of respective rssRouteCheckResult:
     334                 :            :    *  - rss_route
     335                 :            :    *  - world_model
     336                 :            :    */
     337                 :         17 :   virtual void situationsCreated(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &ego_vehicle,
     338                 :            :                                  RssObjectResult &rssObjectResult)
     339                 :            :   {
     340                 :            :     (void)ego_vehicle;
     341                 :            :     (void)rssObjectResult;
     342                 :         17 :   }
     343                 :            :   /*!
     344                 :            :    * @brief callback after all situations for all ego vehicles have been created
     345                 :            :    */
     346                 :         11 :   virtual void situationsCreated(RssObjectResultList &objectResultList)
     347                 :            :   {
     348                 :            :     (void)objectResultList;
     349                 :         11 :   }
     350                 :            :   /*!
     351                 :            :    * @brief callback after all constellations for all ego vehicles have been checked
     352                 :            :    */
     353                 :         11 :   virtual void checksPerformed(RssObjectResultList &objectResultList)
     354                 :            :   {
     355                 :            :     (void)objectResultList;
     356                 :         11 :   }
     357                 :            :   /*!
     358                 :            :    *  @}
     359                 :            :    */
     360                 :            : 
     361                 :            : private:
     362                 :            :   /*! Function to update object maps with latest adapter classes
     363                 :            :    */
     364                 :            :   template <class Data, class Adapter>
     365                 :            :   void updateObjectMap(std::chrono::system_clock::time_point const &currentTime,
     366                 :            :                        std::map<::ad::rss::world::ObjectId, Data> &objectMap,
     367                 :            :                        std::vector<typename Adapter::Ptr> const &activeObjectAdapterList);
     368                 :            : 
     369                 :            :   /*!
     370                 :            :    *  @brief update the RssCheck objects in respect to the routes of the RssEgoVehicleAdapter list
     371                 :            :    */
     372                 :            :   void updateRssCheckers();
     373                 :            : 
     374                 :            :   RssRouteCheckResult createSituationForRoute(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &ego_vehicle,
     375                 :            :                                               RssRoute const &rss_route);
     376                 :            : 
     377                 :            :   RssObjectResultList createSituations();
     378                 :            : 
     379                 :            :   void appendObjectsToResults(RssObjectResultList &objectResultList);
     380                 :            : 
     381                 :            :   void updateRssCheckerCallbacks(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &egoVehicle,
     382                 :            :                                  RssRouteCheckMap::iterator routeCheckerMapIter);
     383                 :            : 
     384                 :            :   void updateRssCheckerCallbacksLocked(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &egoVehicle,
     385                 :            :                                        RssRouteCheckMap::iterator routeCheckerMapIter);
     386                 :            : 
     387                 :            :   bool callCalculateTrajectorySetsCallback(CalculateTrajectorySetsCallbackFunctionType callback,
     388                 :            :                                            ::ad::rss::world::ObjectId const &egoVehicleId,
     389                 :            :                                            ::ad::rss::map::RssRouteId const &routeId,
     390                 :            :                                            ::ad::rss::core::RelativeConstellation const &constellation,
     391                 :            :                                            ::ad::rss::world::ObjectId const &objectId,
     392                 :            :                                            ::ad::rss::core::RelativeObjectState const &vehicleState,
     393                 :            :                                            ::ad::geometry::Polygon &brakePolygon,
     394                 :            :                                            ::ad::geometry::Polygon &continueForwardPolygon);
     395                 :            : 
     396                 :            :   ::ad::physics::Duration const mDropOutdatedObjectsDuration;
     397                 :            :   ::ad::rss::world::TimeIndex mTimeIndex;
     398                 :            :   ::ad::map::lane::LaneIdSet mMapAreaLanes;
     399                 :            :   ConnectingRoutesCache mConnectingRoutesCache;
     400                 :            : 
     401                 :            :   RssRouteCheckerPedestrianDataMap<OBJECT_INSTANCE_TYPE> mPedestrianMap;
     402                 :            :   RssRouteCheckerVehicleDataMap<OBJECT_INSTANCE_TYPE> mVehicleMap;
     403                 :            :   RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE> mEgoVehicleMap;
     404                 :            :   ::ad::physics::Distance mArtificialObjectSamplingDistance;
     405                 :            :   ArtificialObjectInjectionCallbackFunctionType mArtificialObjectsInjectionCallback;
     406                 :            :   RssRouteCheckerArtificialObjectDataMap<OBJECT_INSTANCE_TYPE> mArtificialObjectMap;
     407                 :            : 
     408                 :            :   // one writer mutex for the callbacks of the class should be sufficient
     409                 :            :   // vast majority of concurrent access is of shared nature
     410                 :            :   mutable std::shared_timed_mutex mCallbackRwLock;
     411                 :            : 
     412                 :            :   std::map<world::ObjectType, CalculateTrajectorySetsCallbackFunctionType> mCalculateTrajectorySetsCallbackMap;
     413                 :            : };
     414                 :            : 
     415                 :            : } // namespace map
     416                 :            : } // namespace rss
     417                 :            : } // namespace ad
     418                 :            : 
     419                 :            : #include "ad/rss/map/detail/RssRouteCheckerDetail.hpp"