File content as of revision 1:d4b2668eea6c:

#ifndef WAYPOINT_GENERATOR_HPP
#define WAYPOINT_GENERATOR_HPP

#include <list>

template <class PoseClass>
class WaypointGenerator
{
    public:
    WaypointGenerator(PoseClass start, PoseClass goal, double duration_time)
    {    
        start_ = start;
        goal_ = goal;
        duration_time_ = duration_time;
    }
    WaypointGenerator(PoseClass start, PoseClass goal, PoseClass gap, double duration_time)
    {
        /*int n = (goal - start) / gap;
        //printf("n is %d\r\n", n);
        double time_step = duration_time / n;
        gap = (goal - start) / n;
        for(int i = 0; i <= n; i++)
        {
            auto way = start + (gap * i);
            way = constraint(start, goal, way);
            way.time_stamp = time_step * (double)i;
            waypoint_list_.push_back( std::make_unique<Pose2D>(way) );
        }
        way_n_ = waypoint_list_.size();*/
        //dump();
    }
    
    PoseClass constraint(PoseClass start, PoseClass goal, PoseClass way)
    {
        way.ConstraintUpper(goal, goal - start);
        way.ConstraintLower(start, goal - start);
        return way;
    }
    
    PoseClass GetPose(int i)
    {
        if(i >= way_n_) return *(waypoint_list_[way_n_-1]);
        else return *(waypoint_list_[i]);
    }
    
    PoseClass GetPose(float t)
    {
        PoseClass way = start_ + (goal_ - start_) * (t/duration_time_);
        way = constraint(start_, goal_, way);
        return way;
    }
    
    void dump()
    {
        /*for(int i = 0; i<waypoint_list_.size(); i++)
        {
            printf("x:%d, y:%d, th:%d, t:%d\r\n"
            , (int)((*(waypoint_list_[i])).x*1000)
            , (int)((*(waypoint_list_[i])).y*1000)
            , (int)((*(waypoint_list_[i])).theta*1000)
            , (int)((*(waypoint_list_[i])).time_stamp*1000)
            );
        }*/
    }
    
    private:
    std::vector<std::unique_ptr<PoseClass>> waypoint_list_;
    int way_n_;
    PoseClass start_;
    PoseClass goal_;
    double duration_time_;
};

#endif