Intersection.hpp

Go to the documentation of this file.

// ----------------- BEGIN LICENSE BLOCK ---------------------------------
//
// Copyright (C) 2018-2021 Intel Corporation
//
// SPDX-License-Identifier: MIT
//
// ----------------- END LICENSE BLOCK -----------------------------------
#pragma once
 
#include <map>
#include <memory>
#include <sstream>
#include <string>
#include <vector>
#include "ad/map/intersection/CoreIntersection.hpp"
#include "ad/map/intersection/IntersectionType.hpp"
#include "ad/map/intersection/TurnDirection.hpp"
#include "ad/map/landmark/Types.hpp"
#include "ad/map/lane/Types.hpp"
#include "ad/map/match/Types.hpp"
#include "ad/map/point/Types.hpp"
#include "ad/map/route/RouteOperation.hpp"
 
namespace ad {
namespace map {
namespace intersection {
 
class Intersection;
 
typedef std::shared_ptr<Intersection> IntersectionPtr;
 
typedef std::shared_ptr<Intersection const> IntersectionConstPtr;
 
class Intersection : public CoreIntersection
{
public:
  static IntersectionPtr getIntersectionForRoadSegment(route::RouteIterator const &routeIterator);
 
  static std::vector<IntersectionPtr> getIntersectionsForRoute(route::FullRoute const &route);
 
  static IntersectionPtr getNextIntersectionOnRoute(route::FullRoute const &route);
 
  IntersectionType intersectionType() const;
 
  TurnDirection turnDirection() const;
 
  landmark::LandmarkIdSet const &applicableTrafficLights() const;
 
  lane::LaneIdSet const &lanesOnRoute() const;
 
  point::ParaPointList const &paraPointsOnRoute() const;
 
  lane::LaneIdSet const &incomingLanesOnRoute() const;
 
  lane::LaneIdSet const &outgoingLanesOnRoute() const;
 
  point::ParaPointList const &incomingParaPointsOnRoute() const;
 
  point::ParaPointList const &outgoingParaPointsOnRoute() const;
 
  point::ParaPointList const &outgoingParaPoints() const;
 
  lane::LaneIdSet const &internalLanesWithHigherPriority() const;
 
  lane::LaneIdSet const &internalLanesWithLowerPriority() const;
 
  lane::LaneIdSet const &incomingLanes() const;
 
  lane::LaneIdSet const &outgoingLanes() const;
 
  lane::LaneIdSet const &incomingLanesWithLowerPriority() const;
 
  lane::LaneIdSet const &incomingLanesWithHigherPriority() const;
 
  point::ParaPointList const &incomingParaPoints() const;
 
  point::ParaPointList const &incomingParaPointsWithHigherPriority() const;
 
  point::ParaPointList const &incomingParaPointsWithLowerPriority() const;
 
  lane::LaneIdSet const &crossingLanes() const;
 
  route::RoutePlanningCounter getRoutePlanningCounter() const;
 
  route::SegmentCounter getRouteSegmentCountFromDestination() const;
 
  void updateRouteCounters(route::RoutePlanningCounter newRoutePlanningCounter,
                           route::SegmentCounter newRouteSegmentCounter);
 
  route::RouteParaPoint getIntersectionStartOnRoute() const;
 
  physics::Distance objectInterpenetrationDistanceWithIntersection(match::Object const &object) const;
 
  bool objectOnIncomingLane(match::MapMatchedObjectBoundingBox const &object) const;
 
  bool objectOnIncomingLaneWithHigherPriority(match::MapMatchedObjectBoundingBox const &object) const;
 
  bool objectOnIncomingLaneWithLowerPriority(match::MapMatchedObjectBoundingBox const &object) const;
 
  bool objectOnInternalLaneWithHigherPriority(match::MapMatchedObjectBoundingBox const &object) const;
 
  bool objectOnInternalLaneWithLowerPriority(match::MapMatchedObjectBoundingBox const &object) const;
 
  bool objectOnLaneWithHigherPriority(match::MapMatchedObjectBoundingBox const &object) const;
 
  bool objectOnLaneWithLowerPriority(match::MapMatchedObjectBoundingBox const &object) const;
 
  bool objectOnCrossingLane(match::MapMatchedObjectBoundingBox const &object) const;
 
  bool objectOnIntersectionRoute(match::MapMatchedObjectBoundingBox const &object) const;
 
  bool objectRouteCrossesIntersectionRoute(route::FullRoute const &objectRoute) const;
 
  bool objectRouteFromSameArmAsIntersectionRoute(route::FullRoute const &objectRoute) const;
 
  bool objectRouteOppositeToIntersectionRoute(route::FullRoute const &objectRoute) const;
 
  bool objectRouteCrossesLanesWithHigherPriority(route::FullRoute const &objectRoute) const;
 
  physics::Speed getSpeedLimit() const;
 
