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/access/Logging.hpp>
      15                 :            : #include <ad/rss/core/Logging.hpp>
      16                 :            : #include <ad/rss/core/shared_lock_guard.hpp>
      17                 :            : #include <ad/rss/map/Logging.hpp>
      18                 :            : #include <ad/rss/map/RssAppendRoadBoundariesMode.hpp>
      19                 :            : #include <ad/rss/map/RssConstellationConfiguration.hpp>
      20                 :            : #include <ad/rss/map/RssObjectAdapter.hpp>
      21                 :            : #include <ad/rss/map/RssObjectData.hpp>
      22                 :            : #include <ad/rss/map/RssRoute.hpp>
      23                 :            : #include <ad/rss/map/RssWorldModelCreation.hpp>
      24                 :            : #include <map>
      25                 :            : 
      26                 :            : /*!
      27                 :            :  * @brief namespace rss
      28                 :            :  */
      29                 :            : namespace ad {
      30                 :            : /*!
      31                 :            :  * @brief namespace rss
      32                 :            :  */
      33                 :            : namespace rss {
      34                 :            : /*!
      35                 :            :  * @brief namespace map
      36                 :            :  */
      37                 :            : namespace map {
      38                 :            : 
      39                 :            : /**
      40                 :            :  *  @brief class for interacting with the RssRouteChecker in a thread safe manner
      41                 :            :  *
      42                 :            :  *  Allows to register callbacks and configuration values that get called on calculations of RssRouteChecker
      43                 :            :  *  via the RssObjectAdapter classes default implementations.
      44                 :            :  */
      45                 :            : template <class OBJECT_INSTANCE_TYPE> class RssRouteCheckerControl
      46                 :            : {
      47                 :            : public:
      48                 :            :   /*!
      49                 :            :    * \brief Smart pointer on RssObjectAdapter
      50                 :            :    */
      51                 :            :   typedef std::shared_ptr<RssRouteCheckerControl> Ptr;
      52                 :            : 
      53                 :            :   /*!
      54                 :            :    * \brief Smart pointer on constant RssObjectAdapter
      55                 :            :    */
      56                 :            :   typedef std::shared_ptr<RssRouteCheckerControl const> ConstPtr;
      57                 :            : 
      58                 :            :   /** @brief default constructor
      59                 :            :    */
      60                 :        219 :   RssRouteCheckerControl() = default;
      61                 :            : 
      62                 :            :   /** @brief default destructor
      63                 :            :    */
      64                 :        219 :   virtual ~RssRouteCheckerControl() = default;
      65                 :            : 
      66                 :            :   /** @brief Callback function type to query the default RSS parameters to be used for the given other vehicle instance
      67                 :            :    */
      68                 :            :   using DefaultRssDynamicsCallbackFunctionType
      69                 :            :     = std::function<::ad::rss::world::RssDynamics(OBJECT_INSTANCE_TYPE vehicle)>;
      70                 :            : 
      71                 :            :   /** @brief Register a callback function to query the default RSS parameters to be used for the respective
      72                 :            :    * vehicle instance of given objectType
      73                 :            :    */
      74                 :       1095 :   void registerDefaultRssDynamicsCallback(::ad::rss::world::ObjectType const objectType,
      75                 :            :                                           DefaultRssDynamicsCallbackFunctionType callback)
      76                 :            :   {
      77         [ +  - ]:       1095 :     std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
      78   [ +  -  +  - ]:       1095 :     mDefaultRssDynamicsCallbacks[objectType] = callback;
      79                 :       1095 :   }
      80                 :            : 
      81                 :            :   /** @returns \c true if a callback function to query the default RSS parameters to be used for the respective
      82                 :            :    * vehicle instance of given objectType is registered
      83                 :            :    */
      84                 :            :   bool isDefaultRssDynamicsCallbackRegistered(::ad::rss::world::ObjectType const objectType) const
      85                 :            :   {
      86                 :            :     core::shared_lock_guard const shared_lock(mCallbackRwLock);
      87                 :            :     return mDefaultRssDynamicsCallbacks.find(objectType) != mDefaultRssDynamicsCallbacks.end();
      88                 :            :   }
      89                 :            : 
      90                 :            :   /** @returns the default RSS parameters to be used for the given vehicle instance of given objectType by calling the
      91                 :            :    * registered callback function (called by RssVehicleAdapter)
      92                 :            :    *
      93                 :            :    *  If no callback function is registered an invalid default constructed ::ad::rss::world::RssDynamics() is returned.
      94                 :            :    */
      95                 :        455 :   ::ad::rss::world::RssDynamics getDefaultRssDynamics(::ad::rss::world::ObjectType const objectType,
      96                 :            :                                                       OBJECT_INSTANCE_TYPE vehicle) const
      97                 :            :   {
      98         [ +  - ]:        455 :     core::shared_lock_guard const shared_lock(mCallbackRwLock);
      99         [ +  - ]:        455 :     auto findResult = mDefaultRssDynamicsCallbacks.find(objectType);
     100         [ +  - ]:        455 :     if (findResult != mDefaultRssDynamicsCallbacks.end())
     101                 :            :     {
     102         [ +  - ]:        455 :       return findResult->second(vehicle);
     103                 :            :     }
     104                 :          0 :     return ::ad::rss::world::RssDynamics();
     105                 :        455 :   }
     106                 :            : 
     107                 :            :   /** @brief Callback function type to query the RssConstellationConfiguration of a concrete vehicle constellation
     108                 :            :    * within the RssRouteChecker calculations.
     109                 :            :    *
     110                 :            :    *  @param[in] route The current route of the ego vehicle under consideration
     111                 :            :    *  @param[in] egoVehicleObject the ego vehicle object data of this constellation
     112                 :            :    *  @param[in] egoVehicle the ego vehicle instance pointer of this constellation
     113                 :            :    *  @param[in] vehicleObject the vehicle object data of this constellation
     114                 :            :    *  @param[in] vehicle the vehicle instance pointer of this constellation
     115                 :            :    *
     116                 :            :    * @returns RssConstellationConfiguration to be used for the constellation
     117                 :            :    */
     118                 :            :   using VehicleConstellationCallbackFunctionType
     119                 :            :     = std::function<::ad::rss::map::RssConstellationConfiguration(::ad::rss::map::RssRoute const &route,
     120                 :            :                                                                   ::ad::rss::map::RssObjectData const &egoVehicleObject,
     121                 :            :                                                                   OBJECT_INSTANCE_TYPE egoVehicle,
     122                 :            :                                                                   ::ad::rss::map::RssObjectData const &vehicleObject,
     123                 :            :                                                                   OBJECT_INSTANCE_TYPE vehicle)>;
     124                 :            : 
     125                 :            :   /** @brief Register a callback function to query the RssConstellationConfiguration of a concrete vehicle constellation
     126                 :            :    * within the RssRouteChecker calculations.
     127                 :            :    */
     128                 :        219 :   void registerVehicleConstellationCallback(VehicleConstellationCallbackFunctionType callback)
     129                 :            :   {
     130         [ +  - ]:        219 :     std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
     131         [ +  - ]:        219 :     mVehicleConstellationCallback = callback;
     132                 :        219 :   }
     133                 :            : 
     134                 :            :   /** @returns \c true if a callback function to query the RssConstellationConfiguration of a concrete vehicle
     135                 :            :    * constellation within the RssRouteChecker calculations is registered
     136                 :            :    */
     137                 :            :   bool isVehicleConstellationCallbackRegistered() const
     138                 :            :   {
     139                 :            :     core::shared_lock_guard const shared_lock(mCallbackRwLock);
     140                 :            :     return bool(mVehicleConstellationCallback);
     141                 :            :   }
     142                 :            : 
     143                 :            :   /** @brief Returns the RssConstellationConfiguration of a concrete vehicle constellation within the RssRouteChecker
     144                 :            :    * calculations
     145                 :            :    *  by calling the registered callback function (called by RssEgoVehicleAdapter)
     146                 :            :    *
     147                 :            :    *  @param[in] route The current route of the ego vehicle under consideration
     148                 :            :    *  @param[in] egoVehicleObject the ego vehicle object data of this constellation
     149                 :            :    *  @param[in] egoVehicle the ego vehicle instance pointer of this constellation
     150                 :            :    *  @param[in] vehicleObject the other vehicle object data of this constellation
     151                 :            :    *  @param[in] vehicle the other vehicle instance pointer of this constellation
     152                 :            :    *
     153                 :            :    * @returns RssConstellationConfiguration to be used for the constellation
     154                 :            :    */
     155                 :            :   ::ad::rss::map::RssConstellationConfiguration
     156                 :         44 :   getVehicleConstellation(::ad::rss::map::RssRoute const &route,
     157                 :            :                           ::ad::rss::map::RssObjectData const &egoVehicleObject,
     158                 :            :                           OBJECT_INSTANCE_TYPE egoVehicle,
     159                 :            :                           ::ad::rss::map::RssObjectData const &vehicleObject,
     160                 :            :                           OBJECT_INSTANCE_TYPE vehicle) const
     161                 :            :   {
     162         [ +  - ]:         44 :     core::shared_lock_guard const shared_lock(mCallbackRwLock);
     163         [ +  - ]:         44 :     if (bool(mVehicleConstellationCallback))
     164                 :            :     {
     165         [ +  - ]:         44 :       return mVehicleConstellationCallback(route, egoVehicleObject, egoVehicle, vehicleObject, vehicle);
     166                 :            :     }
     167                 :          0 :     return ::ad::rss::map::RssConstellationConfiguration();
     168                 :         44 :   }
     169                 :            : 
     170                 :            :   /** @brief Callback function type to query the RssConstellationConfiguration of a concrete pedestrian constellation
     171                 :            :    * within the RssRouteChecker calculations.
     172                 :            :    *
     173                 :            :    *  @param[in] route The current route of the ego vehicle under consideration
     174                 :            :    *  @param[in] egoVehicleObject the ego vehicle object data of this constellation
     175                 :            :    *  @param[in] egoVehicle the ego vehicle instance pointer of this constellation
     176                 :            :    *  @param[in] pedestrianObject the pedestrian object data of this constellation
     177                 :            :    *  @param[in] pedestrian the pedestrian instance pointer of this constellation
     178                 :            :    *
     179                 :            :    * @returns RssConstellationConfiguration to be used for the constellation
     180                 :            :    */
     181                 :            :   using PedestrianConstellationCallbackFunctionType
     182                 :            :     = std::function<::ad::rss::map::RssConstellationConfiguration(::ad::rss::map::RssRoute const &route,
     183                 :            :                                                                   ::ad::rss::map::RssObjectData const &egoVehicleObject,
     184                 :            :                                                                   OBJECT_INSTANCE_TYPE egoVehicle,
     185                 :            :                                                                   ::ad::rss::map::RssObjectData const &pedestrianObject,
     186                 :            :                                                                   OBJECT_INSTANCE_TYPE pedestrian)>;
     187                 :            : 
     188                 :            :   /** @brief Register a callback function to query the RssConstellationConfiguration of a concrete pedestrian
     189                 :            :    * constellation within the RssRouteChecker calculations.
     190                 :            :    */
     191                 :        219 :   void registerPedestrianConstellationCallback(PedestrianConstellationCallbackFunctionType callback)
     192                 :            :   {
     193         [ +  - ]:        219 :     std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
     194         [ +  - ]:        219 :     mPedestrianConstellationCallback = callback;
     195                 :        219 :   }
     196                 :            : 
     197                 :            :   /** @returns \c true if a callback function to query the RssConstellationConfiguration of a concrete pedestrian
     198                 :            :    * constellation within the RssRouteChecker calculations is registered
     199                 :            :    */
     200                 :            :   bool isPedestrianConstellationCallbackRegistered() const
     201                 :            :   {
     202                 :            :     core::shared_lock_guard const shared_lock(mCallbackRwLock);
     203                 :            :     return bool(mPedestrianConstellationCallback);
     204                 :            :   }
     205                 :            : 
     206                 :            :   /** @brief Returns the RssConstellationConfiguration of a concrete pedestrian constellation within the RssRouteChecker
     207                 :            :    * calculations
     208                 :            :    *  by calling the registered callback function (called by RssEgoVehicleAdapter)
     209                 :            :    *
     210                 :            :    *  @param[in] route The current route of the ego vehicle under consideration
     211                 :            :    *  @param[in] egoVehicleObject the ego vehicle object data of this constellation
     212                 :            :    *  @param[in] egoVehicle the ego vehicle instance pointer of this constellation
     213                 :            :    *  @param[in] pedestrianObject the pedestrian object data of this constellation
     214                 :            :    *  @param[in] pedestrian the pedestrian instance pointer of this constellation
     215                 :            :    *
     216                 :            :    * @returns RssConstellationConfiguration to be used for the constellation
     217                 :            :    */
     218                 :            :   ::ad::rss::map::RssConstellationConfiguration
     219                 :          0 :   getPedestrianConstellation(::ad::rss::map::RssRoute const &route,
     220                 :            :                              ::ad::rss::map::RssObjectData const &egoVehicleObject,
     221                 :            :                              OBJECT_INSTANCE_TYPE egoVehicle,
     222                 :            :                              ::ad::rss::map::RssObjectData const &pedestrianObject,
     223                 :            :                              OBJECT_INSTANCE_TYPE pedestrian) const
     224                 :            :   {
     225         [ #  # ]:          0 :     core::shared_lock_guard const shared_lock(mCallbackRwLock);
     226         [ #  # ]:          0 :     if (bool(mPedestrianConstellationCallback))
     227                 :            :     {
     228         [ #  # ]:          0 :       return mPedestrianConstellationCallback(route, egoVehicleObject, egoVehicle, pedestrianObject, pedestrian);
     229                 :            :     }
     230                 :          0 :     return ::ad::rss::map::RssConstellationConfiguration();
     231                 :          0 :   }
     232                 :            : 
     233                 :            :   /** @brief Callback function type to query the RssConstellationConfiguration of a concrete artificial object
     234                 :            :    * constellation within the RssRouteChecker calculations.
     235                 :            :    *
     236                 :            :    *  @param[in] route The current route of the ego vehicle under consideration
     237                 :            :    *  @param[in] egoVehicleObject the ego vehicle object data of this constellation
     238                 :            :    *  @param[in] egoVehicle the ego vehicle instance pointer of this constellation
     239                 :            :    *  @param[in] artificialObject the artificial object data of this constellation
     240                 :            :    *
     241                 :            :    * @returns RssConstellationConfiguration to be used for the constellation
     242                 :            :    */
     243                 :            :   using ArtificialObjectConstellationCallbackFunctionType = std::function<::ad::rss::map::RssConstellationConfiguration(
     244                 :            :     ::ad::rss::map::RssRoute const &route,
     245                 :            :     ::ad::rss::map::RssObjectData const &egoVehicleObject,
     246                 :            :     OBJECT_INSTANCE_TYPE egoVehicle,
     247                 :            :     ::ad::rss::map::RssObjectData const &artificialObject)>;
     248                 :            : 
     249                 :            :   /** @brief Register a callback function to query the RssConstellationConfiguration of a concrete artificial object
     250                 :            :    * constellation within the RssRouteChecker calculations.
     251                 :            :    */
     252                 :        219 :   void registerArtificialObjectConstellationCallback(ArtificialObjectConstellationCallbackFunctionType callback)
     253                 :            :   {
     254         [ +  - ]:        219 :     std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
     255         [ +  - ]:        219 :     mArtificialObjectConstellationCallback = callback;
     256                 :        219 :   }
     257                 :            : 
     258                 :            :   /** @returns \c true if a callback function to query the RssConstellationConfiguration of a concrete artificial object
     259                 :            :    * constellation within the RssRouteChecker calculations is registered
     260                 :            :    */
     261                 :            :   bool isArtificialObjectConstellationCallbackRegistered() const
     262                 :            :   {
     263                 :            :     core::shared_lock_guard const shared_lock(mCallbackRwLock);
     264                 :            :     return bool(mArtificialObjectConstellationCallback);
     265                 :            :   }
     266                 :            : 
     267                 :            :   /** @brief Returns the RssConstellationConfiguration of a concrete artificial object constellation within the
     268                 :            :    * RssRouteChecker calculations
     269                 :            :    *  by calling the registered callback function (called by RssEgoVehicleAdapter)
     270                 :            :    *  (can be used by derivatives of RssRouteChecker)
     271                 :            :    *
     272                 :            :    *  @param[in] route The current route of the ego vehicle under consideration
     273                 :            :    *  @param[in] egoVehicleObject the ego vehicle object data of this constellation
     274                 :            :    *  @param[in] egoVehicle the ego vehicle instance pointer of this constellation
     275                 :            :    *  @param[in] artificialObject the artificial object data of this constellation
     276                 :            :    *
     277                 :            :    * @returns RssConstellationConfiguration to be used for the constellation
     278                 :            :    */
     279                 :            :   ::ad::rss::map::RssConstellationConfiguration
     280                 :          4 :   getArtificialObjectConstellation(::ad::rss::map::RssRoute const &route,
     281                 :            :                                    ::ad::rss::map::RssObjectData const &egoVehicleObject,
     282                 :            :                                    OBJECT_INSTANCE_TYPE egoVehicle,
     283                 :            :                                    ::ad::rss::map::RssObjectData const &artificialObject) const
     284                 :            :   {
     285         [ +  - ]:          4 :     core::shared_lock_guard const shared_lock(mCallbackRwLock);
     286         [ +  - ]:          4 :     if (bool(mArtificialObjectConstellationCallback))
     287                 :            :     {
     288         [ +  - ]:          4 :       return mArtificialObjectConstellationCallback(route, egoVehicleObject, egoVehicle, artificialObject);
     289                 :            :     }
     290                 :          0 :     return ::ad::rss::map::RssConstellationConfiguration();
     291                 :          4 :   }
     292                 :            : 
     293                 :            :   /** @brief sets the current mode for respecting the road boundaries
     294                 :            :    */
     295                 :            :   void setRoadBoundariesMode(const ::ad::rss::map::RssAppendRoadBoundariesMode roadBoundariesMode)
     296                 :            :   {
     297                 :            :     mRssAppendRoadBoundariesMode = roadBoundariesMode;
     298                 :            :   }
     299                 :            : 
     300                 :            :   /** @returns the current mode for respecting the road boundaries (called by RssEgoVehicleAdapter)
     301                 :            :    */
     302                 :         28 :   ::ad::rss::map::RssAppendRoadBoundariesMode getRoadBoundariesMode() const
     303                 :            :   {
     304                 :         28 :     return mRssAppendRoadBoundariesMode;
     305                 :            :   }
     306                 :            : 
     307                 :            :   /** @brief sets the current green traffic lights to be considered
     308                 :            :    */
     309                 :            :   void setCurrentGreenTrafficLights(::ad::map::landmark::LandmarkIdSet const &currentGreenTrafficLights)
     310                 :            :   {
     311                 :            :     mCurrentGreenTrafficLights = currentGreenTrafficLights;
     312                 :            :   }
     313                 :            : 
     314                 :            :   /** @returns the current green traffic lights to be considered (called by RssEgoVehicleAdapter)
     315                 :            :    */
     316                 :        240 :   ::ad::map::landmark::LandmarkIdSet getCurrentGreenTrafficLights() const
     317                 :            :   {
     318                 :        240 :     return mCurrentGreenTrafficLights;
     319                 :            :   }
     320                 :            : 
     321                 :            :   /** @brief sets the distance interpreted as a jump of a vehicle triggering new route calucations
     322                 :            :    */
     323                 :        219 :   void setPositionJumpTraveledDistance(ad::physics::Distance const positionJumpTraveledDistance)
     324                 :            :   {
     325                 :        219 :     mPositionJumpTraveledDistance = positionJumpTraveledDistance;
     326                 :        219 :   }
     327                 :            : 
     328                 :            :   /** @brief gets the distance interpreted as a jump of a vehicle triggering new route calucations
     329                 :            :    */
     330                 :         16 :   ad::physics::Distance getPositionJumpTraveledDistance() const
     331                 :            :   {
     332                 :         16 :     return mPositionJumpTraveledDistance;
     333                 :            :   }
     334                 :            : 
     335                 :            :   /*! @brief Sets the distance below which a detailed analysis becomes necessary regardless of the objects stopping
     336                 :            :    * distance estimates.
     337                 :            :    *
     338                 :            :    *  Far away object might not be analyzed in detail, if the stopping distances allow it
     339                 :            :    */
     340                 :            :   void setMinimumDistanceToObjectsThatHaveToBeAnalyzed(
     341                 :            :     ad::physics::Distance const minimumDistanceToObjectsThatHaveToBeAnalyzed)
     342                 :            :   {
     343                 :            :     mMinimumDistanceToObjectsThatHaveToBeAnalyzed = minimumDistanceToObjectsThatHaveToBeAnalyzed;
     344                 :            :   }
     345                 :            : 
     346                 :            :   /*! @brief Defines the distance below which a detailed analysis becomes necessary regardless of the objects stopping
     347                 :            :    * distance estimates.
     348                 :            :    *
     349                 :            :    *  Far away object might not be analyzed in detail, if the stopping distances allow it
     350                 :            :    */
     351                 :         28 :   ad::physics::Distance getMinimumDistanceToObjectsThatHaveToBeAnalyzed() const
     352                 :            :   {
     353                 :         28 :     return mMinimumDistanceToObjectsThatHaveToBeAnalyzed;
     354                 :            :   }
     355                 :            : 
     356                 :            :   void setConsiderPositionConfidence(bool const consider_position_confidence)
     357                 :            :   {
     358                 :            :     mConsiderPositionConfidence = consider_position_confidence;
     359                 :            :   }
     360                 :        481 :   bool getConsiderPositionConfidence()
     361                 :            :   {
     362                 :        481 :     return mConsiderPositionConfidence;
     363                 :            :   }
     364                 :            : 
     365                 :            :   /** @brief sets the current log level
     366                 :            :    */
     367                 :            :   void setLogLevel(const spdlog::level::level_enum logLevel)
     368                 :            :   {
     369                 :            :     ::ad::rss::core::getLogger()->set_level(logLevel);
     370                 :            :     ::ad::rss::map::getLogger()->set_level(logLevel);
     371                 :            :   }
     372                 :            : 
     373                 :            :   /** @brief sets the current map log level
     374                 :            :    */
     375                 :            :   void setMapLogLevel(const spdlog::level::level_enum mapLogLevel)
     376                 :            :   {
     377                 :            :     ::ad::map::access::getLogger()->set_level(mapLogLevel);
     378                 :            :   }
     379                 :            : 
     380                 :            :   /** @brief RssRoutingTargetOperation type from RssVehicleAdapter
     381                 :            :    */
     382                 :            :   using RssRoutingTargetOperation = typename RssVehicleAdapter<OBJECT_INSTANCE_TYPE>::RssRoutingTargetOperation;
     383                 :            : 
     384                 :            :   /** @brief RssRoutingTargetCommand type from RssVehicleAdapter
     385                 :            :    */
     386                 :            :   using RssRoutingTargetCommand = typename RssVehicleAdapter<OBJECT_INSTANCE_TYPE>::RssRoutingTargetCommand;
     387                 :            : 
     388                 :            :   /** @brief sets the the current active routing targets (called by RssVehicleAdapter)
     389                 :            :    *
     390                 :            :    * @param[in] vehicleId object id of the vehicle reporting its active routing targets
     391                 :            :    * @param[in] activeRoutingTargets the active routing targets
     392                 :            :    */
     393                 :         26 :   void setActiveRoutingTargets(ad::rss::world::ObjectId vehicleId,
     394                 :            :                                ::ad::map::point::ENUPointList const &activeRoutingTargets)
     395                 :            :   {
     396         [ +  - ]:         26 :     std::lock_guard<std::mutex> const lock(mRoutingLock);
     397   [ +  -  +  - ]:         26 :     mActiveRoutingTargets[vehicleId] = activeRoutingTargets;
     398                 :         26 :   }
     399                 :            : 
     400                 :            :   /** @returns the the current active routing targets of the respective vehicle
     401                 :            :    *
     402                 :            :    * @param[in] vehicleId object id of the vehicle reporting its active routing targets
     403                 :            :    */
     404                 :            :   ::ad::map::point::ENUPointList getActiveRoutingTargets(ad::rss::world::ObjectId vehicleId) const
     405                 :            :   {
     406                 :            :     std::lock_guard<std::mutex> const lock(mRoutingLock);
     407                 :            :     auto findResult = mActiveRoutingTargets.find(vehicleId);
     408                 :            :     if (findResult != mActiveRoutingTargets.end())
     409                 :            :     {
     410                 :            :       return findResult->second;
     411                 :            :     }
     412                 :            :     return ::ad::map::point::ENUPointList();
     413                 :            :   }
     414                 :            : 
     415                 :            :   /**  @brief appends a list of routing targets to the current routing target list of the respective vehicle
     416                 :            :    *
     417                 :            :    * @param[in] vehicleId object id of the vehicle to append the routing targets
     418                 :            :    * @param[in] routingTargetsToAppend the routing targets to append
     419                 :            :    */
     420                 :            :   void appendRoutingTargets(ad::rss::world::ObjectId vehicleId,
     421                 :            :                             ::ad::map::point::ENUPointList const &routingTargetsToAppend)
     422                 :            :   {
     423                 :            :     std::lock_guard<std::mutex> const lock(mRoutingLock);
     424                 :            :     RssRoutingTargetOperation operation;
     425                 :            :     operation.command = RssRoutingTargetCommand::AppendTargets;
     426                 :            :     operation.routingTargets = routingTargetsToAppend;
     427                 :            :     auto const insertResult = mRoutingTargetOperations.insert(std::make_pair(vehicleId, operation));
     428                 :            :     if (!insertResult.second)
     429                 :            :     {
     430                 :            :       insertResult.first->second.routingTargets.insert(
     431                 :            :         insertResult.first->second.routingTargets.end(), routingTargetsToAppend.begin(), routingTargetsToAppend.end());
     432                 :            :     }
     433                 :            :   }
     434                 :            : 
     435                 :            :   /**  @brief replaces the current routing targets of the respective vehicle
     436                 :            :    *
     437                 :            :    * @param[in] vehicleId object id of the vehicle to replace the routing targets
     438                 :            :    * @param[in] newRoutingTargets the new routing targets to be applied
     439                 :            :    */
     440                 :        152 :   void replaceRoutingTargets(ad::rss::world::ObjectId vehicleId,
     441                 :            :                              ::ad::map::point::ENUPointList const &newRoutingTargets)
     442                 :            :   {
     443         [ +  - ]:        152 :     std::lock_guard<std::mutex> const lock(mRoutingLock);
     444                 :        152 :     RssRoutingTargetOperation operation;
     445                 :        152 :     operation.command = RssRoutingTargetCommand::ReplaceTargets;
     446         [ +  - ]:        152 :     operation.routingTargets = newRoutingTargets;
     447   [ +  -  +  - ]:        152 :     auto const insertResult = mRoutingTargetOperations.insert(std::make_pair(vehicleId, operation));
     448         [ -  + ]:        152 :     if (!insertResult.second)
     449                 :            :     {
     450         [ #  # ]:          0 :       insertResult.first->second = operation;
     451                 :            :     }
     452                 :        152 :   }
     453                 :            : 
     454                 :            :   /** @returns the current routing target operation of the respective vehicle (called by RssVehicleAdapter)
     455                 :            :    *
     456                 :            :    * @param[in] vehicleId object id of the vehicle to apply the routing target operation
     457                 :            :    */
     458                 :        178 :   RssRoutingTargetOperation getCurrentRoutingTargetOperation(ad::rss::world::ObjectId vehicleId)
     459                 :            :   {
     460                 :        178 :     RssRoutingTargetOperation resultOperation;
     461         [ +  - ]:        178 :     std::lock_guard<std::mutex> const lock(mRoutingLock);
     462         [ +  - ]:        178 :     auto findResult = mRoutingTargetOperations.find(vehicleId);
     463         [ +  + ]:        178 :     if (findResult != mRoutingTargetOperations.end())
     464                 :            :     {
     465         [ +  - ]:        152 :       resultOperation = findResult->second;
     466         [ +  - ]:        152 :       mRoutingTargetOperations.erase(vehicleId);
     467                 :            :     }
     468                 :        356 :     return resultOperation;
     469                 :        178 :   }
     470                 :            : 
     471                 :            :   /**  @brief drop the current route of the respective vehicle
     472                 :            :    *
     473                 :            :    *  Enforces a new route calculation of the vehicle by replacing the routing targets with the current active ones.
     474                 :            :    *
     475                 :            :    * @param[in] vehicleId object id of the vehicle to replace the routing targets
     476                 :            :    */
     477                 :            :   void dropRoute(ad::rss::world::ObjectId vehicleId)
     478                 :            :   {
     479                 :            :     replaceRoutingTargets(vehicleId, getActiveRoutingTargets(vehicleId));
     480                 :            :   }
     481                 :            : 
     482                 :            :   /** @brief erase all data of a vehicle from internal lists (called by ~RssVehicleAdapter)
     483                 :            :    */
     484                 :         10 :   void dropVehicle(ad::rss::world::ObjectId vehicleId)
     485                 :            :   {
     486         [ +  - ]:         10 :     std::lock_guard<std::mutex> const lock(mRoutingLock);
     487         [ +  - ]:         10 :     mRoutingTargetOperations.erase(vehicleId);
     488         [ +  - ]:         10 :     mActiveRoutingTargets.erase(vehicleId);
     489                 :         10 :   }
     490                 :            : 
     491                 :            : private:
     492                 :            :   // one writer mutex for the callbacks of the class should be sufficient
     493                 :            :   // vast majority of concurrent access is of shared nature
     494                 :            :   mutable std::shared_timed_mutex mCallbackRwLock;
     495                 :            : 
     496                 :            :   //// the registered callback functions
     497                 :            :   std::map<::ad::rss::world::ObjectType, DefaultRssDynamicsCallbackFunctionType> mDefaultRssDynamicsCallbacks;
     498                 :            : 
     499                 :            :   VehicleConstellationCallbackFunctionType mVehicleConstellationCallback;
     500                 :            :   PedestrianConstellationCallbackFunctionType mPedestrianConstellationCallback;
     501                 :            :   ArtificialObjectConstellationCallbackFunctionType mArtificialObjectConstellationCallback;
     502                 :            : 
     503                 :            :   ::ad::rss::map::RssAppendRoadBoundariesMode mRssAppendRoadBoundariesMode{
     504                 :            :     ::ad::rss::map::RssAppendRoadBoundariesMode::Off};
     505                 :            :   ::ad::map::landmark::LandmarkIdSet mCurrentGreenTrafficLights;
     506                 :            : 
     507                 :            :   mutable std::mutex mRoutingLock;
     508                 :            : 
     509                 :            :   std::map<ad::rss::world::ObjectId, ::ad::map::point::ENUPointList> mActiveRoutingTargets;
     510                 :            :   std::map<ad::rss::world::ObjectId, RssRoutingTargetOperation> mRoutingTargetOperations;
     511                 :            : 
     512                 :            :   ad::physics::Distance mPositionJumpTraveledDistance{20.};
     513                 :            :   ad::physics::Distance mMinimumDistanceToObjectsThatHaveToBeAnalyzed{RssWorldModelCreation::cMinConnectedRouteLength};
     514                 :            :   bool mConsiderPositionConfidence{true};
     515                 :            : };
     516                 :            : 
     517                 :            : } // namespace map
     518                 :            : } // namespace rss
     519                 :            : } // namespace ad