navigateTo.hpp

#ifndef NAVIGATETO_HPP
#define NAVIGATETO_HPP

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

typedef enum {
    ENGAGECONF,
    DISENGAGECONF
} navigateToFileType;

class NavigateTo : public PrivateTask
{
    MOVE3D_STATIC_LOGGER;
public:
    NavigateTo();

    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);

    bool initAll();
    bool initialize();
    bool finalize();
    bool findCandidateSolutions();
    bool getRandomSol();
    bool findConfigurations();
    bool findTrajectories();
    bool computeSolutionTrajectories(int alternativeId);
//    virtual bool findAlternative();
    bool setToSolution(int solutionId, bool computeMP);
    double computeCost(int altId);
//    virtual vector<TaskSubSolution *> setNextTraj(TaskSubSolution* doneTSS);
    bool setAgents(std::multimap<std::string,std::string> agents_name);
    bool setObjects(std::multimap<std::string,std::string> objects_name);
    bool setPoints(std::multimap<std::string,p3d_point> points);
    bool setdata(std::multimap<std::string,std::string> additionalData);
    void smoothSolution(int alternativeId);
    std::string getDescr();
    std::string getTextFromValues();

    //0 None, 1 Right, 2 left, 3, both
    int checkHandAttachement();

    static bool compareCells (GTP2DCell* i, GTP2DCell* j);
    static double combineCosts(double distTraveled, double angleAndRot, double distToObj);

    confPtr_t getRandomArmPosFromFileBasedOnConf(navigateToFileType fileType, confPtr_t q_tmp);

private:

    p3d_point _targetPoint; // the point to reach (defined by either an object, an agent or a point)

    //the needed confs
    confPtr_t _disengagedConf;      // from init conf, back a little
    confPtr_t _navigationConf;      // from _disengagedConf switch to a navigation conf
    confPtr_t _approachConf;        // after navigation conf
    confPtr_t _manipulationConf;    // deploy at the same place as _approachConf in order to manipulate
    confPtr_t _targetConf;          // final configuration
    GTP2DGrid* _grid;

    std::vector<p3d_point> _disengageTraj;   //between initconf          and _disengagedConf
    p3d_traj* _squizTraj;                    //between _disengagedConf   and _navigationConf
    std::vector<p3d_point> _traj;            //between _navigationConf   and _approachConf
    p3d_traj* _deployTraj;                   //between _approachConf     and _manipulationConf
    std::vector<p3d_point> _engageTraj;      //between _manipulationConf and _targetConf

    double _distToDeploy;
    double _distTodisengage;

    bool isTargetAPoint;
    Robot* _targetObject;

    std::vector<GTP2DCell*> _reachableCells;

    unsigned int _idCurrentSol;

    bool _engage;
    bool _disengage;

};

#endif // NAVIGATE_HPP