  bool onlySolidTrafficLightsOnRoute();
 
private:
  bool isSolidTrafficLight(landmark::LandmarkId trafficLightId);
 
  landmark::TrafficLightType extractTrafficLightType(landmark::LandmarkId trafficLightId);
 
  Intersection(route::FullRoute const &route,
               route::RoadSegmentList::const_iterator const &lastSegmentBeforeIntersection,
               route::RoadSegmentList::const_iterator const &firstSegmentWithinIntersection);
  Intersection() = delete;
 
  IntersectionType mIntersectionType{IntersectionType::Unknown};
 
  route::RoutePlanningCounter mRoutePlanningCounter;
 
  route::SegmentCounter mSegmentCountFromDestination;
 
  lane::LaneIdSet mLanesOnRoute;
 
  point::ParaPointList mParaPointsOnRoute;
 
  lane::LaneIdSet mIncomingLanesOnRoute;
 
  point::ParaPointList mIncomingParaPointsOnRoute;
 
  lane::LaneIdSet mInternalLanesWithHigherPriority;
 
  lane::LaneIdSet mInternalLanesWithLowerPriority;
 
  lane::LaneIdSet mInternalLanesFromSameIntersectionArm;
 
  lane::LaneIdSet mIncomingLanes;
 
  point::ParaPointList mIncomingParaPoints;
 
  lane::LaneIdSet mOutgoingLanesOnRoute{};
 
  point::ParaPointList mOutgoingParaPointsOnRoute;
 
  point::ParaPointList mIncomingParaPointsWithHigherPriority;
 
  lane::LaneIdSet mIncomingLanesWithHigherPriority;
 
  point::ParaPointList mIncomingParaPointsWithLowerPriority;
 
  lane::LaneIdSet mIncomingLanesWithLowerPriority;
 
  lane::LaneIdSet mCrossingLanes;
 
  std::map<TurnDirection, lane::LaneIdSet> mIntersectionArms;
 
  landmark::LandmarkIdSet mTrafficLightIds;
 
  TurnDirection mTurnDirection{TurnDirection::Unknown};
 
  physics::Speed mSpeedLimit;
 
  void extractRightOfWayAndCollectTrafficLights(route::LaneInterval const &laneInterval,
                                                lane::LaneIdSet const &successors,
                                                lane::LaneId &toLaneId);
 
  void extractIncomingLanes();
  void extractCrossingLanes();
  void extractLanesWithHigherPriority();
  void extractLanesWithLowerPriority();
  void extractLanesFromSameIntersectionArm();
  void calculateParaPoints();
 
  void calculateEnteringProrityParaPoints();
 
  void orderIntersectionArmsAndExtractTurnDirection();
 
  void adjustLanesForHasWay();
 
  void adjustLanesForYield();
 
  void adjustPriorityToRight();
 
  void adjustLanesForTrafficLight();
 
  bool turnDirectionCrossesStraightTraffic() const;
 
  void collectTrafficLights(lane::LaneId fromLaneId, lane::LaneId toLaneId, bool useSuccessor);
 
  lane::LaneIdSet successorsOnRouteLeavingIntersection(route::FullRoute const &route,
                                                       route::RoadSegmentList::const_iterator const &roadSegmentIt,
                                                       route::LaneSegment const &laneSegment);
 
  bool segmentLeavesIntersectionOnRoute(route::FullRoute const &route,
                                        route::RoadSegmentList::const_iterator const &roadSegmentIt,
                                        lane::LaneId laneId);
 
  lane::LaneIdSet successorsOnRouteLeavingIntersection(route::LaneSegment const &laneSegment);
 
  void calculateSpeedLimit();
 
  void addLaneAndSuccessorsToInternalLanesWithHigherPriority(lane::LaneId const &lanes,
                                                             TurnDirection const restrictToOutgoingIntersectionArm
                                                             = TurnDirection::Unknown);
 
  void addLaneAndSuccessorsToInternalLanesWithHigherPriority(lane::LaneIdSet const &lanes,
                                                             TurnDirection const restrictToOutgoingIntersectionArm
                                                             = TurnDirection::Unknown);
 
  void addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(
    lane::LaneId const &lanes, TurnDirection const restrictToOutgoingIntersectionArm = TurnDirection::Unknown);
 
  void addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(
    lane::LaneIdSet const &lanes, TurnDirection const restrictToOutgoingIntersectionArm = TurnDirection::Unknown);
 
  bool outgoingIntersectionArmCanBeReached(lane::LaneId const laneId, TurnDirection const outgoingIntersectionArm);
};
 
} // namespace intersection
} // namespace map
} // namespace ad
 
namespace std {
 
std::ostream &operator<<(std::ostream &os, ::ad::map::intersection::Intersection const &intersection);
 
static inline std::string to_string(::ad::map::intersection::Intersection const &intersection)
{
  stringstream sstream;
  sstream << intersection;
  return sstream.str();
}
} // namespace std