HRICS_MultiHandOver.h

#ifndef HRICS_HRICS_MULTIHANDOVER_H
#define HRICS_HRICS_MULTIHANDOVER_H

#include "HRICS_Workspace.hpp"
#include <vector>
#include "planner/planner.hpp"
#include "utils/multiHandOver/SimpleGraph.h"
#include "Grid/HRICS_MultiAgentGrid.hpp"
//#include "utils/MultiHandOverUtils.hpp"
#include "utils/MultiHandOverAlgos.h"
#include "utils/MultiHandOverDisplay.h"
#include "Logging/Logger.h"

namespace HRICS {

class MultiHandOver :public  HumanAwareMotionPlanner
{
    MOVE3D_STATIC_LOGGER;
public:

    //typedef HRICS::MultiHandOverSolution MultiHandOverSolution;
    typedef enum {NONE,RANDOM_SEARCH,LWA_SEARCH,WA_SEARCH} SearchAlgo_t;

    MultiHandOver(double cell_size=0.20);
    virtual ~MultiHandOver();

    void testForReinit();
    void createGrid();
    void dumpVar();
    void setDefaultEnvVar(void);
    bool initFromConfXML(std::istream &is);
    bool initFromConfXML(std::string path);

    void initToolSet();
    MHO::ToolSet* getToolSet(){return this->_tool_set;}
    MHO::MultiHandOverHeuristic* createAstarHeuristic(void);
    void setDefaultHandOverTesters(std::vector<std::string> &some_defaults){this->_handover_testers_defaults_set=some_defaults;}
    unsigned int runScriptFromEnvVar();
    unsigned int runScript(std::string filename);
    unsigned int run();

    unsigned int findAgents();
    void fetchAgentsConfsInitGoTo();
    unsigned int findManipulableObjects();
    unsigned int computeAgentsGraph(void);
    void initGrid(void);
    void computeGrid(void);
    void computeFrontiers(void){_grid->searchAllFrontiers();_progression_state=FRONTIERS_DISTANCE;}
    void resetAgentsConf(void);
    void resetManipulablesConf(void);

    Configuration getAgentTargetConf(unsigned int a);
    MultiAgentCell* getAgentTargetCell(unsigned int a);

    Configuration getAgentInitConf(unsigned int a);
    MultiAgentCell* getAgentInitCell(unsigned int a);

    void setToComputeHOTraj(bool activate,int solution=-1,int step=0);
    void computeLongestSolutionDistance();
    unsigned int computeRawSolutions(float cost_majoration);
    void clearSolutions(void);
    void insert_solution_sorted(MHO::MultiHandOverSolution* sol);

    void searchSolutionLWAstar(bool no_lazy);

    void postProcessSolution(MHO::MultiHandOverSolution *sol);
    void searchSolutions(void);
    bool handOverPossible(unsigned int a1, unsigned int a2);
    bool handOverFindConfs(MHO::MultiHandOverSolution * solution, int frontier_index=-1);
    bool handOverFindConfs(FrontierPtr frontier);
    MHO::MultiHandOverSolution* changeHandOverSolution( MHO::MultiHandOverSolution * solution);
    double computeNavigationTrajectories(MHO::MultiHandOverSolution * solution, int action_index=-1);
    void computeHandOverTrajectory(MHO::MultiHandOverSolution * solution, int step=-1);
    void computeSubTaskTrajectory(MHO::SubTask* sub_task);
    MHO::MultiHandOverSolution* optimizeSolution(MHO::MultiHandOverSolution *solution);
    MHO::MultiHandOverSolution* optimizeFrontier(MHO::MultiHandOverSolution *solution, unsigned int action_index, double keep_distance, double min_distance);

    MultiAgentCell* findOptimalCellNullDistanceFrontier(MHO::MultiHandOverSolution* solution,unsigned int action_index);

    double navigationRelatedCost(double dist,unsigned int agent);
    double navigationRelatedCostTime(double dist,unsigned int agent,double &time);
    double computeSolutionCost(MHO::MultiHandOverSolution * solution);

    bool getSolutionDataForMHP(std::vector<MHO::MultiHandOverMhpHO> &hand_overs,std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &path, double &cost, double &duration, std::string &main_agent_name);

