give.hpp

#ifndef GIVE_HPP
#define GIVE_HPP

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

#define MAX_NBR_TEST 1000

class Give : public CooperativeTask
{
    MOVE3D_STATIC_LOGGER;
public:
    Give();
    bool initAll();
    bool initialize();
    bool finalize();
    bool findCandidateSolutions();
    bool getRandomSol();
    bool findConfigurations();
    bool findTrajectories();
    bool computeSolutionTrajectories(TaskSolution *TS);
    virtual TaskSolution *createSolution(bool computeMP);
//    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);
    bool setSupportObject(std::multimap<std::string,std::string> objects_name);
    void smoothSolution(int alternativeId);
    std::string getDescr();
    std::string getTextFromValues();

    bool setTargetPoint(std::multimap<std::string,p3d_point> points);


    bool findCurrentTestPoint();
    configPt findObjectConf();

    bool checkTargetReach();





protected:

    gpGrasp _curGrasp;

    p3d_traj* approachTraj;

    confPtr_t _objectFinalConf;
    confPtr_t _agentFinalConf;


    bool _hasTargedPoint;
    p3d_point _targetPoint;

    p3d_point _currentTest;

//    vector<int> targetHandIds;
    std::vector<std::pair<int, p3d_point> > _targetShoulder;
    double _mainAgentReach;
    double _targetAgentReach;
    p3d_point _mainAgentShoulder;

    int _currTargetHandId;

    int maxTests;
    WorldStateFacts *_WSF;

};


#endif // GIVE_HPP