Program Listing for File RoutingCost.h

↰ Return to documentation for file (include/lanelet2_routing/RoutingCost.h)

#pragma once

#include <lanelet2_core/Exceptions.h>
#include <lanelet2_core/Forward.h>
#include <lanelet2_core/primitives/LaneletOrArea.h>
#include <lanelet2_traffic_rules/TrafficRules.h>

#include "lanelet2_routing/Forward.h"

namespace lanelet {
namespace routing {

class RoutingCost {  // NOLINT
 public:
  virtual ~RoutingCost() = default;

  virtual double getCostSucceeding(const traffic_rules::TrafficRules& trafficRules, const ConstLaneletOrArea& from,
                                   const ConstLaneletOrArea& to) const = 0;

  virtual double getCostLaneChange(const traffic_rules::TrafficRules& trafficRules, const ConstLanelets& from,
                                   const ConstLanelets& to) const = 0;
};

class RoutingCostDistance : public RoutingCost {
 public:
  explicit RoutingCostDistance(double laneChangeCost, double minLaneChangeLength = 0.)
      : laneChangeCost_{laneChangeCost}, minChangeLength_{minLaneChangeLength} {
    if (laneChangeCost_ < 0.0) {
      throw InvalidInputError("Lane change cost must be positive, but it is " + std::to_string(laneChangeCost_));
    }
  }
  inline double getCostSucceeding(const traffic_rules::TrafficRules& /*trafficRules*/, const ConstLaneletOrArea& from,
                                  const ConstLaneletOrArea& to) const override {
    auto getLength = [this](auto& lltOrArea) -> double { return this->length(lltOrArea); };
    return (from.applyVisitor(getLength) + to.applyVisitor(getLength)) / 2;
  }
  inline double getCostLaneChange(const traffic_rules::TrafficRules& /*trafficRules*/, const ConstLanelets& from,
                                  const ConstLanelets& /*to*/) const noexcept override {
    if (minChangeLength_ <= 0.) {
      return laneChangeCost_;
    }
    auto totalLength = std::accumulate(from.begin(), from.end(), 0.,
                                       [this](double acc, auto& llt) { return acc + this->length(llt); });
    return totalLength >= minChangeLength_ ? laneChangeCost_ : std::numeric_limits<double>::infinity();
  }

 private:
  static double length(const ConstLanelet& ll) noexcept;
  static double length(const ConstArea& ar) noexcept;
  const double laneChangeCost_, minChangeLength_;  // NOLINT
};

class RoutingCostTravelTime : public RoutingCost {
 public:
  RoutingCostTravelTime() = delete;

  explicit RoutingCostTravelTime(double laneChangeCost, double minLaneChangeTime = 0.)
      : laneChangeCost_{laneChangeCost}, minChangeTime_{minLaneChangeTime} {
    if (laneChangeCost_ < 0.0) {
      throw InvalidInputError("Lane change cost must be positive, but it is " + std::to_string(laneChangeCost_));
    }
  }

  inline double getCostLaneChange(const traffic_rules::TrafficRules& trafficRules, const ConstLanelets& from,
                                  const ConstLanelets& /*to*/) const noexcept override {
    if (minChangeTime_ <= 0.) {
      return laneChangeCost_;
    }
    auto changeTime = std::accumulate(from.begin(), from.end(), 0., [this, &trafficRules](double acc, auto& llt) {
      return acc + this->travelTime(trafficRules, llt);
    });
    return changeTime >= minChangeTime_ ? laneChangeCost_ : std::numeric_limits<double>::infinity();
  }
  inline double getCostSucceeding(const traffic_rules::TrafficRules& trafficRules, const ConstLaneletOrArea& from,
                                  const ConstLaneletOrArea& to) const override {
    auto getTravelTime = [&trafficRules, this](auto& lltOrArea) -> double {
      return this->travelTime(trafficRules, lltOrArea);
    };
    return (from.applyVisitor(getTravelTime) + to.applyVisitor(getTravelTime)) / 2;
  }

 private:
  static double travelTime(const traffic_rules::TrafficRules& trafficRules, const ConstLanelet& ll);
  static double travelTime(const traffic_rules::TrafficRules& trafficRules, const ConstArea& ar);
  const Velocity maxSpeed_;                      // NOLINT
  const double laneChangeCost_, minChangeTime_;  // NOLINT
};

inline RoutingCostPtrs defaultRoutingCosts() {
  return RoutingCostPtrs{std::make_shared<RoutingCostDistance>(10.), std::make_shared<RoutingCostTravelTime>(5.)};
}
}  // namespace routing
}  // namespace lanelet