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 ResetConfig(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