RssRouteCheckerControl.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/access/Logging.hpp>
#include <ad/rss/core/Logging.hpp>
#include <ad/rss/core/shared_lock_guard.hpp>
#include <ad/rss/map/Logging.hpp>
#include <ad/rss/map/RssAppendRoadBoundariesMode.hpp>
#include <ad/rss/map/RssConstellationConfiguration.hpp>
#include <ad/rss/map/RssObjectAdapter.hpp>
#include <ad/rss/map/RssObjectData.hpp>
#include <ad/rss/map/RssRoute.hpp>
#include <ad/rss/map/RssWorldModelCreation.hpp>
#include <map>
 
namespace ad {
namespace rss {
namespace map {
 
template <class OBJECT_INSTANCE_TYPE> class RssRouteCheckerControl
{
public:
  typedef std::shared_ptr<RssRouteCheckerControl> Ptr;
 
  typedef std::shared_ptr<RssRouteCheckerControl const> ConstPtr;
 
  RssRouteCheckerControl() = default;
 
  virtual ~RssRouteCheckerControl() = default;
 
  using DefaultRssDynamicsCallbackFunctionType
    = std::function<::ad::rss::world::RssDynamics(OBJECT_INSTANCE_TYPE vehicle)>;
 
  void registerDefaultRssDynamicsCallback(::ad::rss::world::ObjectType const objectType,
                                          DefaultRssDynamicsCallbackFunctionType callback)
  {
    std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
    mDefaultRssDynamicsCallbacks[objectType] = callback;
  }
 
  bool isDefaultRssDynamicsCallbackRegistered(::ad::rss::world::ObjectType const objectType) const
  {
    core::shared_lock_guard const shared_lock(mCallbackRwLock);
    return mDefaultRssDynamicsCallbacks.find(objectType) != mDefaultRssDynamicsCallbacks.end();
  }
 
  ::ad::rss::world::RssDynamics getDefaultRssDynamics(::ad::rss::world::ObjectType const objectType,
                                                      OBJECT_INSTANCE_TYPE vehicle) const
  {
    core::shared_lock_guard const shared_lock(mCallbackRwLock);
    auto findResult = mDefaultRssDynamicsCallbacks.find(objectType);
    if (findResult != mDefaultRssDynamicsCallbacks.end())
    {
      return findResult->second(vehicle);
    }
    return ::ad::rss::world::RssDynamics();
  }
 
  using VehicleConstellationCallbackFunctionType
    = std::function<::ad::rss::map::RssConstellationConfiguration(::ad::rss::map::RssRoute const &route,
                                                                  ::ad::rss::map::RssObjectData const &egoVehicleObject,
                                                                  OBJECT_INSTANCE_TYPE egoVehicle,
                                                                  ::ad::rss::map::RssObjectData const &vehicleObject,
                                                                  OBJECT_INSTANCE_TYPE vehicle)>;
 
  void registerVehicleConstellationCallback(VehicleConstellationCallbackFunctionType callback)
  {
    std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
    mVehicleConstellationCallback = callback;
  }
 
  bool isVehicleConstellationCallbackRegistered() const
  {
    core::shared_lock_guard const shared_lock(mCallbackRwLock);
    return bool(mVehicleConstellationCallback);
  }
 
  ::ad::rss::map::RssConstellationConfiguration
  getVehicleConstellation(::ad::rss::map::RssRoute const &route,
                          ::ad::rss::map::RssObjectData const &egoVehicleObject,
                          OBJECT_INSTANCE_TYPE egoVehicle,
                          ::ad::rss::map::RssObjectData const &vehicleObject,
                          OBJECT_INSTANCE_TYPE vehicle) const
  {
    core::shared_lock_guard const shared_lock(mCallbackRwLock);
    if (bool(mVehicleConstellationCallback))
    {
      return mVehicleConstellationCallback(route, egoVehicleObject, egoVehicle, vehicleObject, vehicle);
    }
    return ::ad::rss::map::RssConstellationConfiguration();
  }
 
  using PedestrianConstellationCallbackFunctionType
    = std::function<::ad::rss::map::RssConstellationConfiguration(::ad::rss::map::RssRoute const &route,
                                                                  ::ad::rss::map::RssObjectData const &egoVehicleObject,
                                                                  OBJECT_INSTANCE_TYPE egoVehicle,
                                                                  ::ad::rss::map::RssObjectData const &pedestrianObject,
                                                                  OBJECT_INSTANCE_TYPE pedestrian)>;
 
  void registerPedestrianConstellationCallback(PedestrianConstellationCallbackFunctionType callback)
  {
    std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
    mPedestrianConstellationCallback = callback;
  }
 
  bool isPedestrianConstellationCallbackRegistered() const
  {
    core::shared_lock_guard const shared_lock(mCallbackRwLock);
    return bool(mPedestrianConstellationCallback);
  }
 
  ::ad::rss::map::RssConstellationConfiguration
  getPedestrianConstellation(::ad::rss::map::RssRoute const &route,
                             ::ad::rss::map::RssObjectData const &egoVehicleObject,
                             OBJECT_INSTANCE_TYPE egoVehicle,
                             ::ad::rss::map::RssObjectData const &pedestrianObject,
                             OBJECT_INSTANCE_TYPE pedestrian) const
  {
    core::shared_lock_guard const shared_lock(mCallbackRwLock);
    if (bool(mPedestrianConstellationCallback))
    {
      return mPedestrianConstellationCallback(route, egoVehicleObject, egoVehicle, pedestrianObject, pedestrian);
    }
    return ::ad::rss::map::RssConstellationConfiguration();
  }
 
  using ArtificialObjectConstellationCallbackFunctionType = std::function<::ad::rss::map::RssConstellationConfiguration(
    ::ad::rss::map::RssRoute const &route,
    ::ad::rss::map::RssObjectData const &egoVehicleObject,
    OBJECT_INSTANCE_TYPE egoVehicle,
    ::ad::rss::map::RssObjectData const &artificialObject)>;
 
  void registerArtificialObjectConstellationCallback(ArtificialObjectConstellationCallbackFunctionType callback)
  {
    std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
    mArtificialObjectConstellationCallback = callback;
  }
 
  bool isArtificialObjectConstellationCallbackRegistered() const
  {
    core::shared_lock_guard const shared_lock(mCallbackRwLock);
    return bool(mArtificialObjectConstellationCallback);
  }
 
  ::ad::rss::map::RssConstellationConfiguration
  getArtificialObjectConstellation(::ad::rss::map::RssRoute const &route,
                                   ::ad::rss::map::RssObjectData const &egoVehicleObject,
                                   OBJECT_INSTANCE_TYPE egoVehicle,
                                   ::ad::rss::map::RssObjectData const &artificialObject) const
  {
    core::shared_lock_guard const shared_lock(mCallbackRwLock);
    if (bool(mArtificialObjectConstellationCallback))
    {
      return mArtificialObjectConstellationCallback(route, egoVehicleObject, egoVehicle, artificialObject);
    }
    return ::ad::rss::map::RssConstellationConfiguration();
  }
 
  void setRoadBoundariesMode(const ::ad::rss::map::RssAppendRoadBoundariesMode roadBoundariesMode)
  {
    mRssAppendRoadBoundariesMode = roadBoundariesMode;
  }
 
  ::ad::rss::map::RssAppendRoadBoundariesMode getRoadBoundariesMode() const
  {
    return mRssAppendRoadBoundariesMode;
  }
 
  void setCurrentGreenTrafficLights(::ad::map::landmark::LandmarkIdSet const &currentGreenTrafficLights)
  {
    mCurrentGreenTrafficLights = currentGreenTrafficLights;
  }
 
  ::ad::map::landmark::LandmarkIdSet getCurrentGreenTrafficLights() const
  {
    return mCurrentGreenTrafficLights;
  }
 
  void setPositionJumpTraveledDistance(ad::physics::Distance const positionJumpTraveledDistance)
  {
    mPositionJumpTraveledDistance = positionJumpTraveledDistance;
  }
 
  ad::physics::Distance getPositionJumpTraveledDistance() const
  {
    return mPositionJumpTraveledDistance;
  }
 
  void setMinimumDistanceToObjectsThatHaveToBeAnalyzed(
    ad::physics::Distance const minimumDistanceToObjectsThatHaveToBeAnalyzed)
  {
    mMinimumDistanceToObjectsThatHaveToBeAnalyzed = minimumDistanceToObjectsThatHaveToBeAnalyzed;
  }
 
  ad::physics::Distance getMinimumDistanceToObjectsThatHaveToBeAnalyzed() const
  {
    return mMinimumDistanceToObjectsThatHaveToBeAnalyzed;
  }
 
  void setConsiderPositionConfidence(bool const consider_position_confidence)
  {
    mConsiderPositionConfidence = consider_position_confidence;
  }
  bool getConsiderPositionConfidence()
  {
    return mConsiderPositionConfidence;
  }
 
  void setLogLevel(const spdlog::level::level_enum logLevel)
  {
    ::ad::rss::core::getLogger()->set_level(logLevel);
    ::ad::rss::map::getLogger()->set_level(logLevel);
  }
 
  void setMapLogLevel(const spdlog::level::level_enum mapLogLevel)
  {
    ::ad::map::access::getLogger()->set_level(mapLogLevel);
  }
 
  using RssRoutingTargetOperation = typename RssVehicleAdapter<OBJECT_INSTANCE_TYPE>::RssRoutingTargetOperation;
 
  using RssRoutingTargetCommand = typename RssVehicleAdapter<OBJECT_INSTANCE_TYPE>::RssRoutingTargetCommand;
 
  void setActiveRoutingTargets(ad::rss::world::ObjectId vehicleId,
                               ::ad::map::point::ENUPointList const &activeRoutingTargets)
  {
    std::lock_guard<std::mutex> const lock(mRoutingLock);
    mActiveRoutingTargets[vehicleId] = activeRoutingTargets;
  }
 
  ::ad::map::point::ENUPointList getActiveRoutingTargets(ad::rss::world::ObjectId vehicleId) const
  {
    std::lock_guard<std::mutex> const lock(mRoutingLock);
    auto findResult = mActiveRoutingTargets.find(vehicleId);
    if (findResult != mActiveRoutingTargets.end())
    {
      return findResult->second;
    }
    return ::ad::map::point::ENUPointList();
  }
 
  void appendRoutingTargets(ad::rss::world::ObjectId vehicleId,
                            ::ad::map::point::ENUPointList const &routingTargetsToAppend)
  {
    std::lock_guard<std::mutex> const lock(mRoutingLock);
    RssRoutingTargetOperation operation;
    operation.command = RssRoutingTargetCommand::AppendTargets;
    operation.routingTargets = routingTargetsToAppend;
    auto const insertResult = mRoutingTargetOperations.insert(std::make_pair(vehicleId, operation));
    if (!insertResult.second)
    {
      insertResult.first->second.routingTargets.insert(
        insertResult.first->second.routingTargets.end(), routingTargetsToAppend.begin(), routingTargetsToAppend.end());
    }
  }
 
  void replaceRoutingTargets(ad::rss::world::ObjectId vehicleId,
                             ::ad::map::point::ENUPointList const &newRoutingTargets)
  {
    std::lock_guard<std::mutex> const lock(mRoutingLock);
    RssRoutingTargetOperation operation;
    operation.command = RssRoutingTargetCommand::ReplaceTargets;
    operation.routingTargets = newRoutingTargets;
    auto const insertResult = mRoutingTargetOperations.insert(std::make_pair(vehicleId, operation));
    if (!insertResult.second)
    {
      insertResult.first->second = operation;
    }
  }
 
  RssRoutingTargetOperation getCurrentRoutingTargetOperation(ad::rss::world::ObjectId vehicleId)
  {
    RssRoutingTargetOperation resultOperation;
    std::lock_guard<std::mutex> const lock(mRoutingLock);
    auto findResult = mRoutingTargetOperations.find(vehicleId);
    if (findResult != mRoutingTargetOperations.end())
    {
      resultOperation = findResult->second;
      mRoutingTargetOperations.erase(vehicleId);
    }
    return resultOperation;
  }
 
  void dropRoute(ad::rss::world::ObjectId vehicleId)
  {
    replaceRoutingTargets(vehicleId, getActiveRoutingTargets(vehicleId));
  }
 
  void dropVehicle(ad::rss::world::ObjectId vehicleId)
  {
    std::lock_guard<std::mutex> const lock(mRoutingLock);
    mRoutingTargetOperations.erase(vehicleId);
    mActiveRoutingTargets.erase(vehicleId);
  }
 
private:
  // 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<::ad::rss::world::ObjectType, DefaultRssDynamicsCallbackFunctionType> mDefaultRssDynamicsCallbacks;
 
  VehicleConstellationCallbackFunctionType mVehicleConstellationCallback;
  PedestrianConstellationCallbackFunctionType mPedestrianConstellationCallback;
  ArtificialObjectConstellationCallbackFunctionType mArtificialObjectConstellationCallback;
 
  ::ad::rss::map::RssAppendRoadBoundariesMode mRssAppendRoadBoundariesMode{
    ::ad::rss::map::RssAppendRoadBoundariesMode::Off};
  ::ad::map::landmark::LandmarkIdSet mCurrentGreenTrafficLights;
 
  mutable std::mutex mRoutingLock;
 
  std::map<ad::rss::world::ObjectId, ::ad::map::point::ENUPointList> mActiveRoutingTargets;
  std::map<ad::rss::world::ObjectId, RssRoutingTargetOperation> mRoutingTargetOperations;
 
  ad::physics::Distance mPositionJumpTraveledDistance{20.};
  ad::physics::Distance mMinimumDistanceToObjectsThatHaveToBeAnalyzed{RssWorldModelCreation::cMinConnectedRouteLength};
  bool mConsiderPositionConfidence{true};
};
 
} // namespace map
} // namespace rss
} // namespace ad