RssUnstructuredConstellationChecker.hpp

Go to the documentation of this file.

// ----------------- BEGIN LICENSE BLOCK ---------------------------------
//
// Copyright (C) 2020-2022 Intel Corporation
//
// SPDX-License-Identifier: LGPL-2.1-only
//
// ----------------- END LICENSE BLOCK -----------------------------------
 
#pragma once
 
#include <ad/geometry/GeometryOperation.hpp>
#include <cstdint>
#include <map>
#include "ad/rss/core/RelativeConstellation.hpp"
#include "ad/rss/core/shared_lock_guard.hpp"
#include "ad/rss/state/RssState.hpp"
#include "ad/rss/world/TimeIndex.hpp"
 
namespace ad {
namespace rss {
namespace unstructured {
 
class RssUnstructuredConstellationChecker
{
public:
  enum class SafeState
  {
    safe,
    unsafeBrakeOtherHasPrio,
    unsafeBrakeEgoHasPrio,
    unsafeBrakeBoth
  };
 
  enum class DrivingMode
  {
    DriveAway,
    ContinueForward,
    Brake,
    Invalid
  };
 
  RssUnstructuredConstellationChecker();
 
  RssUnstructuredConstellationChecker(RssUnstructuredConstellationChecker const &other);
 
  ~RssUnstructuredConstellationChecker() = default;
 
  RssUnstructuredConstellationChecker &operator=(RssUnstructuredConstellationChecker const &other);
 
  bool calculateRssStateUnstructured(world::TimeIndex const &time_index,
                                     core::RelativeConstellation const &constellation,
                                     state::UnstructuredConstellationStateInformation &egoStateInfo,
                                     state::RssState &rssState);
 
  using CalculateTrajectorySetsCallbackFunctionType
    = std::function<bool(::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)
  {
    std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
    mCalculateTrajectorySetsCallbackMap[objectType] = calculateTrajectorySetsCallback;
  }
 
private:
  bool calculateUnstructuredConstellationStateInfo(::ad::rss::core::RelativeConstellation const &constellation,
                                                   ::ad::rss::world::ObjectId const &objectId,
                                                   core::RelativeObjectState const &objectState,
                                                   state::UnstructuredConstellationStateInformation &stateInfo) const;
 
  bool calculateState(core::RelativeConstellation const &constellation,
                      state::UnstructuredConstellationStateInformation const &egoStateInfo,
                      state::UnstructuredConstellationStateInformation const &otherStateInfo,
                      state::UnstructuredConstellationRssState &rssState);
 
  bool calculateDriveAwayAngle(::ad::geometry::Point const &egoVehicleLocation,
                               ::ad::geometry::Point const &otherVehicleLocation,
                               physics::Angle const &maxAllowedAngleWhenBothStopped,
                               ::ad::geometry::HeadingRange &range) const;
 
  typedef std::map<core::RelativeConstellationId, bool> OtherMustBrakeStateBeforeDangerThresholdTimeMap;
 
  OtherMustBrakeStateBeforeDangerThresholdTimeMap mOtherMustBrakeStatesBeforeDangerThresholdTime;
 
  OtherMustBrakeStateBeforeDangerThresholdTimeMap mNewOtherMustBrakeStatesBeforeDangerThresholdTime;
 
  world::TimeIndex mCurrentTimeIndex{0u};
 
  struct DriveAwayState
  {
    ::ad::geometry::HeadingRange allowedHeadingRange;
    physics::Distance2D otherPosition;
  };
 
  std::map<core::RelativeConstellationId, DriveAwayState> mDriveAwayStateMap;
 
  // 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 unstructured
} // namespace rss
} // namespace ad