rrtstar.h

Go to the documentation of this file.

#ifndef RRTSTAR_H
#define RRTSTAR_H
#include "obstacles.h"
#include <stdlib.h>
#include <vector>
#include <math.h>
#include<iostream>
using namespace std;
using namespace std;
using namespace Eigen;

struct Node {
    vector<Node *> children;
    Node *parent;
    Vector2f position;
    float orientation;
    double cost;
};

class RRTSTAR
{
public:
    RRTSTAR();
    void initialize();
    Node* getRandomNode();
    Node* nearest(Vector2f point);
    void near(Vector2f point, float radius, vector<Node *>& out_nodes);
    double distance(Vector2f &p, Vector2f &q);
    double Cost(Node *q);
    double PathCost(Node *qFrom, Node *qTo);
    Vector3f newConfig(Node *q, Node *qNearest);

    void add(Node *qNearest, Node *qNew);
    bool reached();
    void setStepSize(double step);
    void setMaxIterations(int iter);
    void deleteNodes(Node *root);

    /*Functions to set the parameters*/
    void setStartPose(double x,double y,double theta);
    void setGoalPose(double x, double y);
    void setWorldInfo(double width,double height);
    void set_max_iter(int x);
    void set_step_size(double x);
    void set_bot_radius(double x);
    void set_goalBias(double x);
    void set_turn_radius(double x);
    void set_rrt_star_neighbour_factor(double x);
    void set_bot_follow_dubin(bool x);

    RRTObstacles *obstacles; 
    vector<Node *> nodes; 
    vector<Node *> path; 
    Node *root; 
    Node *lastNode; 
    /* Parameters to be set before using RRT*/
    Vector2f startPos; 
    Vector2f endPos; 
    double start_orient; 
    int max_iter; 
    double step_size; 
    double world_width; 
    double world_height;
    double bot_radius; 
    double goalBias; 
    double turn_radius; 
    bool bot_follow_dubin; 
    double rrt_star_neighbour_factor; 
};

#endif // RRTSTAR_H