    MHO::MultiHandOverSolution * getLowCostRandomSolution(double sigma, int *index=0);


    typedef enum {DIST_BEST_AGENT =0, BEST_HANDOVER_SERIES} AStarHeuristic_t;
    void setAstarHeuristic(MultiHandOver::AStarHeuristic_t heuristic){_a_star_heuristic = heuristic;}
    AStarHeuristic_t getAstarHeuristic(void){return _a_star_heuristic;}
    void setSearchAlgo(MultiHandOver::SearchAlgo_t algo){_search_algo=algo;}
    MultiHandOver::SearchAlgo_t getSearchAlgo(){return _search_algo;}
    double getEpsilonForLWAstar(){return _epsilon;}
    void setGridCellSize(double size){_cellSize=size;}
    double getGridCellSize(void){return _cellSize;}
    void setEpsilonForLWAstar(double e){this->_epsilon=std::max(1.,e);}
    void setCostsFactors(double dist, double time, double agent_costs, double hri){ _f_dist=dist;_f_time=time;_f_agent_cost=agent_costs;_f_hri=hri;}
    void getCostsFactors(double &dist, double &time, double &agent_costs, double &hri){ dist=_f_dist;time=_f_time;agent_costs=_f_agent_cost;hri=_f_hri;}

    void setSearchSolutionStopCondition(int max_it,int overheat,int time_limit_ms){ _max_iterations=max_it;_overheat_temperature=overheat;_time_limit=time_limit_ms;}
    void getSearchSolutionStopCondition(int &max_it,int &overheat,int &time_limit_ms){ max_it=_max_iterations;overheat=_overheat_temperature;time_limit_ms=_time_limit;}

    FrontierCells::FrontierCheckStatus getFrontierMaxCheckStatus(){return _max_frontier_check_status;}
    void setFrontierMaxCheckStatus(FrontierCells::FrontierCheckStatus fcs){_max_frontier_check_status = fcs;}

    const std::vector<Robot*>& getAgents(void){return this->_agents;}
    const std::vector<Robot*>& getManipulableObjects(void){return this->_manipulable;}

    MHO::MultiHandOverAgent *getAgentStruct(unsigned int a){return &_mho_agents[a];}

    void setFirstAgent(Robot* f){_first_agent=f;setFirstAgentNode();}
    Robot* getFirstAgent(void){return _first_agent;}
    void setTargetAgent(Robot* f){_target_agent=f;setTargetAgentNode();}
    Robot* getTargetAgent(void){return _target_agent;}
    void setTargetAgent(std::string name);
    void setFirstAgent(std::string name);
    void setTargetAgent(p3d_rob* rob);
    void setFirstAgent(p3d_rob* rob);
    void setRandomizeFirstTarget(bool b){_random_first_target_agents=b;}
    void setObject(unsigned int i){setObject(_manipulable[i]);}
    void setObject(Robot* obj){_object=obj;_object_bb_vects.clear();}
    Robot *getObject(){return _object;}
    void setObject(p3d_rob* obj);

    bool usePostProcess(){return this->_use_post_process;}
    void usePostProcess(bool b){this->_use_post_process=b;}

    void setFirstAgentNode(void);
    void setTargetAgentNode(void);


    void setAgentUseCost(unsigned int i,double cost){this->_mho_agents[i].use_cost=cost;}//this->_agents_use_costs[i]=cost;}
    double getAgentUseCost(unsigned int i){return this->_mho_agents[i].use_cost;}//this->_agents_use_costs[i];}
    double getAgentUseCost(Robot * r);
    //std::vector<double> const & getAgentUseCost(){return this->_agents_use_costs;}
    //void setAgentUseCost(std::vector<double> const & costs){this->_agents_use_costs=costs;}
    void setAgentSpeed(unsigned int i, double speed){this->_mho_agents[i].speed=speed;}//this->_agents_speeds[i]=speed;}
    double getAgentSpeed(unsigned int i){return this->_mho_agents[i].speed;}//this->_agents_speeds[i];}
    bool isHuman(unsigned int i){return this->_mho_agents[i].is_human;}
    //std::vector<double> const & getAgentsSpeeds(){return this->_agents_speeds;}
    //void setAgentsSpeeds(std::vector<double> const & speeds){this->_agents_speeds=speeds;}

