Line data    Source code

       1             : #ifndef POLY_TRAJECTORY_FACTORY_H
       2             : #define POLY_TRAJECTORY_FACTORY_H
       3             : 
       4             : #include <trajectory/Trajectory6DOFLimits.h>
       5             : #include <trajectory/factory/TrajectoryFactoryInterface.h>
       6             : #include <utils/Transforms.h>
       7             : 
       8             : #include <bb_controls_msgs/action/locomotion.hpp>
       9             : #include <bb_controls_msgs/srv/spline_traj.hpp>
      10             : #include <memory>
      11             : #include <string>
      12             : #include <vector>
      13             : 
      14             : /**
      15             :  * @brief Factory that builds polynomial waypoint-following trajectories
      16             :  *
      17             :  * Parses goal waypoints from either a SplineTraj service request or a
      18             :  * Locomotion action goal, converts relative goals to absolute world-frame
      19             :  * coordinates, optionally inserts intermediate heading-interpolation waypoints,
      20             :  * and constructs a PolyTrajectory.  Uses a builder pattern to allow optional
      21             :  * configuration after construction.
      22             :  *
      23             :  * @see PolyTrajectory
      24             :  */
      25           1 : class PolyTrajectoryFactory : public TrajectoryFactoryInterface {
      26             : public:
      27           0 :     using Request    = bb_controls_msgs::srv::SplineTraj::Request;
      28           0 :     using Locomotion = bb_controls_msgs::action::Locomotion;
      29             : 
      30             :     /**
      31             :      * @brief Constructs the factory from a SplineTraj service request
      32             :      *
      33             :      * @param req    [in] Service request containing goal poses and relative-coordinate flags
      34             :      * @param origin [in] Starting vehicle state used for relative-to-absolute conversion
      35             :      * @param limits [in] Per-axis velocity and acceleration limits
      36             :      */
      37           1 :     PolyTrajectoryFactory(const Request& req, const State& origin, std::shared_ptr<Trajectory6DOFLimits> limits);
      38             : 
      39             :     /**
      40             :      * @brief Constructs the factory from a Locomotion action goal
      41             :      *
      42             :      * @param goal   [in] Action goal containing per-axis setpoint arrays and flags
      43             :      * @param origin [in] Starting vehicle state used for relative-to-absolute conversion
      44             :      * @param limits [in] Per-axis velocity and acceleration limits
      45             :      */
      46           1 :     PolyTrajectoryFactory(const Locomotion::Goal& goal, const State& origin,
      47             :                           std::shared_ptr<Trajectory6DOFLimits> limits);
      48             : 
      49             :     /**
      50             :      * @brief Sets the origin twist norm threshold below which the initial twist is zeroed
      51             :      *
      52             :      * @param max_norm [in] Threshold on the L2 norm of the origin twist
      53             :      *
      54             :      * @return Reference to this factory for method chaining
      55             :      */
      56           1 :     PolyTrajectoryFactory& with_origin_twist_threshold(double max_norm);
      57             : 
      58             :     /**
      59             :      * @brief Sets the TF world frame ID embedded in visualisation messages
      60             :      *
      61             :      * @param frame_id [in] TF frame ID string
      62             :      *
      63             :      * @return Reference to this factory for method chaining
      64             :      */
      65           1 :     PolyTrajectoryFactory& with_frame_id(const std::string& frame_id);
      66             : 
      67             :     /**
      68             :      * @brief Controls whether yaw angles are unwrapped across consecutive waypoints
      69             :      *
      70             :      * @param needs_angle_unwrapping [in] True to enable yaw unwrapping (default)
      71             :      *
      72             :      * @return Reference to this factory for method chaining
      73             :      */
      74           1 :     PolyTrajectoryFactory& with_angle_unwrapping(bool needs_angle_unwrapping = true);
      75             : 
      76             :     /**
      77             :      * @brief Builds and returns the configured trajectory
      78             :      *
      79             :      * @return Unique pointer to the constructed PolyTrajectory, or nullptr on failure
      80             :      */
      81           1 :     std::unique_ptr<TrajectoryBase> build() override;
      82             : 
      83             : private:
      84             :     State origin_;
      85             :     std::shared_ptr<Trajectory6DOFLimits> limits_;
      86             :     std::vector<Pose> raw_goals_;
      87             :     double origin_twist_norm_threshold_ = 0.03;
      88             :     bool relative_xy_                   = false;
      89             :     bool relative_z_                    = false;
      90             :     bool relative_heading_              = false;
      91             :     bool interpolate_heading_           = false;
      92             :     bool needs_angle_unwrapping_        = true;
      93             :     std::string frame_id_               = "world_ned";
      94             :     bool is_valid_                      = true;
      95             : 
      96             :     /**
      97             :      * @brief Resolves raw_goals_ into absolute world-frame poses
      98             :      *
      99             :      * @return Vector of absolute goal poses
     100             :      */
     101             :     std::vector<Pose> compute_absolute_goals() const;
     102             : 
     103             :     /**
     104             :      * @brief Inserts intermediate waypoints so the vehicle faces each goal before translating
     105             :      *
     106             :      * @param goals [in] Absolute goal poses
     107             :      *
     108             :      * @return Expanded goal list with intermediate heading waypoints interleaved
     109             :      */
     110             :     std::vector<Pose> add_heading_interpolation(const std::vector<Pose>& goals) const;
     111             : };
     112             : 
     113             : #endif  // POLY_TRAJECTORY_FACTORY_H