RouteOperation.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 "ad/map/lane/Types.hpp"
#include "ad/map/match/Types.hpp"
#include "ad/map/point/Types.hpp"
#include "ad/map/route/LaneIntervalOperation.hpp"
#include "ad/map/route/Routing.hpp"
#include "ad/map/route/Types.hpp"
#include "ad/physics/Types.hpp"
 
namespace ad {
namespace map {
 
namespace intersection {
class Intersection;
} // namespace intersection
 
namespace route {
 
struct RouteIterator
{
  RouteIterator(FullRoute const &iRoute, RoadSegmentList::const_iterator const &iRoadSegmentIterator)
    : route(iRoute)
    , roadSegmentIterator(iRoadSegmentIterator)
  {
  }
 
  bool isValid() const
  {
    return roadSegmentIterator != std::end(route.roadSegments);
  }
 
  FullRoute const &route;
  RoadSegmentList::const_iterator roadSegmentIterator;
};
 
RouteIterator getRouteIterator(route::RouteParaPoint const &routePosition, route::FullRoute const &route);
 
point::ParaPoint getLaneParaPoint(physics::ParametricValue const &routeParametricOffset,
                                  route::LaneInterval const &laneInterval);
 
physics::Distance signedDistanceToLane(lane::LaneId const &checkLaneId,
                                       FullRoute const &route,
                                       match::MapMatchedPositionConfidenceList const &mapMatchedPositions);
 
point::ParaPointList getLaneParaPoints(route::RouteParaPoint const &routePosition, route::FullRoute const &route);
 
physics::Distance calcLength(FullRoute const &fullRoute);
 
physics::Distance calcLength(RoadSegment const &roadSegment);
 
inline physics::Distance calcLength(LaneSegment const &laneSegment)
{
  return calcLength(laneSegment.laneInterval);
}
 
physics::Distance calcLength(ConnectingRoute const &connectingRoute);
 
physics::Distance calcLength(RouteIterator const &startIterator, RouteIterator const &endIterator);
 
physics::Distance
calcLength(RouteParaPoint const &startRouteParaPoint, RouteParaPoint const &endRouteParaPoint, FullRoute const &route);
 
physics::Duration calcDuration(FullRoute const &fullRoute);
 
physics::Duration calcDuration(RoadSegment const &roadSegment);
 
inline physics::Duration calcDuration(LaneSegment const &laneSegment)
{
  return calcDuration(laneSegment.laneInterval);
}
 
physics::Duration calcDuration(ConnectingRoute const &connectingRoute);
 
restriction::SpeedLimitList getSpeedLimits(RoadSegment const &roadSegment);
 
restriction::SpeedLimitList getSpeedLimits(LaneSegment const &laneSegment);
 
restriction::SpeedLimitList getSpeedLimits(RouteIterator const &startIterator, RouteIterator const &endIterator);
 
restriction::SpeedLimitList getSpeedLimits(FullRoute const &fullRoute);
 
restriction::SpeedLimitList getSpeedLimits(ConnectingRoute const &connectingRoute);
 
bool isWithinInterval(RoadSegment const &roadSegment, point::ParaPoint const &point);
 
struct FindWaypointResult
{
  FindWaypointResult(FullRoute const &route);
 
  FindWaypointResult(FullRoute const &route,
                     point::ParaPoint const &iQueryPosition,
                     RoadSegmentList::const_iterator roadSegmentIter,
                     LaneSegmentList::const_iterator laneIntervalIter)
    : queryRoute(route)
    , roadSegmentIterator(roadSegmentIter)
    , laneSegmentIterator(laneIntervalIter)
    , queryPosition(iQueryPosition)
  {
  }
 
  FindWaypointResult(FindWaypointResult const &other) = default;
 
  FullRoute const &queryRoute;
  RoadSegmentList::const_iterator roadSegmentIterator;
  LaneSegmentList::const_iterator laneSegmentIterator;
  point::ParaPoint queryPosition;
 
  bool isValid() const
  {
    return roadSegmentIterator != queryRoute.roadSegments.end()
      && laneSegmentIterator != roadSegmentIterator->drivableLaneSegments.end();
  }
  FindWaypointResult &operator=(FindWaypointResult const &other);
 
  FindWaypointResult getLeftLane() const;
 
  FindWaypointResult getRightLane() const;
 
