RssRouteChecker.hpp

Go to the documentation of this file.

// ----------------- BEGIN LICENSE BLOCK ---------------------------------
//
// Copyright (C) 2019-2022 Intel Corporation
//
// SPDX-License-Identifier: LGPL-2.1-only
//
// ----------------- END LICENSE BLOCK -----------------------------------
#pragma once
 
#include "ad/map/lane/MapAreaOperation.hpp"
#include "ad/map/route/Planning.hpp"
#include "ad/rss/map/ConnectingRoutesCache.hpp"
#include "ad/rss/map/RssObjectResultList.hpp"
#include "ad/rss/map/RssRouteCheckerData.hpp"
 
namespace ad {
namespace rss {
namespace map {
 
template <class OBJECT_INSTANCE_TYPE> class RssRouteChecker
{
public:
  typedef std::shared_ptr<RssRouteChecker> Ptr;
 
  typedef std::shared_ptr<RssRouteChecker const> ConstPtr;
 
  RssRouteChecker(::ad::physics::Duration const dropOutdatedObjecstDuration)
    : mDropOutdatedObjectsDuration(dropOutdatedObjecstDuration)
    , mTimeIndex(0u)
    , mArtificialObjectSamplingDistance(1.)
    , mArtificialObjectsInjectionCallback(nullptr)
  {
  }
 
  virtual ~RssRouteChecker() = default;
 
  using ArtificialObjectInjectionCallbackFunctionType
    = std::function<RssArtificialObjectAdapterList<OBJECT_INSTANCE_TYPE>(
      RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE> const &egoVehicleMap,
      RssRouteCheckerVehicleDataMap<OBJECT_INSTANCE_TYPE> const &vehicleMap,
      RssRouteCheckerPedestrianDataMap<OBJECT_INSTANCE_TYPE> const &pedestrianMap)>;
 
  void registerArtificialObjectInjectionCallback(
    ::ad::physics::Distance const artificialObjectSamplingDistance,
    ArtificialObjectInjectionCallbackFunctionType artificialObjectsInjectionCallback);
 
  using CalculateTrajectorySetsCallbackFunctionType
    = std::function<bool(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &egoVehicle,
                         ::ad::rss::map::RssRouteId const &routeId,
                         ::ad::rss::core::RelativeConstellation const &constellation,
                         ::ad::rss::world::ObjectId const &objectId,
                         ::ad::rss::core::RelativeObjectState const &vehicleState,
                         ::ad::geometry::Polygon &brakePolygon,
                         ::ad::geometry::Polygon &continueForwardPolygon)>;
 
  void
  registerCalculateTrajectorySetsCallback(world::ObjectType objectType,
                                          CalculateTrajectorySetsCallbackFunctionType calculateTrajectorySetsCallback);
 
  virtual RssObjectResultList
  checkObjects(std::chrono::system_clock::time_point const &currentTime,
               ::ad::physics::Distance const vehicleSamplingDistance,
               RssEgoVehicleAdapterList<OBJECT_INSTANCE_TYPE> &egoVehicles,
               RssVehicleAdapterList<OBJECT_INSTANCE_TYPE> &vehicles,
               ::ad::physics::Distance const pedestrianSamplingDistance,
               RssPedestrianAdapterList<OBJECT_INSTANCE_TYPE> &pedestrians,
               typename RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::RouteExtensionMode routeExtensionMode);
 
  void restrictOperationToMapArea(::ad::map::lane::LaneIdSet const &lanesToConsider);
 
  bool restrictOperationToMapArea(::ad::map::point::BoundingSphere const &bounding_sphere);
 
  bool restrictOperationToMapArea(std::vector<ad::map::lane::MapAreaPredictionStartPoint> const &predictionStartPoints);
 
  bool restrictOperationToIntersectionArea(::ad::map::point::ENUPoint const &intersectionPoint,
                                           ::ad::physics::Distance const &intersectionDistance);
 
  void clearOperationToMapAreaRestriction();
  bool isEgoVehicleAvailable(::ad::rss::world::ObjectId ego_object_id)
  {
    return mEgoVehicleMap.find(ego_object_id) != mEgoVehicleMap.end();
  }
 
  bool isEgoVehicleActive(::ad::rss::world::ObjectId ego_object_id)
  {
    auto findResult = mEgoVehicleMap.find(ego_object_id);
    if (findResult != mEgoVehicleMap.end())
    {
      return findResult->second.mActive;
    }
    else
    {
      return false;
    }
  }
 
protected:
  virtual void objectDataUpdated(RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE> &egoVehicleMap)
  {
    (void)egoVehicleMap;
  }
  virtual void objectDataUpdated(RssRouteCheckerVehicleDataMap<OBJECT_INSTANCE_TYPE> &vehicleMap)
  {
    (void)vehicleMap;
  }
  virtual void objectDataUpdated(RssRouteCheckerPedestrianDataMap<OBJECT_INSTANCE_TYPE> &pedestrianMap)
  {
    (void)pedestrianMap;
  }
  virtual void objectDataUpdated(RssRouteCheckerArtificialObjectDataMap<OBJECT_INSTANCE_TYPE> &artificialObjectMap)
  {
    (void)artificialObjectMap;
  }
  virtual void routesUpdated(RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE> &egoVehicleMap)
  {
    (void)egoVehicleMap;
  }
  virtual void routesUpdated(RssRouteCheckerVehicleDataMap<OBJECT_INSTANCE_TYPE> &vehicleMap)
  {
    (void)vehicleMap;
  }
  virtual void situationCreated(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &ego_vehicle,
                                RssRouteCheckResult &rssRouteCheckResult)
  {
    (void)ego_vehicle;
    (void)rssRouteCheckResult;
  }
  virtual void situationsCreated(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &ego_vehicle,
                                 RssObjectResult &rssObjectResult)
  {
    (void)ego_vehicle;
    (void)rssObjectResult;
  }
  virtual void situationsCreated(RssObjectResultList &objectResultList)
  {
    (void)objectResultList;
  }
  virtual void checksPerformed(RssObjectResultList &objectResultList)
  {
    (void)objectResultList;
  }
private:
  template <class Data, class Adapter>
  void updateObjectMap(std::chrono::system_clock::time_point const &currentTime,
                       std::map<::ad::rss::world::ObjectId, Data> &objectMap,
                       std::vector<typename Adapter::Ptr> const &activeObjectAdapterList);
 
  void updateRssCheckers();
 
  RssRouteCheckResult createSituationForRoute(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &ego_vehicle,
                                              RssRoute const &rss_route);
 
  RssObjectResultList createSituations();
 
  void appendObjectsToResults(RssObjectResultList &objectResultList);
 
  void updateRssCheckerCallbacks(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &egoVehicle,
                                 RssRouteCheckMap::iterator routeCheckerMapIter);
 
  void updateRssCheckerCallbacksLocked(RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &egoVehicle,
                                       RssRouteCheckMap::iterator routeCheckerMapIter);
 
  bool callCalculateTrajectorySetsCallback(CalculateTrajectorySetsCallbackFunctionType callback,
                                           ::ad::rss::world::ObjectId const &egoVehicleId,
                                           ::ad::rss::map::RssRouteId const &routeId,
                                           ::ad::rss::core::RelativeConstellation const &constellation,
                                           ::ad::rss::world::ObjectId const &objectId,
                                           ::ad::rss::core::RelativeObjectState const &vehicleState,
                                           ::ad::geometry::Polygon &brakePolygon,
                                           ::ad::geometry::Polygon &continueForwardPolygon);
 
  ::ad::physics::Duration const mDropOutdatedObjectsDuration;
  ::ad::rss::world::TimeIndex mTimeIndex;
  ::ad::map::lane::LaneIdSet mMapAreaLanes;
  ConnectingRoutesCache mConnectingRoutesCache;
 
  RssRouteCheckerPedestrianDataMap<OBJECT_INSTANCE_TYPE> mPedestrianMap;
  RssRouteCheckerVehicleDataMap<OBJECT_INSTANCE_TYPE> mVehicleMap;
  RssRouteCheckerEgoVehicleDataMap<OBJECT_INSTANCE_TYPE> mEgoVehicleMap;
  ::ad::physics::Distance mArtificialObjectSamplingDistance;
  ArtificialObjectInjectionCallbackFunctionType mArtificialObjectsInjectionCallback;
  RssRouteCheckerArtificialObjectDataMap<OBJECT_INSTANCE_TYPE> mArtificialObjectMap;
 
  // one writer mutex for the callbacks of the class should be sufficient
  // vast majority of concurrent access is of shared nature
  mutable std::shared_timed_mutex mCallbackRwLock;
 
  std::map<world::ObjectType, CalculateTrajectorySetsCallbackFunctionType> mCalculateTrajectorySetsCallbackMap;
};
 
} // namespace map
} // namespace rss
} // namespace ad
 
#include "ad/rss/map/detail/RssRouteCheckerDetail.hpp"