    double getTimeHO(unsigned int a1,unsigned int a2);
    void setTimesHO(double rr,double hh,double rh){_r_r_ho_time=rr;_r_h_ho_time=rh;_h_h_ho_time=hh;}




    void setCheckFrontierBefore(bool b){this->_check_frontier_before=b;}
    bool getCheckFrontiersBefore(void){return this->_check_frontier_before;}

    void setUseRrtInFrontierCheck(bool b){_use_rrt_for_frontier_check=b;}
    void setUseDummySmoothInFrontierCheck(bool b){_use_dummy_smooth_for_frontier_check=b;}
    double getShoulderHeight(unsigned int agent);
    Eigen::Vector3d getObjectMainDirection(void);
    Configuration confForBBAxisAlign(Robot * object, Eigen::Vector3d const & main_dir, Eigen::Vector3d const & second_dir);

    void setOrientationObjectForHO(Eigen::Vector3d &dir1);
    void checkAllMetaFrontiersWithObject(void);
    bool checkMetaFrontiersWithObject(unsigned int a1,unsigned int a2);
    bool checkMetaFrontierWithObject(unsigned int a1,unsigned int a2,MetaFrontierPtr mf);
    bool randomCheckFrontiersWithObject(std::tr1::shared_ptr<FrontierCells> & frontier_found, unsigned int a1, unsigned int a2,unsigned int max_it);
    bool checkFrontierWithObject(FrontierCells &frontier);

    void printSolutionsForest(void);
    void printMainTimes(std::vector<double> times);
    void printLWAStats();

    void setAgentIndexToDrawGrid(unsigned int index){
        if(_grid) this->_grid->setAgentIndexToDrawGrid(index);
    }

    void setDrawAgentAccessibilityGrid(bool b,bool boundariesOnly=false){if(_grid) this->_grid->setDrawAgentAccessibilityGrid(b,boundariesOnly);}
    void setDrawFrontiers(bool b,unsigned int i=0,unsigned int j=0){if(_grid) _grid->setDrawFrontiers(b,i,j);}

    void drawObjectWhenFrontierChecking(bool b){_draw_object_frontiers_checks=b;}
    void setDrawAgentsDuringSearch(bool b){_draw_agents_holding_conf_during_search=b;}
    void setDraw2dPaths(bool b){_draw_2d_path=b;}
    void setDrawAll2dPaths(bool b){_draw_all_2d_pathes=b;}
    void setDrawLWA(bool b){_draw_lwa=b;if(_display_lwa_in_grid_obj){_display_lwa_in_grid_obj->setDraw(b);}}

    MHO::MultiHandOverSolution::CostData getSolutionDataStudio(int solution);
    double setSolutionStepRobotsConfs(int solution, int step, MHO::MultiHandOverSolution::CostData &cost_data);
    void getConfHandOver(int solution, int step, std::string robot_name, confPtr_t &conf);
    void draw2dPath(void);
    void draw2dPath(std::vector<MultiAgentCell*> const & path, int color_id, bool alt=false);
#ifdef DRAW_OPTIMIZE_FRONTIER
    void setDrawOptimizeFrontier(bool b){_draw_optimize_frontier=b;}
#else
    void setDrawOptimizeFrontier(bool b){if(_showText){std::cout<<"DRAW_OPTIMIZE_FRONTIER macro not defined";}}
#endif

    void playTaskStepByStep(MHO::Task* task);
    void playSolutionTrajSync(MHO::Task *task);
    bool  playSolutionTaskSyncStep(int solution=-1);
    void computeTaskTrajs(MHO::Task* task);
    void setSubTaskTrajToRobot(MHO::SubTask* subtask);
    void playSolutionTask(int solution);
    void setPlaySolutionTask(int solution);
    void setPlaySolutionTaskTraj(int solution);
    void setPlaySolutionSyncStepByStep(int solution);
    bool isEndPlaySolutionTaskTraj();

    MultiAgentGrid *getPlanGrid(void){return _grid;}



private:

