MultiAgentHandOvers.h

#ifndef MULTIAGENTHANDOVERS_H
#define MULTIAGENTHANDOVERS_H

#include "MultiHandOverUtils.hpp"
#include "ConfGenerator.h"
#include "Chronometer.hpp"
#include <map>

namespace HRICS {
namespace MHO{
class AbstractHandOverTest{
public:
    //AbstractHandOverTest();
    virtual ~AbstractHandOverTest(){}
    virtual bool check(FrontierPtr frontier)=0;
    virtual std::string getName()=0;
    virtual FrontierCells::FrontierCheckStatus getCheckStatusLevel()=0;
    virtual FrontierCells::FrontierCheckStatus getRequiredCheckStatus()=0;
};

class DummyHandOverTest : public AbstractHandOverTest{
public:
    DummyHandOverTest(FrontierCells::FrontierCheckStatus req,FrontierCells::FrontierCheckStatus lvl):
        _req(req),_lvl(lvl) { }
    virtual ~DummyHandOverTest(){}
    virtual bool check(FrontierPtr frontier){
        bool ok =false;
        if (frontier->check_status>=_req && frontier->status_ok[_req]){
            ok= true;
        }else{
            ok= false;
        }
        frontier->check_status=_lvl;
        frontier->status_ok[_lvl]=ok;
        return ok;
    }
    virtual std::string getName(){return "dummy";}
    virtual FrontierCells::FrontierCheckStatus getCheckStatusLevel(){return _lvl;}
    virtual FrontierCells::FrontierCheckStatus getRequiredCheckStatus(){return _req;}
private:
    FrontierCells::FrontierCheckStatus _req,_lvl;

};

class FrontierTester{
public:
    FrontierTester();
    virtual ~FrontierTester();
    bool test(FrontierPtr frontier,std::string const & method);
    bool test(FrontierPtr frontier, FrontierCells::FrontierCheckStatus target_status);
    void addTester(AbstractHandOverTest* tester,bool is_default=false);
    void addDefault(std::string const & name);
    AbstractHandOverTest* getDefault(FrontierCells::FrontierCheckStatus target_status);
    AbstractHandOverTest* getTester(std::string const & name);
    std::vector< std::pair<std::string,bool> > listTesterNames(bool default_only=false);
    std::vector< std::pair<AbstractHandOverTest*,bool> > listTesters(bool default_only=false);

    unsigned int getCheckCount(FrontierCells::FrontierCheckStatus st){return _check_counts[st].first;}
    double getCheckTime(FrontierCells::FrontierCheckStatus st){return _check_counts[st].second;}
private:
    std::map<std::string,AbstractHandOverTest*> _testers;
    std::map<FrontierCells::FrontierCheckStatus,std::string> _defaults;
    std::map<FrontierCells::FrontierCheckStatus,std::pair<unsigned int,double> > _check_counts;
    Chronometer _chrono;
};

class AbstracHandOverObjectCheck{
public:
    AbstracHandOverObjectCheck(Robot* object,Robot* r1=0,Robot* r2=0):_object(object),
        _robots(std::make_pair(r1,r2)){
        setLogger(&_dummy_logger);
    }
    virtual ~AbstracHandOverObjectCheck(){}
    virtual bool check() =0;

    void timeLimit(double time_limit_ms){_time_limit=time_limit_ms;}
    double timeLimit(){return _time_limit;}
    void object(Robot* obj){_object=obj;}
    Robot* object(void){return _object;}
    void shoulders(Eigen::Vector3d shoulder1,Eigen::Vector3d shoulder2){this->_shoulder1=shoulder1;this->_shoulder2=shoulder2;}
    Eigen::Vector3d shoulder1(){return _shoulder1;}
    Eigen::Vector3d shoulder2(){return _shoulder2;}
    void reach(double r1,double r2){_reach1=r1;_reach2=r2;}
    double reach1(){return _reach1;}
    double reach2(){return _reach2;}
    void grabs(Eigen::Vector3d& g1,Eigen::Vector3d& g2){_grab1=g1;_grab2=g2;}
    Eigen::Vector3d& grab1(){return _grab1;}
    Eigen::Vector3d& grab2(){return _grab2;}

