tracker.h

#include "sensor.h"
 
class Tracker{
    protected:
    ThresholdLearner *learner=NULL;
    public:
    Threshold threshold=Threshold();
 
    Tracker(){}
 
    Threshold * get_threshold(){
        return &threshold;
    }
 
    void register_learner(ThresholdLearner * l){
        learner=l;
    }
    virtual b2Transform get_transform(const Task &t, const CoordinateContainer &pts, Disturbance * observed_disturbance, std::vector <BodyFeatures> & objects)=0; 
 
    virtual b2Transform track(Task &t, const CoordinateContainer &pts, std::vector <BodyFeatures> & objects)=0;
    //void adjust_task(const vertexDescriptor&, TransitionSystem &, Task*, const b2Transform &);                
 
    virtual void on_new_task(Task *task=NULL)=0;
 
    virtual void on_new_reading(Task * task=NULL)=0;
 
    virtual void init(Task * goal)=0;
 
    void make_log();
    
    protected:
 
 
    void log_thresholds(){
        FILE *f= fopen("/tmp/thresholds.txt", "a");
        fprintf(f, "%f\t%f\t%f\t%f\t%f\n", threshold.for_Di().get_x(),
                                            threshold.for_Di().get_y(),
                                            threshold.for_Di().get_angle(),
                                            threshold.for_Di().get_width(),
                                            threshold.for_Di().get_length());
        fclose(f);
    }
 
    
};
 
class DeadReckoner: public Tracker{
    public:
    DeadReckoner(){}
 
    b2Transform get_transform(const Task &t, const CoordinateContainer &pts, Disturbance * observed_disturbance, std::vector <BodyFeatures> & objects){
        return t.getAction().getTransform(LIDAR_SAMPLING_RATE);
    }     
    
    b2Transform track(Task &t, const CoordinateContainer &pts, std::vector <BodyFeatures> & objects);
 
    void on_new_task(Task *task=NULL){} //does nothing
 
    void on_new_reading(Task * task=NULL){};
 
    void init(Task * goal){}
 
 
};
 
class ClosedLoop_Tracker:public Tracker{
    Disturbance tracked_disturbance; //disturbance to be tracked as at task start, kept in memory when task is changed
    b2PolygonShape attention_window; //a box drawn at the beginning of task which bounds the robot and the goal
    public:
 
    ClosedLoop_Tracker(){}
 
    // ClosedLoop_Tracker(Task * goal){
    //     attention_window=sensor_box(Robot::get_vertices(),b2Transform_zero, goal->get_disturbance());
    // }
 
    b2Transform get_transform(const Task &t, const CoordinateContainer &pts, Disturbance * observed_disturbance, std::vector <BodyFeatures> & objects);    
    
    b2Transform track(Task &t, const CoordinateContainer &pts, std::vector <BodyFeatures> & objects);
 
    cv::Rect2f real_world_focus(const Task * );
 
    Disturbance * get_tracked_disturbance(){
        return &tracked_disturbance;
    }
 
    void set_tracked_disturbance(const Disturbance & d){
        tracked_disturbance=d;
    }
 
    std::vector <BodyFeatures>::iterator find_disturbance(std::vector <BodyFeatures> & objects, const BodyFeatures & dist, b2Transform t, float * _least_square=NULL);
 
    void on_new_task(Task *task=NULL);
 
    void on_new_reading(Task * goal=NULL);
 
    void set_attention(b2PolygonShape ps){
        attention_window=ps;
    }
 
    void init(Task * goal){
        attention_window=sensor_box(Robot::get_vertices(),b2Transform_zero, goal->get_disturbance_ptr());
 
    }
 
    private:
 
    float window_area();
 
 
};