diff --git a/include/arbitration_graphs/cost_arbitrator.hpp b/include/arbitration_graphs/cost_arbitrator.hpp index c182c5d4..34ea7b5c 100644 --- a/include/arbitration_graphs/cost_arbitrator.hpp +++ b/include/arbitration_graphs/cost_arbitrator.hpp @@ -3,9 +3,12 @@ #include #include +#include #include #include "arbitrator.hpp" +#include "exceptions.hpp" +#include "types.hpp" namespace arbitration_graphs { @@ -21,6 +24,45 @@ struct CostEstimator { bool isActive) = 0; }; +template +struct BatchCostEstimator { + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + struct Candidate { + SubCommandT command; + bool isActive; + }; + + virtual std::vector estimateCosts(const Time& time, + const EnvironmentModelT& environmentModel, + const std::vector& candidates) = 0; +}; + +template +class PerOptionToBatchAdapter : public BatchCostEstimator { +public: + using CandidateT = typename BatchCostEstimator::Candidate; + using CostEstimatorT = CostEstimator; + explicit PerOptionToBatchAdapter(typename CostEstimatorT::Ptr perOptionEstimator) + : perOptionEstimator_(std::move(perOptionEstimator)) { + } + std::vector estimateCosts(const Time& time, + const EnvironmentModelT& environmentModel, + const std::vector& candidates) override { + std::vector costs; + costs.reserve(candidates.size()); + for (const auto& candidate : candidates) { + costs.push_back( + perOptionEstimator_->estimateCost(time, environmentModel, candidate.command, candidate.isActive)); + } + return costs; + } + +private: + typename CostEstimatorT::Ptr perOptionEstimator_; +}; + template class CostArbitrator : public Arbitrator { public: @@ -29,6 +71,8 @@ class CostArbitrator : public Arbitrator; using ConstPtr = std::shared_ptr; + using BatchCostEstimatorT = BatchCostEstimator; + using CandidateT = typename BatchCostEstimatorT::Candidate; using CostEstimatorT = CostEstimator; using PlaceboVerifierT = verification::PlaceboVerifier; using VerifierT = verification::Verifier; @@ -43,20 +87,19 @@ class CostArbitrator : public Arbitrator::Ptr& behavior, const FlagsT& flags, - const typename CostEstimatorT::Ptr& costEstimator) + const typename BatchCostEstimatorT::Ptr& costEstimator) : ArbitratorBase::Option(behavior, flags), costEstimator_{costEstimator} { } - double estimateCost(const Time& time, - const EnvironmentModelT& environmentModel, - const SubCommandT& command, - bool isActive) const { - double cost = costEstimator_->estimateCost(time, environmentModel, command, isActive); - lastEstimatedCost_ = cost; - return cost; + typename BatchCostEstimatorT::Ptr costEstimator() const { + return costEstimator_; + } + + std::optional lastEstimatedCost(const Time& time) const { + return lastEstimatedCost_.cached(time); } - void resetLastEstimatedCost() const { - lastEstimatedCost_.reset(); + void cacheLastEstimatedCost(const Time& time, const double& cost) const { + lastEstimatedCost_.cache(time, cost); } /*! @@ -88,8 +131,8 @@ class CostArbitrator : public Arbitrator lastEstimatedCost_; + typename BatchCostEstimatorT::Ptr costEstimator_; + mutable util_caching::Cache lastEstimatedCost_; }; @@ -97,12 +140,20 @@ class CostArbitrator : public Arbitrator()) : ArbitratorBase(name, verifier) {}; + void addOption(const typename Behavior::Ptr& behavior, + const typename Option::FlagsT& flags, + const typename BatchCostEstimatorT::Ptr& batchCostEstimator) { + typename Option::Ptr option = std::make_shared