    void robots(Robot* r1,Robot* r2){_robots=std::make_pair(r1,r2);}
    inline std::pair<Robot*,Robot*>& robots(){return _robots;}
    virtual confPtr_t getQMiddle() =0;
    void setLogger(AlgoLoggerInterface* logger){_logger=logger;}
    virtual std::string getName()=0;
private:
    AlgoLoggerInterface *_logger;
    double _time_limit;
    Robot* _object;
    std::pair<Robot*,Robot*> _robots;
    Eigen::Vector3d _shoulder1,_shoulder2;
    Eigen::Vector3d _grab1,_grab2;
    double _reach1,_reach2;
    AlgoLoggerInterface _dummy_logger;
};
class HandOverTestDistance : public AbstractHandOverTest
{
public:
    HandOverTestDistance(std::vector<MultiHandOverAgent> * agents);
    virtual ~HandOverTestDistance(){}
    virtual bool check(FrontierPtr frontier);
    virtual bool check(unsigned int a1, unsigned int a2, MultiAgentCell* c1,MultiAgentCell* c2);
    virtual std::string getName(){return _name;}
    virtual FrontierCells::FrontierCheckStatus getCheckStatusLevel(){return FrontierCells::FCS_NONE;}
    virtual FrontierCells::FrontierCheckStatus getRequiredCheckStatus(){return FrontierCells::FCS_NONE;}
private:
    std::vector<MultiHandOverAgent> *_agents;
    static const std::string _name;
};

class HandOverTestObject: public AbstractHandOverTest
{
public:
    HandOverTestObject(AbstracHandOverObjectCheck* object_check,std::vector<MultiHandOverAgent> *agents);
    virtual ~HandOverTestObject(){}
    virtual bool check(FrontierPtr frontier);
    virtual std::string getName(){return _object_check->getName();}
    virtual FrontierCells::FrontierCheckStatus getCheckStatusLevel(){return FrontierCells::FCS_OBJECT;}
    virtual FrontierCells::FrontierCheckStatus getRequiredCheckStatus(){return FrontierCells::FCS_NONE;}

private:
    AbstracHandOverObjectCheck* _object_check;
    std::vector<MultiHandOverAgent> *_agents;
};

class HandOverTestGrabs: public AbstractHandOverTest
{
public:
    HandOverTestGrabs(std::vector<MultiHandOverAgent> *agents,Robot **object);
    virtual ~HandOverTestGrabs(){}
    virtual bool check(FrontierPtr frontier);
    virtual std::string getName(){return _name;}
    virtual FrontierCells::FrontierCheckStatus getCheckStatusLevel(){return FrontierCells::FCS_GRAB_CONFS;}
    virtual FrontierCells::FrontierCheckStatus getRequiredCheckStatus(){return FrontierCells::FCS_OBJECT;}
private:
    std::vector<MultiHandOverAgent> *_agents;
    Robot **_object_pt_pt;
    static const std::string _name;
};


class HandOverTestPreDefined : public AbstractHandOverTest {
public:
    HandOverTestPreDefined(std::vector<MultiHandOverAgent> *agents, std::string file_directory,double grid_discrete_step);
    virtual ~HandOverTestPreDefined();
    virtual bool check(FrontierPtr frontier);
    virtual std::string getName(){return _name;}
    virtual FrontierCells::FrontierCheckStatus getCheckStatusLevel(){return FrontierCells::FCS_HANDOVER;}
    virtual FrontierCells::FrontierCheckStatus getRequiredCheckStatus(){return FrontierCells::FCS_GRAB_CONFS;}
private:

    struct _ConfGenAlias{
        ConfGenerator* conf_generator; 

        std::vector<HRICS::ConfigHR>  conf_list;
        MultiHandOverAgent *alias_human, *alias_robot;  
        bool use_dummy;
    };
    _ConfGenAlias *_initGetConfGen(MultiHandOverAgent *a1, MultiHandOverAgent *a2);

    std::vector<MultiHandOverAgent> *_agents;
    std::string _file_dir;
    std::map<pair<MultiHandOverAgent*,MultiHandOverAgent*> , _ConfGenAlias> _conf_generators_alias;

    std::pair<double,double> _robot_robot_min_max_dists,_human_robot_min_max_dists;

    static const std::string _name;
    double _grid_pace;
};

class HandOverTestVisible: public AbstractHandOverTest
{
public:
    HandOverTestVisible(std::vector<MultiHandOverAgent> *agents, Robot *object);
    virtual ~HandOverTestVisible() {}
    virtual bool check(FrontierPtr frontier);
    virtual std::string getName() {return _name;}
    virtual FrontierCells::FrontierCheckStatus getCheckStatusLevel(){return FrontierCells::FCS_HANDOVER;}
    virtual FrontierCells::FrontierCheckStatus getRequiredCheckStatus(){return FrontierCells::FCS_GRAB_CONFS;}

private:

    std::vector<MultiHandOverAgent> *_agents;
    Robot *_object_pt;
    static const std::string _name;
};

class HandOverTestSelection : public AbstractHandOverTest
{
public:
    HandOverTestSelection(std::vector<MultiHandOverAgent> *agents, Robot *object,std::string file_directory,double grid_discrete_step);
    HandOverTestSelection(std::vector<MultiHandOverAgent> *agents, Robot *object,HandOverTestPreDefined* predef,HandOverTestVisible* visib,bool own_tester=false);
    virtual ~HandOverTestSelection();
    virtual bool check(FrontierPtr frontier);
    virtual std::string getName() {return _name;}
    virtual FrontierCells::FrontierCheckStatus getCheckStatusLevel(){return FrontierCells::FCS_HANDOVER;}
    virtual FrontierCells::FrontierCheckStatus getRequiredCheckStatus(){return FrontierCells::FCS_GRAB_CONFS;}

private:
    std::vector<MultiHandOverAgent> *_agents;
    Robot *_object_pt;
    HandOverTestPreDefined *_test_pre;
    HandOverTestVisible *_test_vis;

    static const std::string _name;
    bool _is_owner;
};

class HandOverTestTrajectory : public AbstractHandOverTest
{
public:
    HandOverTestTrajectory(std::vector<MultiHandOverAgent> *agents);
    virtual ~HandOverTestTrajectory(){}
    virtual bool check(FrontierPtr frontier);
    virtual std::string getName(){return _name;}
    virtual FrontierCells::FrontierCheckStatus getCheckStatusLevel(){return FrontierCells::FCS_TRAJECTORIES;}
    virtual FrontierCells::FrontierCheckStatus getRequiredCheckStatus(){return FrontierCells::FCS_HANDOVER;}

protected:
    virtual bool computeTraj(Robot *r,confPtr_t start, confPtr_t target, API::Trajectory *traj);

private:
    std::vector<MultiHandOverAgent> *_agents;
    static const std::string _name;
};
}
}

#endif // MULTIAGENTHANDOVERS_H