Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2019-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "ad/map/match/AdMapMatching.hpp"
      10                 :            : #include "ad/rss/map/Logging.hpp"
      11                 :            : #include "ad/rss/map/RssObjectConversion.hpp"
      12                 :            : #include "ad/rss/map/RssRouteCheckerData.hpp"
      13                 :            : 
      14                 :            : namespace ad {
      15                 :            : namespace rss {
      16                 :            : namespace map {
      17                 :            : 
      18                 :            : template <class OBJECT_INSTANCE_TYPE> class ParallelObjectUpdater
      19                 :            : {
      20                 :            : public:
      21                 :         11 :   ParallelObjectUpdater(std::chrono::system_clock::time_point const &currentTime,
      22                 :            :                         ::ad::physics::Distance const vehicleSamplingDistance,
      23                 :            :                         ::ad::physics::Distance const pedestrianSamplingDistance,
      24                 :            :                         ::ad::physics::Distance const artificialObjectSamplingDistance,
      25                 :            :                         ::ad::map::lane::LaneIdSet const &mapAreaLanes)
      26                 :         11 :     : mCurrentTime(currentTime)
      27                 :         11 :     , mVehicleSamplingDistance(vehicleSamplingDistance)
      28                 :         11 :     , mPedestrianSamplingDistance(pedestrianSamplingDistance)
      29                 :         11 :     , mArtificialObjectSamplingDistance(artificialObjectSamplingDistance)
      30                 :         11 :     , mConservativeMaxStoppingDistance(0.)
      31                 :         22 :     , mMaxVehicleLength(0.)
      32                 :            :   {
      33         [ +  - ]:         11 :     mMapMatching.setRelevantLanes(mapAreaLanes);
      34                 :         11 :   }
      35                 :            : 
      36                 :            :   ParallelObjectUpdater(ParallelObjectUpdater const &other) = delete;
      37                 :            :   ParallelObjectUpdater &operator=(ParallelObjectUpdater const &other) = delete;
      38                 :            : 
      39                 :          0 :   void operator()(typename RssRouteCheckerPedestrianDataMap<OBJECT_INSTANCE_TYPE>::value_type &pedestrianToUpdate) const
      40                 :            :   {
      41         [ #  # ]:          0 :     if (pedestrianToUpdate.second.mActive)
      42                 :            :     {
      43                 :          0 :       auto pedestrianAdapter = pedestrianToUpdate.second.getAdapter();
      44         [ #  # ]:          0 :       if (!bool(pedestrianAdapter))
      45                 :            :       {
      46         [ #  # ]:          0 :         throw std::runtime_error("RssRouteChecker::ParallelObjectUpdater>> pedestrian adapter invalid");
      47                 :            :       }
      48                 :            :       pedestrianToUpdate.second.mRssObjectData
      49         [ #  # ]:          0 :         = getRssObjectData(pedestrianAdapter, mPedestrianSamplingDistance, pedestrianAdapter->getObjectType());
      50         [ #  # ]:          0 :       pedestrianToUpdate.second.mClearObjectHistory = !pedestrianAdapter->isObjectIdUniqueOverTime();
      51   [ #  #  #  # ]:          0 :       getLogger()->trace("ParallelObjectUpdater::updatePedestrian[{}] {}",
      52         [ #  # ]:          0 :                          pedestrianAdapter->getObjectId(),
      53                 :          0 :                          pedestrianToUpdate.second.mRssObjectData);
      54                 :          0 :     }
      55                 :          0 :   }
      56                 :            : 
      57                 :          2 :   void operator()(
      58                 :            :     typename RssRouteCheckerArtificialObjectDataMap<OBJECT_INSTANCE_TYPE>::value_type &artificialObjectToUpdate) const
      59                 :            :   {
      60         [ +  - ]:          2 :     if (artificialObjectToUpdate.second.mActive)
      61                 :            :     {
      62                 :          2 :       auto artificialObjectAdapter = artificialObjectToUpdate.second.getAdapter();
      63         [ -  + ]:          2 :       if (!bool(artificialObjectAdapter))
      64                 :            :       {
      65         [ #  # ]:          0 :         throw std::runtime_error("RssRouteChecker::ParallelObjectUpdater>> artificialObject adapter invalid");
      66                 :            :       }
      67         [ +  - ]:          2 :       artificialObjectToUpdate.second.mRssObjectData = getRssObjectData(
      68                 :          2 :         artificialObjectAdapter, mArtificialObjectSamplingDistance, artificialObjectAdapter->getObjectType());
      69         [ +  - ]:          2 :       artificialObjectToUpdate.second.mClearObjectHistory = !artificialObjectAdapter->isObjectIdUniqueOverTime();
      70                 :            : 
      71   [ +  -  +  - ]:          4 :       getLogger()->trace("ParallelObjectUpdater::updateArtificialObject[{}] {}",
      72         [ +  - ]:          4 :                          artificialObjectAdapter->getObjectId(),
      73                 :          2 :                          artificialObjectToUpdate.second.mRssObjectData);
      74                 :          2 :     }
      75                 :          2 :   }
      76                 :            : 
      77                 :          9 :   void operator()(typename RssRouteCheckerVehicleDataMap<OBJECT_INSTANCE_TYPE>::value_type &vehicleToUpdate) const
      78                 :            :   {
      79         [ +  - ]:          9 :     if (vehicleToUpdate.second.mActive)
      80                 :            :     {
      81                 :          9 :       auto vehicleAdapter = vehicleToUpdate.second.getAdapter();
      82         [ -  + ]:          9 :       if (!bool(vehicleAdapter))
      83                 :            :       {
      84         [ #  # ]:          0 :         throw std::runtime_error("RssRouteChecker::ParallelObjectUpdater>> vehicle adapter invalid");
      85                 :            :       }
      86         [ +  - ]:          9 :       updateVehicle(vehicleToUpdate.second, vehicleAdapter->getObjectType());
      87                 :          9 :     }
      88                 :          9 :   }
      89                 :            : 
      90                 :         17 :   void operator()(typename RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE>::value_type &egoVehicleToUpdate) const
      91                 :            :   {
      92         [ +  - ]:         17 :     if (egoVehicleToUpdate.second.mActive)
      93                 :            :     {
      94                 :         17 :       auto egoVehicleAdapter = egoVehicleToUpdate.second.getAdapter();
      95         [ -  + ]:         17 :       if (!bool(egoVehicleAdapter))
      96                 :            :       {
      97         [ #  # ]:          0 :         throw std::runtime_error("RssRouteChecker::ParallelObjectUpdater>> ego_vehicle adapter invalid");
      98                 :            :       }
      99         [ +  - ]:         17 :       updateVehicle(egoVehicleToUpdate.second, egoVehicleAdapter->getObjectType());
     100                 :         17 :     }
     101                 :         17 :   }
     102                 :            : 
     103                 :         28 :   RssObjectData getRssObjectData(typename RssObjectAdapter<OBJECT_INSTANCE_TYPE>::Ptr object,
     104                 :            :                                  physics::Distance const sampling_distance,
     105                 :            :                                  ::ad::rss::world::ObjectType const object_type) const
     106                 :            :   {
     107                 :         28 :     RssObjectData rssObjectData;
     108                 :         28 :     rssObjectData.id = object->getObjectId();
     109                 :         28 :     rssObjectData.type = object_type;
     110                 :         28 :     rssObjectData.last_update = mCurrentTime;
     111         [ +  - ]:         28 :     rssObjectData.match_object = object->getMatchedObject(mMapMatching, sampling_distance);
     112                 :         28 :     rssObjectData.speed_range = object->getSpeedRange();
     113                 :         28 :     rssObjectData.yaw_rate = object->getYawRate();
     114         [ +  - ]:         28 :     rssObjectData.rss_dynamics = object->getDefaultRssDynamics();
     115                 :         28 :     rssObjectData.steering_angle = object->getSteeringAngle();
     116                 :         28 :     return rssObjectData;
     117                 :          0 :   }
     118                 :            : 
     119                 :         26 :   void updateVehicle(RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE> &vehicle,
     120                 :            :                      ::ad::rss::world::ObjectType const object_type) const
     121                 :            :   {
     122                 :         26 :     auto const lastCenterPoint = vehicle.mRssObjectData.match_object.enu_position.center_point;
     123                 :         26 :     auto vehicleAdapter = vehicle.getAdapter();
     124         [ -  + ]:         26 :     if (!bool(vehicleAdapter))
     125                 :            :     {
     126         [ #  # ]:          0 :       throw std::runtime_error("RssRouteChecker::ParallelObjectUpdater>> vehicle adapter invalid");
     127                 :            :     }
     128         [ +  - ]:         26 :     vehicle.mRssObjectData = getRssObjectData(vehicleAdapter, mVehicleSamplingDistance, object_type);
     129         [ +  - ]:         26 :     vehicle.mClearObjectHistory = !vehicleAdapter->isObjectIdUniqueOverTime();
     130   [ +  -  +  - ]:         52 :     getLogger()->trace(
     131         [ +  - ]:         52 :       "ParallelObjectUpdater::updateVehicle[{}] {}", vehicleAdapter->getObjectId(), vehicle.mRssObjectData);
     132                 :            : 
     133   [ +  -  -  + ]:         26 :     if ((::ad::rss::world::ObjectType::OtherObject == vehicle.mRssObjectData.type) || vehicle.mClearObjectHistory)
     134                 :            :     {
     135                 :            :       // OtherObject types are not taking routes into consideration, because using unstructured evaluation
     136                 :            :       // explictly cleanup requests also clear routes
     137                 :          0 :       vehicle.mRoutes.clear();
     138                 :            :       // clear routing targets, too
     139                 :          0 :       vehicle.mRoutingTargets.clear();
     140                 :            :     }
     141   [ +  -  +  +  :         26 :     else if (isValid(lastCenterPoint, false) && (!vehicle.mRoutes.empty()))
             +  -  +  + ]
     142                 :            :     {
     143                 :            :       auto const traveledDistance
     144         [ +  - ]:         16 :         = ad::map::point::distance(vehicle.mRssObjectData.match_object.enu_position.center_point, lastCenterPoint);
     145                 :            : 
     146                 :            :       // check for bigger position jumps of the vehicle
     147   [ +  -  +  -  :         16 :       if (traveledDistance > vehicleAdapter->getPositionJumpTraveledDistance())
                   -  + ]
     148                 :            :       {
     149   [ #  #  #  # ]:          0 :         getLogger()->warn("ParallelObjectUpdater::updateVehicle[{}] Jump of {} in vehicle position detected {} -> {}, "
     150                 :            :                           "larger than allowed distance {}. Force new route!",
     151                 :          0 :                           vehicle.mRssObjectData.id,
     152                 :            :                           traveledDistance,
     153                 :            :                           lastCenterPoint,
     154         [ #  # ]:          0 :                           vehicle.mRssObjectData.match_object.enu_position.center_point,
     155         [ #  # ]:          0 :                           vehicleAdapter->getPositionJumpTraveledDistance());
     156                 :          0 :         vehicle.mRoutes.clear();
     157                 :            :       }
     158                 :            :     }
     159                 :            : 
     160         [ +  - ]:         26 :     updateConservativeMaxStoppingDistance(vehicle);
     161                 :         26 :   }
     162                 :            : 
     163                 :         11 :   ad::physics::Distance getMinRoutePreviewDistance()
     164                 :            :   {
     165                 :         11 :     uint32_t currentMaxStoppingDistance = mConservativeMaxStoppingDistance;
     166                 :            :     // get rid of small fluctuations
     167                 :         11 :     currentMaxStoppingDistance /= 10u;
     168                 :         11 :     currentMaxStoppingDistance++;
     169                 :         11 :     currentMaxStoppingDistance *= 10;
     170                 :         11 :     ::ad::physics::Distance result(2 * currentMaxStoppingDistance);
     171                 :            :     // ensure to be larger than min connected route length
     172         [ +  - ]:         11 :     result = std::max(result, RssWorldModelCreation::cMinConnectedRouteLength);
     173                 :            :     // ensure to consider the max vehicle length to prevent from recalculations
     174         [ +  - ]:         11 :     result += ::ad::physics::Distance(mMaxVehicleLength);
     175                 :            :     // now the route preview distance should be exactly what is required for
     176                 :            :     // world model creation, except for changes due to individualized RssDynamics
     177                 :            :     // of the situations
     178                 :            :     // -> this has an actual positive effect on runtime
     179                 :         11 :     return result;
     180                 :            :   }
     181                 :            : 
     182                 :            : private:
     183                 :            :   // mConservativeMaxStoppingDistance is mutable and we ensure safe multi-threaded update
     184                 :         26 :   void updateConservativeMaxStoppingDistance(RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE> &vehicle) const
     185                 :            :   {
     186                 :         26 :     ::ad::physics::Distance conservativeMinStoppingDistance;
     187         [ +  - ]:         26 :     if (RssObjectConversion::calculateConservativeMinStoppingDistance(vehicle.mRssObjectData.id,
     188                 :         26 :                                                                       vehicle.mRssObjectData.speed_range.maximum,
     189         [ +  - ]:         26 :                                                                       vehicle.mRssObjectData.rss_dynamics,
     190                 :            :                                                                       conservativeMinStoppingDistance))
     191                 :            :     {
     192                 :            :       // lock-free update of mConservativeMaxStoppingDistance
     193                 :         26 :       auto const conservativeMinStoppingDistance_uint
     194                 :         26 :         = static_cast<uint32_t>(conservativeMinStoppingDistance.mDistance);
     195                 :         26 :       uint32_t currentMaxStoppingDistance = mConservativeMaxStoppingDistance;
     196         [ +  + ]:         66 :       while (conservativeMinStoppingDistance_uint > currentMaxStoppingDistance)
     197                 :            :       {
     198                 :         40 :         mConservativeMaxStoppingDistance.compare_exchange_weak(currentMaxStoppingDistance,
     199                 :            :                                                                conservativeMinStoppingDistance_uint);
     200                 :            :       }
     201                 :            :     }
     202                 :            :     // lock-free update of mConservativeMaxStoppingDistance
     203         [ +  - ]:         26 :     auto const vehicleLength_uint = static_cast<uint32_t>(vehicle.getAdapter()->getDimensions().length.mDistance);
     204                 :         26 :     uint32_t currentMaxVehicleLength = mMaxVehicleLength;
     205         [ +  + ]:         48 :     while (vehicleLength_uint > currentMaxVehicleLength)
     206                 :            :     {
     207                 :         22 :       mMaxVehicleLength.compare_exchange_weak(currentMaxVehicleLength, vehicleLength_uint);
     208                 :            :     }
     209                 :         26 :   }
     210                 :            : 
     211                 :            :   std::chrono::system_clock::time_point const mCurrentTime;
     212                 :            :   ::ad::map::match::AdMapMatching mMapMatching;
     213                 :            :   ::ad::physics::Distance const mVehicleSamplingDistance;
     214                 :            :   ::ad::physics::Distance const mPedestrianSamplingDistance;
     215                 :            :   ::ad::physics::Distance const mArtificialObjectSamplingDistance;
     216                 :            :   mutable std::atomic_uint32_t mConservativeMaxStoppingDistance;
     217                 :            :   mutable std::atomic_uint32_t mMaxVehicleLength;
     218                 :            : };
     219                 :            : 
     220                 :            : } // namespace map
     221                 :            : } // namespace rss
     222                 :            : } // namespace ad