planner.h

Go to the documentation of this file.

#ifndef PLANNER_H
#define PLANNER_H
 
#include "task.h"
#include "graphTools.h"
#include <algorithm>
 
//typedef std::pair<vertexDescriptor, std::vector<vertexDescriptor>> Frontier;
 
 
struct Frontier{
    vertexDescriptor frontier=TransitionSystem::null_vertex();
    std::vector<vertexDescriptor> connecting;
 
    Frontier()=default;
 
    Frontier(vertexDescriptor _f, const std::vector<vertexDescriptor>&_c):frontier(_f), connecting(_c){}
 
    bool operator==(const Frontier &f)const{
        return frontier==f.frontier && connecting==f.connecting;
    }
};
 
 
struct ComparePhi{
 
    ComparePhi(){}
 
    bool operator()(const std::pair<State*, Frontier>& p1, const std::pair<State*, Frontier>& p2) const{
        return (*p1.first).phi<(*p2.first).phi;
    }
};
 
 
class ExecutionInfo{
    private:
    vertexDescriptor m_currentVertex=0, m_goalVertex=TransitionSystem::null_vertex();
    Task m_currentTask;
    Task m_overarchingGoal; 
    bool m_been; //has a plan been made to fulfill this overarching goal before?
    std::vector <vertexDescriptor> m_plan;
    protected:
    friend class Configurator;
 
    void been(bool b){m_been=b;}
 
    void goalVertex(vertexDescriptor gv){m_goalVertex=gv;}
    
 
    public:
    ExecutionInfo(){}
 
    ExecutionInfo( vertexDescriptor _cv, vertexDescriptor _goal, Task & _ct, Task & _gt, bool _been, std::vector<vertexDescriptor> _plan){
        m_currentVertex=_cv;
        m_goalVertex=_goal;
        m_currentTask=_ct;
        m_overarchingGoal=_gt;
        m_been=_been;
        m_plan=_plan;
    }
    void overarchingGoal(const Task & og){m_overarchingGoal=og;}
 
 
    vertexDescriptor currentVertex()const{return m_currentVertex;}
 
    vertexDescriptor goalVertex()const{return m_goalVertex;}
 
    Task& currentTask(){return m_currentTask;}
 
    Task& overarchingGoal() {return m_overarchingGoal;}
 
    bool been()const{return m_been;}
 
    std::vector<vertexDescriptor> plan()const{return m_plan;}
 
};
 
 
std::vector <Frontier> frontierVertices(vertexDescriptor v, TransitionSystem& g, ExecutionInfo & info); //returns the closest vertices to the start vertex which are reached by executing a task of the specified direction
 
float evaluationFunction(EndedResult er, const vertexDescriptor &v, std::vector<vertexDescriptor>& p);
 
EndedResult estimateCost(const State &state, b2Transform start, Direction d, Task & _goal); //returns whether the controlGoal has ended and fills node with cost and error
 
 
class Planner{
    protected:
 
    public:
 
    Planner()=default;
 
    virtual std::vector<vertexDescriptor> plan(TransitionSystem g, vertexDescriptor src, ExecutionInfo & info, bool *finished)=0;
 
 
};
 
class HorizonStarPlanner:public Planner{
    protected:
 
    void path2add2(std::vector<std::vector<vertexDescriptor>>::reverse_iterator & path, const std::vector <vertexDescriptor> & add, std::vector<std::vector<vertexDescriptor>> &paths, TransitionSystem &g);
 
    std::vector <vertexDescriptor> best_path(const std::vector<std::vector<vertexDescriptor>>& paths, vertexDescriptor goal,  vertexDescriptor cv,  bool change, const TransitionSystem& g);
 
 
    void addToPriorityQueue(const Frontier &f, std::vector<Frontier>& queue, TransitionSystem &g,const  std::set<vertexDescriptor>&closed, vertexDescriptor goal=TransitionSystem::null_vertex());
 
    // struct CostMap{
    //     std::map<vertexDescriptor, float> map;
 
    //     void init(TransitionSystem & g){
    //         auto vs=boost::vertices(g);
    //         for (auto vi=vs.first; vi!=vs.first; vi++){
    //             if (g[*vi].visited()){
    //                 map.emplace(std::make_pair(*vi,g[*vi].phi));
    //             }
    //         }
    //     }
    // }costMap;
    
public:
 
    std::vector<vertexDescriptor> plan(TransitionSystem g, vertexDescriptor src, ExecutionInfo & info, bool * finished=NULL)override;
 
};
 
class NoPlanner:public Planner{
 
    std::vector<vertexDescriptor> plan(TransitionSystem g, vertexDescriptor src, ExecutionInfo & info, bool * finished=NULL)override{
        return std::vector<vertexDescriptor>();
    }
 
 
};
 
#endif