place.hpp

#ifndef PLACE_HPP
#define PLACE_HPP

#include "manipulationTask.hpp"
#include "Planner-pkg.h"


#define MAX_NBR_TEST 1000

class Place : public ManipulationTask
{
    MOVE3D_STATIC_LOGGER;
public:
    Place();
    bool initAll();
    virtual bool initialize();
    bool finalize();
    bool findCandidateSolutions();
    bool getRandomSol();
    bool findConfigurations();
    bool findTrajectories();
    bool computeSolutionTrajectories(TaskSolution *TS);
    virtual TaskSolution *createSolution(bool computeMP);
    bool testConstraints(int alternativeId);
//    virtual vector<TaskSubSolution*> setNextTraj(TaskSubSolution* doneTSS);
    virtual 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);
    bool setMainObject(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 findSupports();
    bool findRotations();
    std::vector<regionConst> getForceInConstraints();
    std::vector<regionConst> getForbidConstraints();
    bool findCurrentTestPoint();
    configPt findCurrentRotations();
    configPt findObjectConf();
    bool isObjectInSurface();
//    Robot* getMainObjectFromAtt();
//    int getArmIdFromAtt();
//    bool checkArmAtt();
//    bool checkAndSetArmAndObj();
    bool testFactConstraints();

    bool initMightabilities();
    bool getNextMightPoint();

    virtual double getSSSizeLeft();


protected:
    int maxTests;
    Robot* _supportObject;

    p3d_traj* approachTraj;
    p3d_traj* placeTraj;
    p3d_traj* escapeTraj;
    p3d_traj* releaseTraj;

    std::vector<GeometricForm*> _supports;
    std::vector<Robot*> _supportList;
    confPtr_t _confSol;

    bool noSupport; //false of a support is given


    bool _hasTargedPoint;
    p3d_point _targetPoint;

    p3d_point _currentTest;
    GeometricForm* _currentSurface;

    std::vector<configPt> _rotations;



    std::vector<MightabilitiesCell*> _cells;
    unsigned int _cellId;
};



#endif // PLACE_HPP