MultiPathPlanner.h

#ifndef MULTIPATHPLANNER_H
#define MULTIPATHPLANNER_H

#include <vector>
#include "HRI_costspace/HRICS_MultiHandOver.h"
#include "MultiHandOverUtils.hpp"
#include "utils/MultiHandOverAlgos.h"
#include "utils/multiHandOver/LazyWAstar.h"

class MultiPathLWANode : public mho::LWANodeBase
{
public:
    MultiPathLWANode();
    MultiPathLWANode(std::vector<unsigned int> agents, std::vector<HRICS::MultiAgentCell*> positions,
                     HRICS::MHO::AbstractSolutionTools *sol_tools , bool allow_delay=false);
    MultiPathLWANode(std::vector<unsigned int> agents, std::vector<HRICS::MultiAgentCell*> positions,
                     HRICS::MHO::AbstractSolutionTools *sol_tools,
                     std::deque<mho::LWANodeInterface*> *prev_paths, std::deque<mho::LWANodeInterface*>::iterator prev_path_current, bool allow_delay=false);
    MultiPathLWANode(MultiPathLWANode *other);
    virtual ~MultiPathLWANode();
    virtual mho::LWANodeInterface* clone();

    virtual std::vector<LWANodeInterface*> sons();

    virtual double edgeCostEstimationFrom(LWANodeInterface *parent);
    virtual double computeHeuristic(LWANodeInterface *goal);
    virtual double computeTrueCost(LWANodeInterface *other);
    virtual bool isSameConf(LWANodeInterface *other);

    double getDurationMax(){return _duration_max;}

    static MultiPathLWANode *cast(LWANodeInterface *p){return dynamic_cast<MultiPathLWANode *>(p);}

    HRICS::MultiAgentCell* cell(unsigned int i){return (i<_positions.size() ? _positions[i] : 0);}
    void cell(unsigned int i, HRICS::MultiAgentCell*c){ if(i<_positions.size()){ _positions[i]=c;}}
    int agent_index(unsigned int i){return (i<_agents_indices.size() ? _agents_indices[i] : -1);}
    void agent_index(unsigned int i, unsigned int a){ if(i<_agents_indices.size()){ _agents_indices[i]=a;}}
    typedef enum {NEVER,BEGIN,END} FreeWaiting_T;
    void freeWainting(unsigned int i,FreeWaiting_T when){if(i<_agents_indices.size()) _wait_is_free[i]=when;}
    FreeWaiting_T freeWainting(unsigned int i){return (i<_wait_is_free.size() ? _wait_is_free[i] : NEVER);}
    void setAllowDelay(bool b){_allow_delay=b;}
    bool getAllowDelay(){return _allow_delay;}
    void setStartGoal(MultiPathLWANode *start,MultiPathLWANode* goal){_start=start;_goal=goal;}

private:
    std::vector<unsigned int> _agents_indices;
    std::vector<HRICS::MultiAgentCell*> _positions;
    MultiPathLWANode *_start,*_goal;
    double _duration_max;
    bool _allow_delay; 
    std::vector<FreeWaiting_T> _wait_is_free;

    HRICS::MHO::AbstractSolutionTools *_solution_tools;
    std::deque<mho::LWANodeInterface*> *_prev_path;
    std::deque<mho::LWANodeInterface*>::iterator _prev_path_current;

};

class MultiPathPlannerLWADisplayInterface : public mho::DisplaySearchAstarInterface
{
    MOVE3D_STATIC_LOGGER;
public:
    MultiPathPlannerLWADisplayInterface(bool show_text=true,bool draw=false,HRICS::MultiAgentGrid *grid=0):
        _showText(show_text), _draw(draw), _grid(grid),
        _open_c(0),_closed_c(0)
    {}

    void setShowText(bool b){this->_showText=b;}
    void setDraw(bool b,HRICS::MultiAgentGrid* grid){this->_draw=b;this->_grid=grid;}

    virtual void onOpen(mho::LWANodeInterface* open);
    virtual void onClose(mho::LWANodeInterface* closed);
    virtual void onFound(mho::LWANodeInterface* goal);
    virtual void onFail();
    virtual void onAbort();
    virtual void onEnd(std::vector<std::pair<unsigned int,double> > times);

private:
    bool _showText;
    bool _draw;
    vector<HRICS::MultiAgentCell*> _cells_for_draw;
    HRICS::MultiAgentGrid *_grid;
    double _open_c;
    double _closed_c;
};

class MultiPathPlanner
{
    MOVE3D_STATIC_LOGGER;
public:
    MultiPathPlanner();
    ~MultiPathPlanner();

    bool compute(double time_limit_ms=-1);

    std::vector<HRICS::MultiAgentCell*> findParking(const std::deque<mho::LWANodeInterface *> &existing_path, unsigned int agent_to_park);

    std::deque<mho::LWANodeInterface*> getPath();

    void reset();

    void setAgents(std::vector<unsigned int> agents_ids);
    void setStart(std::vector<HRICS::MultiAgentCell*> cells);
    void setStart(unsigned int index, HRICS::MultiAgentCell* start);
    void setStartByID(unsigned int agent_id,HRICS::MultiAgentCell* start);
    void setGoal(std::vector<HRICS::MultiAgentCell*> cells);
    void setGoal(unsigned int index, HRICS::MultiAgentCell* goal);
    void setGoalByID(unsigned int agent_id,HRICS::MultiAgentCell* goal);

    void useParkingAreas(bool b,HRICS::MultiAgentGrid* grid){_park=b;_grid=grid;}

    void setSolutionTools(HRICS::MHO::AbstractSolutionTools *sol_tools);

    void setDisplay(bool showText,bool drawGrid=false,HRICS::MultiAgentGrid* grid=0);

    void setHierarchy(bool h){_hierachy=h;}
    bool getHierarchy(){return _hierachy;}

private:
    bool _hierachy;
    bool _park;
    std::vector<unsigned int> _agents_indices;
    std::vector<HRICS::MultiAgentCell*> _start_pos;
    std::vector<HRICS::MultiAgentCell*> _goal_pos;

    double _epsilon;
    mho::LazyWeightedAstar *_lwa;
    MultiPathLWANode *_start,*_goal;

    MultiPathPlannerLWADisplayInterface *_display;
    HRICS::MHO::AbstractSolutionTools *_solution_tools;
    HRICS::MultiAgentGrid *_grid;
};

class MultiPathCheck
{
public:
    MultiPathCheck(HRICS::MultiAgentGrid* grid, HRICS::MHO::Task* task,
                   std::map<HRICS::MHO::MultiHandOverAgent*,HRICS::MultiAgentCell*> start_state);
    bool check();
    std::vector<std::pair<HRICS::MHO::SubTask*,HRICS::MHO::SubTask*> > & getTasksInCollision(){return _colliding_stk;}
    void setTask(HRICS::MHO::Task task);
private:
    HRICS::MHO::Task* _tk;
    std::map<HRICS::MHO::MultiHandOverAgent*,HRICS::MultiAgentCell*> _start_state;
    HRICS::MultiAgentGrid *_grid;
    std::vector<std::pair<HRICS::MHO::SubTask*,HRICS::MHO::SubTask*> > _colliding_stk;
};

#endif // MULTIPATHPLANNER_H