Line data Source code
1 : #ifndef ENCIRCLE_TRAJECTORY_H 2 : #define ENCIRCLE_TRAJECTORY_H 3 : 4 : #include <mav_trajectory_generation/motion_defines.h> 5 : #include <mav_trajectory_generation/trajectory.h> 6 : #include <trajectory/TrajectoryBase.h> 7 : 8 : #include <optional> 9 : #include <string> 10 : #include <vector> 11 : 12 : /** 13 : * @brief Arc trajectory centred on the vehicle's starting position, with an outward spiral approach 14 : * 15 : * The trajectory is planned in polar coordinates \f$(r, \theta)\f$ centred at the 16 : * vehicle's starting position. \f$r\f$ grows from \f$0\f$ to \f$r_{target}\f$ over 17 : * the first \f$\text{spiral\_proportion} \times |\Delta\theta|\f$ of the arc, then 18 : * remains constant for the remainder. At every point the vehicle's yaw is aligned 19 : * with the tangent to the instantaneous circle, plus an optional fixed offset. 20 : * 21 : * Cartesian position, velocity, and acceleration are derived analytically from 22 : * the polar trajectory at query time using the standard polar–Cartesian identities. 23 : */ 24 1 : class EncircleTrajectory : public TrajectoryBase { 25 : public: 26 : /** 27 : * @brief Constructs and plans the encircle trajectory 28 : * 29 : * @param origin [in] Starting vehicle state; the arc is centred at this position 30 : * @param radius [in] Target orbit radius in metres 31 : * @param turn_angle [in] Total arc angle in radians; positive = CCW, negative = CW 32 : * @param tangent_offset [in] Yaw offset from the arc tangent in radians; 0 means the 33 : * vehicle faces exactly along the direction of motion 34 : * @param spiral_proportion [in] Fraction of the arc over which \f$r\f$ grows from 0 to radius; 35 : * 0 = move radially outward first then arc at full radius, 36 : * 1 = spiral outward throughout the entire arc 37 : * @param vehicle_limits [in] Per-axis velocity and acceleration limits 38 : * @param world_frame_id [in] TF frame ID for visualisation messages 39 : */ 40 1 : EncircleTrajectory(const State& origin, double radius, double turn_angle, double tangent_offset, 41 : double spiral_proportion, std::shared_ptr<Trajectory6DOFLimits> vehicle_limits, 42 : const std::string& world_frame_id = "world_ned"); 43 : 44 : /** 45 : * @brief Returns true if trajectory planning succeeded 46 : * 47 : * @return True if a valid trajectory was generated 48 : */ 49 1 : bool is_valid() const; 50 : 51 : /** 52 : * @brief Returns true if the elapsed time has reached or exceeded the trajectory duration 53 : * 54 : * @param t [in] Current ROS time in seconds 55 : * 56 : * @return True when the trajectory is complete 57 : */ 58 1 : bool is_finished(double t) const override; 59 : 60 : /** 61 : * @brief Returns trajectory progress as a fractional segment index 62 : * 63 : * @param t [in] Current ROS time in seconds 64 : * 65 : * @return Progress where integer values indicate segment boundaries 66 : */ 67 1 : double get_progress(double t) const override; 68 : 69 : /** 70 : * @brief Returns the total planned trajectory duration 71 : * 72 : * @return Duration in seconds 73 : */ 74 1 : double get_expected_total_time() const override; 75 : 76 : protected: 77 : /** 78 : * @brief Evaluates the polar trajectory and converts to a world-frame Cartesian State 79 : * 80 : * @param t [in] Current ROS time in seconds 81 : * 82 : * @return Desired State in the world frame at the given time 83 : */ 84 1 : State get_model_world(double t) const override; 85 : 86 : private: 87 : State origin_; 88 : double radius_ = 0.0; 89 : double turn_angle_ = 0.0; 90 : double tangent_offset_ = 0.0; 91 : double spiral_proportion_ = 1.0; 92 : mav_trajectory_generation::Trajectory trajectory_; 93 : bool valid_ = false; 94 : 95 : static constexpr double MIN_SEGMENT_TIME = 0.01; 96 : static constexpr int DERIVATIVE_TO_OPTIMIZE = mav_trajectory_generation::derivative_order::ACCELERATION; 97 : 98 : /** 99 : * @brief Fraction of the XY velocity/acceleration budget allocated to the radial dimension; 100 : * the remainder goes to the angular dimension 101 : */ 102 : static constexpr double ALPHA = 0.5; 103 : 104 : /** 105 : * @brief Computes fractional progress within the current trajectory segment 106 : * 107 : * @param t [in] Elapsed time since trajectory start in seconds 108 : * 109 : * @return Fractional segment index 110 : */ 111 : double calculate_progress(double t) const; 112 : 113 : /** 114 : * @brief Plans the two-phase polar \f$(r, \theta)\f$ polynomial trajectory 115 : * 116 : * @return Planned 2D polar trajectory, or nullopt on planning failure 117 : */ 118 : std::optional<mav_trajectory_generation::Trajectory> plan_trajectory(); 119 : }; 120 : 121 : #endif // ENCIRCLE_TRAJECTORY_H
