configurator.h

#ifndef CONFIGURATOR_H
#define CONFIGURATOR_H
#include <dirent.h>
#include <thread>
#include <filesystem>
#include <ncurses.h>
#include <fstream>
#include <algorithm>
#include <random>
#include <sys/stat.h>
#include "debug.h"
#include "planner.h"
#include "control_interface.h"
#include "task_controller.h"
#include "tracker.h"
 
class Configurator{
protected:
    int iteration=0, simulatedTasks=0; 
    Task currentTask;
    Controller * task_controller=NULL;
    Tracker * tracker=NULL;
    LIDAR_In * ci=NULL;
    Motor_Out * control=NULL;
    Planner *planner=NULL;
    Logger * logger=NULL;
    bool running =0;
    std::thread * LIDAR_thread=NULL;
    float simulationStep=2*std::max(ROBOT_HALFLENGTH, ROBOT_HALFWIDTH);
    std::chrono::high_resolution_clock::time_point previousTimeScan;
    GoalChanger * goal_changer=NULL;    
    std::vector<vertexDescriptor>m_plan, current_vertices;
    Task controlGoal;
    CoordinateContainer data2fp;
    TransitionSystem transitionSystem=TransitionSystem(1);
    WorldBuilder *worldBuilder=new WorldBuilder();
    vertexDescriptor currentVertex=MOVING_VERTEX;
    edgeDescriptor movingEdge=edgeDescriptor(), currentEdge=movingEdge;
 
    public:
 
 
Configurator(){
    init();
}
 
Configurator(Task _task){
    init(_task);
}
 
virtual ~Configurator(){
    stop();
    delete worldBuilder;
    worldBuilder=NULL;
}
 
virtual void init(Task _task=Task());
 
virtual bool Spawner(); 
 
int getIteration(){
    return iteration;
}
 
void addIteration(int i=1){
    iteration+=i;
}
 
void assignDisturbanceToTask(const Disturbance & d, Task & t);
 
 
void dummy_vertex(vertexDescriptor src);
 
simResult simulate(Task t, b2World & w);
 
virtual float remainingSimulationTime(const Task * const t=NULL);
 
void estimate_current_vertex();
 
 
void printPlan(std::vector <vertexDescriptor>* p=NULL);
 
 
 
std::pair<edgeDescriptor, bool> addVertex(const vertexDescriptor & src, vertexDescriptor &v1, Edge edge=Edge(), bool topDown=0);
 
virtual void explore_plan(b2World&)=0;
 
//std::vector <vertexDescriptor> back_planner(TransitionSystem&, vertexDescriptor, vertexDescriptor root=0);
 
//starts thread
void start(); 
 
//stops thread
void stop(); 
 
void registerInterface(LIDAR_In *, Motor_Out *);
 
static void run(Configurator *);
 
virtual void change_task();
 
void update_graph(TransitionSystem& g, const TrackingResult & tr);
 
//round angle to a divisor of PI/2
float approximate_angle(float angle, Direction d, simResult::resultType outcome);
 
virtual void adjust_goal_expectation();
 
std::pair <bool, Direction> getOppositeDirection(Direction d);
 
 
void adjust_simulated_task(const vertexDescriptor&v,  Task& t);
 
 
void register_controller(Controller * controller){
    task_controller=controller;
}
 
Controller* get_controller(){
    return task_controller;
}
void register_tracker(Tracker * _tracker){
    if (!_tracker){return;}
    tracker=_tracker;
    tracker->init(controlGoal);
}
 
Tracker * get_tracker()const {
    return tracker;
}
 
Motor_Out * get_motor_interface(){
    return control;
}
 
LIDAR_In * get_lidar_interface(){
    return ci;
}
 
 
void register_planner(Planner * _p){
    planner=_p;
}
 
void setSimulationStep(float f){
    simulationStep=f;
}
 
void register_logger(Logger * l){
    logger=l;
}
 
void register_goalChanger(GoalChanger * gc){
    goal_changer=gc;
}
 
static void MulT(const b2Transform& B, Task & task);
 
 
static void InvMul(const b2Transform& B, Task & task);
 
static void InvMul_(const b2Transform& B, Task & task);
 
static void Mul(const b2Transform& B, Task &task);
 
virtual Robot makeRobot(b2World& world, const b2Transform & start);
 
//Disturbance * getGoalDisturbance(){return &controlGoal.disturbance;}
 
bool areInterfacesSetUp(Configurator * c=NULL);
 
void assignBodyFeatures(Task & t, const BodyFeatures & bf);
 
void assignDimensions(Task & t, float halfLength, float halfWidth);
 
void register_worldBuilder(WorldBuilder * wb){
    if (worldBuilder){
        delete worldBuilder;
    }
    worldBuilder=wb;
}
 
b2Transform get_start(const Task *const t){
    if (!t){
        throw std::invalid_argument("null pointer to task");
    }
    return t->start;    
}
 
Direction get_direction(const Task *const t){
    if (!t){
        throw std::invalid_argument("null pointer to task");
    }
    return t->direction;    
}
};
 
class ReactiveConfigurator:public Configurator{
    protected:
    void explore_plan(b2World&)override;
 
    float remainingSimulationTime(const Task *const t)override;
 
    public:
 
    ReactiveConfigurator(){};
 
    ReactiveConfigurator(Task _task){
        init(_task);
}
    
 
};
 
 #endif