Planner.hh

#ifndef GRAIL_PLANNER_H
#define GRAIL_PLANNER_H
 
#include "WorldState/WorldState.hh"
#include "Action/Action.hh"
#include "../../GrailData/GrailDebugInfo/PlannerSnapshots.h"
 
#include <functional>
#include <memory>
#include <vector>
#include <queue>
#include <map>
 
namespace grail
{
    namespace planning
    {
        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(MemoryPool& memory);
            Planner(MemoryPool& memory, const WorldState& initialState, const Config& config = Config{});
 
            void SetIterationLimit(std::size_t iterationLimit);
            void SetMaxPlanLength(int maxDepth);
            void SetMaxPlanCost(double maxCost);
            void SetHeuristic(PlannerHeuristic heuristic);
            void ResetConfig(const Config& config);
 
            void PushCondition(ConditionFunction condition);
            AbstractPlan GetPartialPlan() const;
            PlannerIterationSnapshot GetPlanSnapshot(std::size_t goalNodeIndex, std::size_t iteration) const;
            void Iterate(std::size_t iterationCount) noexcept;
            void IterateWithSnapshot(std::size_t iterationCount, 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(const 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);
                Node(const WorldState& state);
                Node(WorldState&& state);
                Node& operator=(Node&& other);
            };
 
            struct Edge
            {
                Edge(std::size_t sourceIndex, std::size_t destinationIndex, Action&& action);
 
                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;
 
            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;
            MemoryPool& memory;
        };
    }
}
#endif //GRAIL_PLANNER_H