PolyTrajectory.h

Go to the documentation of this file.

#ifndef POLY_TRAJECTORY_H
#define POLY_TRAJECTORY_H
 
#include <mav_trajectory_generation/motion_defines.h>
#include <mav_trajectory_generation/trajectory.h>
#include <trajectory/TrajectoryBase.h>
 
#include <optional>
#include <string>
#include <vector>
 
class PolyTrajectory : public TrajectoryBase {
public:
    PolyTrajectory(const State& origin, const std::vector<Pose>& targets,
                   std::shared_ptr<Trajectory6DOFLimits> vehicle_limits,
                   const std::string& world_frame_id = "world_ned", bool needs_angle_unwrapping = true);
 
    bool is_valid() const;
 
    bool is_finished(double t) const override;
 
    double get_progress(double t) const override;
 
    double get_expected_total_time() const override;
 
    int get_total_num_waypoints() const;
 
    static constexpr double DEFAULT_MIN_SEGMENT_TIME = 0.01;
 
    static constexpr int DEFAULT_DERIVATIVE_TO_OPTIMIZE = mav_trajectory_generation::derivative_order::ACCELERATION;
 
protected:
    State get_model_world(double t) const override;
 
private:
    State start_state_;
    mav_trajectory_generation::Trajectory trajectory_;
    std::vector<VectorXd> waypoints_;
    bool valid_ = false;
    bool needs_angle_unwrapping_;
 
    double calculate_progress(double t) const;
 
    static void unwrap_angles(std::vector<VectorXd>& waypoints);
 
    std::vector<VectorXd> create_waypoints(const State& origin, const std::vector<Pose>& targets) const;
 
    std::optional<mav_trajectory_generation::Trajectory> plan_trajectory(const std::vector<VectorXd>& waypoints,
                                                                         const State& start_state) const;
};
 
#endif  // POLY_TRAJECTORY_H