Planner.hh

// Copyright QED Software 2023.
 
#ifndef GRAIL_PLANNER_H
#define GRAIL_PLANNER_H
 
#include "../../GrailData/DebugInfo/PlannerSnapshots.h"
#include "Action/Action.hh"
#include "WorldState/WorldState.hh"
 
#include <cstdlib>
#include <functional>
#include <limits>
#include <map>
#include <memory>
#include <queue>
#include <vector>
 
namespace grail
{
    #undef max
    #undef min
    
namespace planner
{
    using ConditionFunction = std::function<bool(const WorldState&)>;
 
    using PlannerHeuristic = std::function<double(const WorldState&, const std::vector<ConditionFunction>&)>;
 
    class Planner
    {
    public:
        struct Config
        {
            Config() noexcept
            {
            }
 
            PlannerHeuristic heuristic = nullptr;
 
            std::size_t iterationLimit = 500;
 
            int maxPlanLength = -1;
 
            double maxPlanCost = -1;
 
            std::size_t initialNodeCapacity = 128;
        };
 
        struct AbstractPlan
        {
            std::vector<Action> actions{};
            float totalCost = 0.0f;
        };
 
        Planner();
 
        Planner(std::unique_ptr<WorldState> initialState, const Config& config = Config{});
 
        void SetIterationLimit(std::size_t iterationLimit);
 
        void SetMaxPlanLength(int maxLength);
 
        void SetMaxPlanCost(double maxCost);
 
        void SetHeuristic(PlannerHeuristic heuristic);
 
        void SetConfig(const Config& config);
 
        void PushCondition(ConditionFunction condition);
 
        AbstractPlan GetPartialPlan() const;
 
        data::PlannerIterationSnapshot GetPlanSnapshot(std::size_t goalNodeIndex, std::size_t iteration) const;
 
        void Iterate(std::size_t iterationCount) noexcept;
 
        void IterateWithSnapshot(std::size_t iterationCount,
                                 data::PlannerReasonerSnapshot& snapshot,
                                 std::size_t minDebugSnapshotIteration = 0,
                                 std::size_t maxDebugSnapshotIteration = std::numeric_limits<std::size_t>::max())
        noexcept;
 
        void SetPlanFoundCallback(const std::function<void(const AbstractPlan&)> callbackFunction);
 
        void SetPlanningFailedCallback(const std::function<void()> callbackFunction);
 
        void Reset(std::unique_ptr<WorldState> initialState);
 
        bool IsComputing() const;
 
    private:
        //helper structs start
        struct Edge;
 
        struct Node
        {
            WorldState state;
            int depth = 0;
            double score = 0;
            double distanceFromGoal = std::numeric_limits<double>::max();
            std::shared_ptr<Edge> pathEdge = nullptr;
 
            Node(Node&& other) noexcept;
            Node(const WorldState& state);
            Node(WorldState&& state);
            Node& operator=(Node&& other) noexcept;
        };
 
        struct Edge
        {
            Edge(std::size_t sourceIndex, std::size_t destinationIndex, Action&& action);
            Edge(Edge&& otherEdge) = default;
 
            std::size_t sourceIndex = 0;
            std::size_t destinationIndex = 0;
            Action action;
        };
 
        struct EdgePriority
        {
            EdgePriority(const std::shared_ptr<Edge>& edge, double dijkstraScore, double aStarScore);
 
            std::shared_ptr<Edge> edge;
            double dijkstraScore;
            double aStarScore;
        };
 
        class EdgePriorityComparer
        {
        public:
            bool operator()(const EdgePriority& one, const EdgePriority& other)
            {
                return one.aStarScore > other.aStarScore;
            }
        };
 
        //helper structs end
 
        void AddNode(Node&& node);
        bool IsGoalAchieved(const WorldState& state) const;
 
        std::unique_ptr<WorldState> initialState;
        Config plannerConfig = {};
 
        std::vector<ConditionFunction> goalConditions = {};
        std::function<void(const AbstractPlan&)> onPlanFound = nullptr;
        std::function<void()> onPlanningFailed = nullptr;
 
        //cached data for iterative approach
        std::size_t nextNodeIndex = 0;
        std::size_t nodeClosestToGoalIndex = std::numeric_limits<std::size_t>::max();
        std::size_t lastChosenNodeIndex = std::numeric_limits<std::size_t>::max();
        int maxNodeDepth = 0;
        std::priority_queue<EdgePriority, std::vector<EdgePriority>, EdgePriorityComparer> edges{};
        std::vector<Node> generatedNodes{};
        std::vector<std::size_t> closedNodeIndices{};
        std::size_t currentIteration = 0;
        bool isPlanningFinished = true;
    };
}
}
#endif //GRAIL_PLANNER_H