  std::vector<FindWaypointResult> getSuccessorLanes() const;
 
  std::vector<FindWaypointResult> getPredecessorLanes() const;
};
 
physics::Distance calcLength(FindWaypointResult const &findWaypointResult);
 
FindWaypointResult findWaypoint(point::ParaPoint const &position, route::FullRoute const &route);
 
FindWaypointResult findWaypoint(lane::LaneId const &laneId, route::FullRoute const &route);
 
FindWaypointResult findNearestWaypoint(point::ParaPointList const &positions, route::FullRoute const &route);
 
FindWaypointResult findNearestWaypoint(match::MapMatchedPositionConfidenceList const &mapMatchedPositions,
                                       route::FullRoute const &route);
 
FindWaypointResult findCenterWaypoint(match::Object const &object, route::FullRoute const &route);
 
FindWaypointResult objectOnRoute(match::MapMatchedObjectBoundingBox const &object, route::FullRoute const &route);
 
FindWaypointResult intersectionOnRoute(intersection::Intersection const &intersection, route::FullRoute const &route);
 
bool isConnectedRoutePartOfAnIntersection(route::ConnectingRoute const &connectingRoute);
 
enum class ShortenRouteResult
{
  Succeeded,
  SucceededBeforeRoute,
  SucceededRouteEmpty,
  SucceededIntersectionNotCut,
  FailedRouteEmpty
};
 
enum class ShortenRouteMode
{
  Normal,
  PrependIfSucceededBeforeRoute,
  DontCutIntersectionAndPrependIfSucceededBeforeRoute
};
 
ShortenRouteResult shortenRoute(point::ParaPoint const &currentPosition,
                                route::FullRoute &route,
                                ShortenRouteMode const shortenRouteMode = ShortenRouteMode::Normal);
 
ShortenRouteResult shortenRoute(point::ParaPointList const &currentPositions,
                                route::FullRoute &route,
                                ShortenRouteMode const shortenRouteMode = ShortenRouteMode::Normal);
 
ShortenRouteResult shortenRoute(match::MapMatchedPositionConfidenceList const &mapMatchedPositions,
                                route::FullRoute &route,
                                ShortenRouteMode const shortenRouteMode = ShortenRouteMode::Normal);
 
bool calculateRouteParaPointAtDistance(route::FullRoute const &route,
                                       route::RouteParaPoint const &origin,
                                       physics::Distance const &distance,
                                       route::RouteParaPoint &resultingPoint);
 
bool getRouteParaPointFromParaPoint(point::ParaPoint const &paraPoint,
                                    FullRoute const &route,
                                    route::RouteParaPoint &routeParaPoint);
 
enum class RouteSectionCreationMode
{
  SingleLane,   
  AllRouteLanes 
};
 
FullRoute getRouteSection(FindWaypointResult const &currentLane,
                          physics::Distance const &distanceFront,
                          physics::Distance const &distanceEnd,
                          FullRoute const &route,
                          RouteSectionCreationMode const routeSectionCreationMode
                          = RouteSectionCreationMode::SingleLane);
 
route::FullRoute getRouteSection(point::ParaPoint const &centerPoint,
                                 physics::Distance const &distanceFront,
                                 physics::Distance const &distanceEnd,
                                 route::FullRoute const &route,
                                 RouteSectionCreationMode const routeSectionCreationMode
                                 = RouteSectionCreationMode::SingleLane);
 
route::FullRoute getRouteSection(match::Object const &object,
                                 route::FullRoute const &route,
                                 RouteSectionCreationMode const routeSectionCreationMode
                                 = RouteSectionCreationMode::SingleLane);
 
struct FindLaneChangeResult
{
  explicit FindLaneChangeResult(FullRoute const &route);
 
  FullRoute const &queryRoute;
 
  RoadSegmentList::const_iterator laneChangeStartRouteIterator;
 
  LaneSegmentList::const_iterator laneChangeStartLaneSegmentIterator;
 
  RoadSegmentList::const_iterator laneChangeEndRouteIterator;
 
  LaneSegmentList::const_iterator laneChangeEndLaneSegmentIterator;
 
  LaneChangeDirection laneChangeDirection;
 
  uint32_t numberOfConnectedLaneChanges{0u};
 