    enum {INIT, FRONTIERS_DISTANCE,FRONTIERS_OBJECT_PATH,END} _progression_state;
    std::vector<MHO::MultiHandOverAgent> _mho_agents;
    std::vector<Robot*> _agents;
    std::vector<Robot*> _manipulable;
    mho::SimpleGraph _agents_frontiers_dist_graph;
    std::vector<mho::Node*> _agents_nodes;

    double _r_r_ho_time,_r_h_ho_time,_h_h_ho_time;

    double _f_dist,_f_time,_f_agent_cost,_f_hri;
    double _max_dist, _max_time,_max_use_cost;

    bool _use_rrt_for_frontier_check;
    bool _use_dummy_smooth_for_frontier_check;

    bool _check_frontier_before;

    // ///// //////////////////////////////
    // ///// FOR STATS   //////////////////
    // ///// //////////////////////////////
    enum STAT_FIELDS {SF_OK,SF_COST,SF_COST_LWA,
                      SF_AGENT_FROM, SF_AGENT_TO,
                      SF_EPSILON,SF_TIME_FACT,SF_AGENT_FACT,SF_HRI_FACT,SF_DIST_FACT,
                      SF_TIME_TOT,SF_TIME_LWA,SF_TIME_POSTPROC,
                      SF_TMP_COST_NB,  SF_TMP_COST_MEAN,   SF_TMP_COST_STDEV,
                      SF_TRUE_COST_NB, SF_TRUE_COST_MEAN,  SF_TRUE_COST_STDEV,
                      SF_HO_COST_NB,   SF_HO_COST_MEAN,    SF_HO_COST_STDEV,
                      SF_HEURISTIC_NB, SF_HEURISTIC_MEAN,  SF_HEURISTIC_STDEV,
                      SF_AGENT_NB,SF_HO_NB,SF_OBJ_DIST,SF_EXEC_TIME,// < stats about the solution
                      SF_SIZE};
    std::vector< std::vector < double > > _lwa_stats;


    FrontierCells::FrontierCheckStatus _max_frontier_check_status;
    MHO::ToolSet *_tool_set;
    std::vector<std::string> _handover_testers_defaults_set;

    std::vector<Configuration> _agents_init_confs;
    std::vector<Configuration> _agents_target_confs;
    std::vector<Configuration> _manipulables_init_confs;

    Robot *_first_agent;
    Robot *_target_agent;
    bool _random_first_target_agents;
    Robot *_object;
    std::vector<Eigen::Vector3d> _object_bb_vects;
    std::vector<int> _object_directions_sorted_for_ho;
    mho::Node *_first_agent_node;
    mho::Node *_target_agent_node;

    vector<pair<double,vector<mho::NodeInterface*> > > _raw_solution;

    bool _use_post_process;

    std::vector<MHO::MultiHandOverSolution*> _solutions;
    std::vector<MHO::Task*> _solutions_as_tasks;
    MHO::MultiHandOverSolution * _best_solution;
    //solution search parameters
    SearchAlgo_t _search_algo;
    AStarHeuristic_t _a_star_heuristic;
    //for LWA*
    double _epsilon;
    //for random search
    int _time_limit;
    int _max_iterations;
    int _overheat_temperature;

    bool _first_run;
    bool _compute_for_all_robots;
    bool _compute_traj_only;
    MHO::MultiHandOverSolution * _solution_to_compute_traj;
    unsigned int _step_to_compute_traj;

    //GRID
    HRICS::MultiAgentGrid * _grid;
    double _cellSize;

    bool _showText;
    bool _draw_object_frontiers_checks;
    bool _draw_agents_holding_conf_during_search;
    bool _draw_2d_path;
    unsigned int _solution_index_to_draw;
    unsigned int _solution_step_index_to_draw;
    bool _play_solution_task;
    bool _play_solution_task_traj;
    bool _play_solution_sync_step_by_step;
    bool _draw_all_2d_pathes;
    bool _draw_lwa;
    MHO::DisplayLWAstarInGrid* _display_lwa_in_grid_obj;
    MHO::MultiHandOverDisplay *_mho_display;
#ifdef DRAW_OPTIMIZE_FRONTIER
    bool _draw_optimize_frontier;
#endif
};

} // namespace HRICS

#endif // HRICS_HRICS_MULTIHANDOVER_H