point.hpp

#ifndef POINT_HPP
#define POINT_HPP

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


class Point : public PrivateTask
{
    MOVE3D_STATIC_LOGGER;
public:
    Point();
    virtual 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, bool computeMP);

    virtual bool findCandidateSolutions();
    virtual bool getRandomSol();
    virtual bool findConfigurations();
    virtual bool findTrajectories();
    virtual bool computeSolutionTrajectories(int alternativeId);
    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) {std::cout << "not implemented" << std::endl;}
    virtual std::string getDescr();
    virtual std::string getTextFromValues();


    virtual void showMainObjectRandomTransform() {}


    bool setArmToUse(armLabel arm_t);
    //this is a specific function used in case we need ids.
    int getArmId();

    Robot* getMainAgent() {return mainAgent;}
private:
    int _armId;
    p3d_point _mainPoint;
    p3d_point _targetPoint;
    Robot* _targetObject;
    armLabel m_arm;
    gpHand_type handType;
    ManipulationPlanner* Mp;

    confPtr_t _sol;

//    p3d_matrix4 current_test;
    double _dist;
    double _armReach;
    p3d_matrix4 _m;
    double cX, cY, cZ, cRx, cRy, cRz;
    double mX, mY, mZ, mRx, mRy, mRz;
    bool isNear;
    bool isStraight;

    double dist_to_move;
    double _nbTests;

    p3d_traj* _traj;
};

#endif // POINT_HPP