Line data    Source code

       1             : #ifndef TRAJECTORY_BASE_H
       2             : #define TRAJECTORY_BASE_H
       3             : 
       4             : #include <trajectory/Trajectory6DOFLimits.h>
       5             : #include <utils/Transforms.h>
       6             : 
       7             : #include <memory>
       8             : 
       9             : /**
      10             :  * @brief Abstract base class for all trajectory representations
      11             :  *
      12             :  * A trajectory is a time-parameterised sequence of States expressed in the
      13             :  * world frame.  Subclasses implement get_model_world() to evaluate the desired
      14             :  * position, velocity, and acceleration at a given time.  TrajectoryBase
      15             :  * converts that world-frame result to the body frame via the Jacobian transform
      16             :  * before returning it to the caller.
      17             :  */
      18           1 : class TrajectoryBase {
      19             : public:
      20           0 :     virtual ~TrajectoryBase() = default;
      21             : 
      22             :     /**
      23             :      * @brief Records the start time of the trajectory
      24             :      *
      25             :      * @param t [in] ROS time at which execution begins, in seconds
      26             :      */
      27           1 :     void start(double t);
      28             : 
      29             :     /**
      30             :      * @brief Returns the desired body-frame state at time t
      31             :      *
      32             :      * Internally calls get_model_world() and transforms the result to the body
      33             :      * frame using the current vehicle orientation.
      34             :      *
      35             :      * @param t          [in] Current ROS time in seconds
      36             :      * @param curr_state [in] Current vehicle state (orientation used for frame conversion)
      37             :      *
      38             :      * @return Desired State in the body frame
      39             :      */
      40           1 :     State get_model(double t, const State& curr_state);
      41             : 
      42             :     /**
      43             :      * @brief Returns true if the trajectory has completed at time t
      44             :      *
      45             :      * @param t [in] Current ROS time in seconds
      46             :      *
      47             :      * @return True once elapsed time exceeds the trajectory duration
      48             :      */
      49           1 :     virtual bool is_finished(double t) const = 0;
      50             : 
      51             :     /**
      52             :      * @brief Returns fractional trajectory progress at time t
      53             :      *
      54             :      * @param t [in] Current ROS time in seconds
      55             :      *
      56             :      * @return Progress value; semantics are implementation-defined but typically
      57             :      *         increases from 0 to the number of segments
      58             :      */
      59           1 :     virtual double get_progress(double t) const = 0;
      60             : 
      61             :     /**
      62             :      * @brief Returns the total expected duration of the trajectory
      63             :      *
      64             :      * @return Expected duration in seconds
      65             :      */
      66           1 :     virtual double get_expected_total_time() const = 0;
      67             : 
      68             :     /**
      69             :      * @brief Builds a PoseArray message sampling the trajectory for RViz visualisation
      70             :      *
      71             :      * @param t         [in] Current ROS time used for the message stamp
      72             :      * @param sample_hz [in] Sampling rate in Hz (default: 20 Hz)
      73             :      *
      74             :      * @return PoseArray containing sampled world-frame poses along the trajectory
      75             :      */
      76           1 :     geometry_msgs::msg::PoseArray get_visualization_msg(double t, double sample_hz = 20.0) const;
      77             : 
      78             : protected:
      79             :     /** @brief Velocity and acceleration limits for trajectory planning */
      80             :     std::shared_ptr<Trajectory6DOFLimits> vehicle_limits_;
      81             : 
      82             :     /** @brief TF frame ID embedded in visualisation message headers */
      83             :     std::string world_frame_id_;
      84             : 
      85             :     /** @brief True after start() has been called */
      86             :     bool started_ = false;
      87             : 
      88             :     /** @brief ROS time at which start() was called */
      89             :     double start_time_ = 0.0;
      90             : 
      91             :     /**
      92             :      * @brief Returns elapsed time since start(), or 0 if not yet started
      93             :      *
      94             :      * @param t [in] Current ROS time in seconds
      95             :      *
      96             :      * @return Elapsed time in seconds
      97             :      */
      98           1 :     double get_elapsed_time(double t) const;
      99             : 
     100             :     /**
     101             :      * @brief Evaluates the desired world-frame state at the given ROS time
     102             :      *
     103             :      * @param t [in] Current ROS time in seconds; elapsed time is computed internally
     104             :      *
     105             :      * @return Desired State expressed in the world frame
     106             :      */
     107           1 :     virtual State get_model_world(double t) const = 0;
     108             : 
     109             : private:
     110             :     /**
     111             :      * @brief Converts a world-frame desired state to the body frame
     112             :      *
     113             :      * @param model_world [in] Desired state in the world frame
     114             :      * @param curr_state  [in] Current vehicle state providing the rotation
     115             :      *
     116             :      * @return Desired State expressed in the body frame
     117             :      */
     118             :     static State convert_model_to_body_frame(const State& model_world, const State& curr_state);
     119             : };
     120             : 
     121             : #endif  // TRAJECTORY_BASE_H