  bool isValid() const
  {
    return laneChangeStartRouteIterator != std::end(queryRoute.roadSegments)
      && laneChangeStartLaneSegmentIterator != std::end(laneChangeStartRouteIterator->drivableLaneSegments)
      && laneChangeEndRouteIterator != std::end(queryRoute.roadSegments)
      && laneChangeEndLaneSegmentIterator != std::end(laneChangeEndRouteIterator->drivableLaneSegments)
      && laneChangeDirection != LaneChangeDirection::Invalid;
  }
 
  physics::Distance calcZoneLength() const;
};
 
FindLaneChangeResult findFirstLaneChange(match::MapMatchedPosition const &currentPositionEgoVehicle,
                                         route::FullRoute const &route);
 
void shortenRouteToDistance(route::FullRoute &route, const physics::Distance &length);
 
void appendLaneSegmentToRoute(route::LaneInterval const &laneInterval,
                              route::FullRoute &route,
                              route::SegmentCounter const segmentCountFromDestination = 0u);
 
bool extendRouteToDistance(route::FullRoute &route,
                           physics::Distance const &length,
                           route::FullRouteList &additionalRoutes,
                           lane::LaneIdSet const &relevantLanes = ad::map::lane::LaneIdSet());
 
bool extendRouteToDestinations(route::FullRoute &route, const std::vector<route::planning::RoutingParaPoint> &dest);
 
bool extendRouteToDestinations(route::FullRoute &route, const std::vector<point::GeoPoint> &dest);
 
bool extendRouteToDestinations(route::FullRoute &route, const std::vector<point::ENUPoint> &dest);
 
void appendRoadSegmentToRoute(route::LaneInterval const &laneInverval,
                              route::RouteLaneOffset const &laneOffset,
                              route::FullRoute &route,
                              lane::LaneIdSet const &relevantLanes);
 
physics::Distance addOpposingLaneSegmentToRoadSegment(point::ParaPoint const &startpoint,
                                                      physics::Distance const &distance,
                                                      route::RoadSegment &roadSegment,
                                                      route::FullRoute &route);
 
bool addOpposingLaneToRoute(point::ParaPoint const &pointOnOppositeLane,
                            physics::Distance const &distanceOnWrongLane,
                            route::FullRoute &route,
                            physics::Distance &coveredDistance);
 
route::FullRoute getRouteExpandedToOppositeLanes(route::FullRoute const &route);
 
route::FullRoute getRouteExpandedToAllNeighborLanes(route::FullRoute const &route);
 
bool calculateBypassingRoute(route::FullRoute const &route, route::FullRoute &bypassingRoute);
 
lane::ECEFBorder getECEFBorderOfRoadSegment(RoadSegment const &roadSegment,
                                            physics::ParametricValue const parametricOffset);
 
inline lane::ECEFBorder getECEFBorderOfRoadSegment(RoadSegment const &roadSegment)
{
  return getECEFBorderOfRoadSegment(roadSegment, physics::ParametricValue(1.));
}
 
lane::ENUBorder getENUBorderOfRoadSegment(RoadSegment const &roadSegment,
                                          physics::ParametricValue const parametricOffset);
 
inline lane::ENUBorder getENUBorderOfRoadSegment(RoadSegment const &roadSegment)
{
  return getENUBorderOfRoadSegment(roadSegment, physics::ParametricValue(1.));
}
 
lane::GeoBorder getGeoBorderOfRoadSegment(RoadSegment const &roadSegment,
                                          physics::ParametricValue const parametricOffset);
 
inline lane::GeoBorder getGeoBorderOfRoadSegment(RoadSegment const &roadSegment)
{
  return getGeoBorderOfRoadSegment(roadSegment, physics::ParametricValue(1.));
}
 
lane::ENUBorderList getENUBorderOfRoute(FullRoute const &route);
 
lane::ECEFBorderList getECEFBorderOfRoute(FullRoute const &route);
 
lane::GeoBorderList getGeoBorderOfRoute(FullRoute const &route);
 
point::ENUHeading getENUHeadingOfRoute(match::Object const &object, FullRoute const &route);
 
bool isObjectHeadingInRouteDirection(match::Object const &object, FullRoute const &route);
 
void shortenSegmentFromBegin(RoadSegment &roadSegment, physics::Distance const &distance);
 
void shortenSegmentFromEnd(RoadSegment &roadSegment, physics::Distance const &distance);
 
} // namespace route
} // namespace map
} // namespace ad