Line data    Source code

       1             : #ifndef POLY_TRAJECTORY_H
       2             : #define POLY_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 Polynomial-optimised trajectory through a sequence of 6DOF waypoints
      14             :  *
      15             :  * Uses mav_trajectory_generation to compute a minimum-acceleration polynomial
      16             :  * trajectory respecting per-axis velocity and acceleration limits.  Consecutive
      17             :  * identical waypoints are handled by inserting constant-position segments.
      18             :  * Yaw angles are unwrapped across waypoints by default to avoid discontinuities.
      19             :  */
      20           1 : class PolyTrajectory : public TrajectoryBase {
      21             : public:
      22             :     /**
      23             :      * @brief Constructs and plans the trajectory from an origin state to a list of target poses
      24             :      *
      25             :      * @param origin                [in] Starting vehicle state (pose, twist used as initial conditions)
      26             :      * @param targets               [in] Ordered list of goal poses
      27             :      * @param vehicle_limits        [in] Per-axis velocity and acceleration limits
      28             :      * @param world_frame_id        [in] TF frame ID for visualisation messages
      29             :      * @param needs_angle_unwrapping [in] If true, yaw angles are unwrapped across waypoints
      30             :      */
      31           1 :     PolyTrajectory(const State& origin, const std::vector<Pose>& targets,
      32             :                    std::shared_ptr<Trajectory6DOFLimits> vehicle_limits,
      33             :                    const std::string& world_frame_id = "world_ned", bool needs_angle_unwrapping = true);
      34             : 
      35             :     /**
      36             :      * @brief Returns true if trajectory planning succeeded
      37             :      *
      38             :      * @return True if a valid trajectory was generated
      39             :      */
      40           1 :     bool is_valid() const;
      41             : 
      42             :     /**
      43             :      * @brief Returns true if the elapsed time has reached or exceeded the trajectory duration
      44             :      *
      45             :      * @param t [in] Current ROS time in seconds
      46             :      *
      47             :      * @return True when the trajectory is complete
      48             :      */
      49           1 :     bool is_finished(double t) const override;
      50             : 
      51             :     /**
      52             :      * @brief Returns trajectory progress as a fractional segment index
      53             :      *
      54             :      * @param t [in] Current ROS time in seconds
      55             :      *
      56             :      * @return Progress where integer values indicate segment boundaries
      57             :      */
      58           1 :     double get_progress(double t) const override;
      59             : 
      60             :     /**
      61             :      * @brief Returns the total planned trajectory duration
      62             :      *
      63             :      * @return Duration in seconds
      64             :      */
      65           1 :     double get_expected_total_time() const override;
      66             : 
      67             :     /**
      68             :      * @brief Returns the number of inter-waypoint segments in the trajectory
      69             :      *
      70             :      * @return Number of segments (total waypoints minus 1)
      71             :      */
      72           1 :     int get_total_num_waypoints() const;
      73             : 
      74             :     /** @brief Minimum segment duration used when inserting constant-position segments (s) */
      75             :     static constexpr double DEFAULT_MIN_SEGMENT_TIME = 0.01;
      76             : 
      77             :     /** @brief Derivative order minimised by the polynomial optimiser (acceleration) */
      78             :     static constexpr int DEFAULT_DERIVATIVE_TO_OPTIMIZE = mav_trajectory_generation::derivative_order::ACCELERATION;
      79             : 
      80             : protected:
      81             :     /**
      82             :      * @brief Evaluates the planned polynomial trajectory at the given time
      83             :      *
      84             :      * @param t [in] Current ROS time in seconds
      85             :      *
      86             :      * @return Desired State in the world frame at the given time
      87             :      */
      88           1 :     State get_model_world(double t) const override;
      89             : 
      90             : private:
      91             :     State start_state_;
      92             :     mav_trajectory_generation::Trajectory trajectory_;
      93             :     std::vector<VectorXd> waypoints_;
      94             :     bool valid_ = false;
      95             :     bool needs_angle_unwrapping_;
      96             : 
      97             :     /**
      98             :      * @brief Computes fractional progress within the current trajectory segment
      99             :      *
     100             :      * @param t [in] Elapsed time since trajectory start in seconds
     101             :      *
     102             :      * @return Fractional segment index
     103             :      */
     104             :     double calculate_progress(double t) const;
     105             : 
     106             :     /**
     107             :      * @brief Unwraps yaw (and roll/pitch) angles across waypoints to avoid discontinuities
     108             :      *
     109             :      * @param waypoints [in,out] Waypoints whose angle components are modified in-place
     110             :      */
     111             :     static void unwrap_angles(std::vector<VectorXd>& waypoints);
     112             : 
     113             :     /**
     114             :      * @brief Converts the origin state and target poses into a flat waypoint list
     115             :      *
     116             :      * @param origin  [in] Starting vehicle state
     117             :      * @param targets [in] Ordered list of goal poses
     118             :      *
     119             :      * @return Vector of 6-element XYZRPY waypoints
     120             :      */
     121             :     std::vector<VectorXd> create_waypoints(const State& origin, const std::vector<Pose>& targets) const;
     122             : 
     123             :     /**
     124             :      * @brief Plans a polynomial trajectory through the given waypoints
     125             :      *
     126             :      * @param waypoints   [in] Ordered 6-element XYZRPY waypoints
     127             :      * @param start_state [in] Initial vehicle state providing the starting twist
     128             :      *
     129             :      * @return Planned trajectory, or nullopt on planning failure
     130             :      */
     131             :     std::optional<mav_trajectory_generation::Trajectory> plan_trajectory(const std::vector<VectorXd>& waypoints,
     132             :                                                                          const State& start_state) const;
     133             : };
     134             : 
     135             : #endif  // POLY_TRAJECTORY_H