fixOnWall.hpp

#ifndef FIXONWALL_HPP
#define FIXONWALL_HPP

#include "GTP/Tasks/manipulationTable/manipulationTask.hpp"
#include "Planner-pkg.h"

class fixOnWall : public ManipulationTask
{
    MOVE3D_STATIC_LOGGER;
public:
    fixOnWall();
    bool initAll();

    bool run(std::multimap<std::string,std::string> agents_name,
             std::multimap<std::string,std::string> objects_name,
             std::multimap<std::string,p3d_point> point3d,
             std::multimap<std::string,std::string> additionalData,
             WorldState* WS);

    virtual bool findCandidateSolutions();
    virtual bool getRandomSol();
    virtual bool findConfigurations();
    virtual bool findTrajectories();
    virtual bool computeSolutionTrajectories(TaskSolution *TS);
    virtual TaskSolution *createSolution(bool computeMP);
//    virtual vector<TaskSubSolution *> setNextTraj(TaskSubSolution* doneTSS);
    virtual bool setAgents(std::multimap<std::string,std::string> agents_name);
    virtual bool setObjects(std::multimap<std::string,std::string> objects_name);
    virtual bool setPoints(std::multimap<std::string,p3d_point> points);
    virtual bool setdata(std::multimap<std::string,std::string> additionalData);
    virtual void smoothSolution(int alternativeId) {std::cout << "not implemented" << std::endl;}
    virtual std::string getDescr();
    virtual std::string getTextFromValues();

    double getConfigCost();


    virtual void showMainObjectRandomTransform() {}


    Robot* getMainAgent() {return mainAgent;}
private:
    GTP2DGrid* _grid;


    Robot* targetAgent;


    std::vector<GTP2DCell*> mainReach;
    std::vector<GTP2DCell*> targetReach;

    GTP2DCell* currMain;
    GTP2DCell* currTarget;
    std::vector<std::pair<GTP2DCell*,GTP2DCell*> > failedPairs;


    p3d_point _target1;
    p3d_point _target2;

    confPtr_t mainEndConf;
    confPtr_t targetEndConf;


};

#endif // FIXONWALL_HPP