RouteAStar.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 "ad/map/route/RouteExpander.hpp"
 
namespace ad {
namespace map {
namespace route {
namespace planning {
 
struct RouteAstarScore
{
  physics::Distance estimatedDistanceToTarget{0.};
};
 
class RouteAstar : public RouteExpander<RouteAstarScore>
{
public:
  using RouteExpander::RoutingCost;
  using RouteExpander::RoutingPoint;
 
  RouteAstar(const RoutingParaPoint &start,
             const RoutingParaPoint &dest,
             physics::Distance const &maxDistance,
             physics::Duration const &maxDuration,
             Type typ);
 
  RouteAstar(const RoutingParaPoint &start,
             const RoutingParaPoint &dest,
             physics::Distance const &maxDistance,
             Type typ);
 
  RouteAstar(const RoutingParaPoint &start,
             const RoutingParaPoint &dest,
             physics::Duration const &maxDuration,
             Type typ);
 
  RouteAstar(const RoutingParaPoint &start, const RoutingParaPoint &dest, Type typ);
 
  bool calculate() override;
 
private:
  void initLanePointer();
 
  void addNeighbor(lane::Lane::ConstPtr originLane,
                   RoutingPoint const &origin,
                   lane::Lane::ConstPtr neighborLane,
                   RoutingPoint const &neighbor,
                   ExpandReason const &expandReason) override;
  physics::Distance costEstimate(lane::Lane::ConstPtr neighborLane, point::ParaPoint const &neighbor);
 
  void reconstructPath(RoutingPoint const &dest);
 
  lane::Lane::ConstPtr mDestLane;
  lane::Lane::ConstPtr mStartLane;
 
  typedef std::set<RoutingParaPoint> RoutingParaPointSet;
 
  RoutingParaPointSet mProcessedPoints;
 
  typedef std::map<RoutingParaPoint, RoutingCost> RoutingParaPointCostMap;
  RoutingParaPointCostMap mProcessingMap;
 
  typedef std::map<RoutingParaPoint, RoutingParaPoint> RoutingChainMap;
  RoutingChainMap mCameFrom;
};
 
} // namespace planning
} // namespace route
} // namespace map
} // namespace ad