trajectory_path.h

#pragma once
 
#include <functional>
#include <memory>
 
#include "software/ai/navigator/trajectory/kinematic_constraints.h"
#include "software/ai/navigator/trajectory/trajectory_path_node.h"
 
using TrajectoryGenerator = std::function<std::shared_ptr<Trajectory2D>(
    const Point& initial_pos, const Point& final_pos, const Vector& initial_vel,
    const KinematicConstraints& constraints)>;
 
class TrajectoryPath : public Trajectory2D
{
   public:
    TrajectoryPath() = delete;
 
    TrajectoryPath(const std::shared_ptr<Trajectory2D>& initial_trajectory,
                   const TrajectoryGenerator& traj_generator);
 
    void append(double connection_time_sec, const Point& destination,
                const KinematicConstraints& constraints);
 
    Point getPosition(double t_sec) const override;
 
    Vector getVelocity(double t_sec) const override;
 
    Vector getAcceleration(double t_sec) const override;
 
    double getTotalTime() const override;
 
    std::vector<Rectangle> getBoundingBoxes() const override;
 
    const std::vector<TrajectoryPathNode>& getTrajectoryPathNodes() const;
 
   private:
    std::vector<TrajectoryPathNode> traj_path;
    TrajectoryGenerator trajectory_generator;
};