task.hpp

#ifndef TASK_HPP
#define TASK_HPP


#include "GTP/GTPTools/grid.hpp"
#include "GTP/GTPTools/worldState.hpp"
#include "GTP/GTPTools/geometricTools/geometricComputations.hpp"
#include "GTP/GTPTools/geometricTools/geometricForms-pkg.hpp"
#include "GTP/GTPTools/taskConstraints.hpp"
#include "GTP/GTPTools/solutionTools/structTools.hpp"
#include "lightPlanner/proto/ManipulationPlanner.hpp"

#include "Logging/Logger.h"

#include "database/semantics/Sem_SemanticsManager.hpp"



#include "move3d-headless.h"
#include "Planner-pkg.h"
#include "Collision-pkg.h"
#include "planner/planEnvironment.hpp"

#include "API/Device/robot.hpp"
#include "GTP/GTPTools/solutionTools/gtpPlan.hpp"


#include <iostream>
#include <map>



#define MAXIKLOOP 200

class WorldState;
class TaskSubSolution;

template < typename T > std::string toString( const T& n );
class TaskAllSolutions;
class TaskSolution;
class TaskConstraints;
class PlanTree;

class Task
{
    MOVE3D_STATIC_LOGGER;
public:
    Task();
    virtual bool initAll() = 0;
    virtual bool initialize() = 0;
    virtual bool finalize() = 0;
    virtual bool findCandidateSolutions() = 0;
    virtual bool findConfigurations() =0;
    virtual bool findTrajectories() =0;
    virtual bool setToSolution(int solutionId, bool computeMP) = 0;
    virtual bool testConstraints(int alternativeId) = 0;
    //validatePoint();
    virtual bool setAgents(std::multimap<std::string,std::string> agents_name) = 0;
    virtual bool setObjects(std::multimap<std::string,std::string> objects_name) = 0;
    virtual bool setPoints(std::multimap<std::string,p3d_point> points) = 0;
    virtual bool setdata(std::multimap<std::string,std::string> additionalData) = 0;
    virtual 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, bool computeMP);
    virtual double heuristic(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, bool computeMP);
    virtual TaskSolution* findAlternative(bool computeMP) = 0;
    virtual double computeCost(int altId) = 0;
    virtual double computeHeuristicCost();
    virtual bool computeSolutionTrajectories(int alternativeId) = 0;
    p3d_traj* compute_traj(confPtr_t q_init, confPtr_t q_goal, int& status);
    virtual bool computeManipulationConfs(confPtr_t& approachC, confPtr_t& graspC, confPtr_t& graspOC, confPtr_t& escapeC, p3d_matrix4 objectPosMat,
                                          gpGrasp grasp, p3d_vector3 approachDir, double approachDist , p3d_vector3 escapeDir, double escapeDist);
    virtual bool computeInverseKinematic(Robot* ag, p3d_matrix4 target_pos, confPtr_t &solution, int armId, bool fromCurPos);
    p3d_traj* compute_traj_using_localPath(confPtr_t q_init, confPtr_t q_goal, int& status);
//    virtual vector<TaskSubSolution*> setNextTraj(TaskSubSolution* doneTSS) =0;
    virtual void smoothSolution(int alternativeId) = 0;
    int getId();
    void setId(int id);
    std::string getType() { return type; }
    TasksId getTypeId() { return typeId; }
    virtual std::string getDescr() = 0;
    virtual std::string getTextFromValues() = 0;
    TaskSubSolution* getSubSolAt(int alternativeId, int id);
    int getSubSolNumberOfTrajs(int alternativeId);

    virtual double getSSSizeLeft();

    TaskAllSolutions* getResult(){return result; }
    void setResult(TaskAllSolutions* r){result = r; }

    void setConstraints(TaskConstraints* c) {_currConstraints = c;}
    TaskConstraints* getCurrConstraint(){return _currConstraints;}

    virtual std::vector<int> getJointsIdsToUse() {return std::vector<int>(0);}

    void setGraph(Graph* g) {_taskGraph = g;}
    Graph* getGraph() {return _taskGraph;}

    void setPlanTree(PlanTree* pt) {_planTree = pt;}
    PlanTree* getPlanTree() {return _planTree;}

    void setCurrentDataType(type_data pt) {currentDataType = pt;}
    type_data getCurrentDataType() {return currentDataType;}

    std::vector<SurfaceAttachement> createRecursiveAttachements(Robot* r);
    std::vector<SurfaceAttachement> createAndLoadSurfaceAttachements(Robot* r);
    void removeSurfaceAttachements(std::vector<SurfaceAttachement> s);


    SolutionData* SD;

protected:
    int task_id;
    std::string type;
    TasksId typeId;
    TaskAllSolutions* result;
    WorldState* _WS;

    int nbMaxLoops;
    TaskConstraints* _currConstraints;

    int _returnReportcount;
    int _returnReportcountMax;

    Graph* _taskGraph;
    PlanTree* _planTree;
    type_data currentDataType;

};
#endif // TASK_HPP