trgen.h

Go to the documentation of this file.

#ifndef __TRGEN_H
#define __TRGEN_H

#ifdef MATLAB_MEX_FILE
#include <mex.h>
#endif

#include <list>
#include <math.h>


struct TrajectoryConstraints {
    double maxv;                
    double maxomega;            
    double maxangacc;           
    double maxacc;              
    double maxcenacc;           
    double maxe;                
};

class Pos {
public:
    double x, y, phi;           
    double v, omega;            
    Pos() : x(0), y(0), phi(0), v(0), omega(0) {};
};

class Point {
public:
    double x, y;
    Point() : x(0), y(0) {};
    Point(double _x, double _y) : x(_x), y(_y) {};
    
    double distanceTo(class Point &wp) {
        double dx = wp.x-x, dy = wp.y-y;
        return sqrt(dx*dx + dy*dy);
    }
    
    double angleTo(class Point &wp) {
        double dx = wp.x-x, dy = wp.y-y;
        return atan2(dy,dx);
    }
    
    bool operator==(Point &o) {
        return (x == o.x && y == o.y);
    }
    
    bool operator!=(Point &o) {
        return !(*this == o);
    }
};

class TrajectoryPoints : public std::list<Point*> {
public:
    ~TrajectoryPoints() {
        for (iterator wp = begin(); wp != end(); wp++) {
            delete(*wp);
        }
        clear();
    }
};


class TrajectorySegment {
protected:
    double t1, t2;
    double acc;                 
public:
    // speeds
    double maxv;                
    double v1, v2;              

    virtual void setMaxV(const TrajectoryConstraints &constr) = 0;
    virtual double getMaxAcc(const TrajectoryConstraints &constr) const { return constr.maxacc; };
    virtual bool isTurn() const { return false; };

    double s, r, alphahalf;
    
    TrajectorySegment() : s(0), r(0), alphahalf(0){};
    virtual ~TrajectorySegment() {};

    virtual double getLength() const = 0; 
    virtual double getUnifiedLength() const {
            return getLength();
    }
    virtual void getPointAt(double distance, Point *p) = 0;
    virtual void shortenBy(double distance, Point *newEnd) = 0;
    virtual TrajectorySegment* splitAt(double distance, Point *newEnd) = 0;

    virtual double startAt(double time) {
        t1 = time;
        t2 = time + getUnifiedLength()/((v1+v2)/2.0);
        acc = (v2-v1)/(t2-t1);
        return t2;
    }
    virtual void getRefPos(double time, Pos &rp) = 0;
    int containsTime(double t) {
        if (t >= t1 && t <= t2)
            return 0;
        else if (t >= t1) 
            return 1;
        else
            return -1;
    }
    double getT1() { return t1; } 
    double getT2() { return t2; } 
#ifdef MATLAB_MEX_FILE
    virtual void plot(const char *style) = 0;
    void plotSpeed(const char *style) {
        char cmd[300];
        sprintf(cmd, "plot([%g %g], [%g %g], '%s')",
                t1, t2, v1, v2, style);
        mexEvalString(cmd);

    }
#endif
};


class Trajectory : public std::list<TrajectorySegment*> {
    Pos initPos;
    iterator currentSeg;
    TrajectoryPoints wayPoints; 
    Point *initialPoint, *finalPoint;
    TrajectoryConstraints constr;
    bool prepared;
    
    void deleteSegments() {
        for (iterator seg = begin(); seg != end(); seg++) {
            delete(*seg);
        }
        clear();
    }

    bool splitSegment(iterator &seg, double distance);
    bool points2segments();
    void corners2arcs();
    void addTurns();
    void calcSpeeds();

    void getSegRefPos(TrajectorySegment *seg, double time, Pos &rp);
    
public:
    double finalHeading;
    bool backward;

    Trajectory(TrajectoryConstraints tc, bool _backward = false) : 
        wayPoints(), initialPoint(0), finalPoint(0), constr(tc), 
        prepared(false), finalHeading(NAN), backward(_backward) {};

    ~Trajectory() {
        deleteSegments();
    }

    void addPoint(double x, double y) {
        finalPoint = new Point(x,y);
        wayPoints.push_back(finalPoint);
    }
    void addTurn(double angle);

    double calcLength();        
    bool prepare(Pos _initPos);
    bool getRefPos(double time, Pos &rp);
#ifdef MATLAB_MEX_FILE
    void plot(const char *style);
    void plotSpeed(const char *style);
#endif
};


#endif

---