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