costMapPlot.h

#ifndef COSTMAPPLOT_H
#define COSTMAPPLOT_H


#include "API/Device/robot.hpp"

#include "Logging/Logger.h"
#include "Logging/socketplot.h"


class CostMapPlot
{
    MOVE3D_STATIC_LOGGER;
public:
    CostMapPlot(const std::string &name);
    ~CostMapPlot();

    void start();

    void plotCurrentTraj();

    void addDof(Robot *r,Joint* j, int dof_index, double min=-std::numeric_limits<double>::infinity(), double max=std::numeric_limits<double>::infinity(), double step=0);

    double firstSample(std::vector<double> &dof_values);
    double nextSample(std::vector<double> &dof_values);
    double moveJointNextSample(unsigned int i,bool &reached_extreme);
    double getDofValue(unsigned int i);

    void updateRobots();
    void saveRobotConfs();
    void restoreRobotConfs();

    unsigned int defaultSampleNbPerJnt() const;
    void setDefaultSampleNbPerJnt(unsigned int defaultSampleNbPerJnt);

    void setAndUpdateJointDof(Joint* j,int dof,double val);

    static void test1();
    static void test2();

private:
    SocketPlot *_socketPlot;
    SocketPlot *_socketPlotTraj;
    std::vector<std::pair<Joint*,int> > _dofs;
    std::vector<double> _curValues;
    std::set<Robot*> _robots_to_update;
    bool _reached_end;
    unsigned int _defaultSampleNbPerJnt;
    std::vector<Configuration> _savedConf;

    std::vector<double> _steps,_min,_max;
    std::string _name,_name_traj;
};

#endif // COSTMAPPLOT_H