moveTo.hpp

#ifndef MOVETO_HPP
#define MOVETO_HPP

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

typedef enum {
    SAVEDCONF,
    GIVENCONF,
    CARTESIAN6DPOINT,
    CARTESIANNDPOINT, // the point can be defined as 4,5 or 6D depending on the needs
    NOTDEFINED
} moveToStrategy;

class MoveTo : public PrivateTask
{
    MOVE3D_STATIC_LOGGER;
public:
    MoveTo();
    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 TaskSolution* findAlternative(bool computeMP);

    virtual bool initAll();
    virtual bool initialize();
    virtual bool finalize();
    virtual bool findCandidateSolutions();
    virtual bool getRandomSol();
    virtual bool findConfigurations();
    virtual bool findTrajectories();
    virtual bool computeSolutionTrajectories(int alternativeId);
    virtual bool computeSolutionTrajectories(TaskSolution *TS);
//    virtual bool findAlternative();
    virtual TaskSolution *createSolution(bool computeMP);
    virtual bool setToSolution(int solutionId, 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);
    virtual std::string getDescr();
    virtual std::string getTextFromValues();

    std::map<std::string, std::vector<double> > getConfiguration(std::string configurationName);
    confPtr_t getConf(Robot* r, std::map<int, std::vector<double> >  toChange);
    confPtr_t computeConfFromPoint(Robot* r, std::vector<double> v, int armId);
private:
    unsigned int current_grasp_id;
    std::vector<gpGrasp> graspList;
//    int _armId;

    p3d_traj* _traj;
    confPtr_t _targetConf;
    std::map<std::string,std::vector<double> > _fileInfo;

    std::string _configurationName;

    int armUsed;

    moveToStrategy _useConf;
    std::vector<double> _givenSol;

};

#endif // MOVETO_HPP