Line data    Source code

       1             : #ifndef ENCIRCLE_TRAJECTORY_FACTORY_H
       2             : #define ENCIRCLE_TRAJECTORY_FACTORY_H
       3             : 
       4             : #include <trajectory/Trajectory6DOFLimits.h>
       5             : #include <trajectory/factory/TrajectoryFactoryInterface.h>
       6             : 
       7             : #include <bb_controls_msgs/srv/encircle_traj.hpp>
       8             : #include <memory>
       9             : #include <string>
      10             : 
      11             : /**
      12             :  * @brief Factory that builds circular arc (encircle) trajectories
      13             :  *
      14             :  * Parses and validates encircle parameters from an EncircleTraj service request
      15             :  * and constructs an EncircleTrajectory.  Uses a builder pattern to allow
      16             :  * optional configuration after construction.
      17             :  *
      18             :  * @see EncircleTrajectory
      19             :  */
      20           1 : class EncircleTrajectoryFactory : public TrajectoryFactoryInterface {
      21             : public:
      22           0 :     using Request = bb_controls_msgs::srv::EncircleTraj::Request;
      23             : 
      24             :     /**
      25             :      * @brief Constructs the factory from an EncircleTraj service request
      26             :      *
      27             :      * Validates that radius is positive, turn angle is non-zero, and
      28             :      * spiral_proportion is in [0, 1].
      29             :      *
      30             :      * @param req    [in] Service request with radius, turn_angle, tangent_offset, and spiral_proportion
      31             :      * @param origin [in] Starting vehicle state
      32             :      * @param limits [in] Per-axis velocity and acceleration limits
      33             :      */
      34           1 :     EncircleTrajectoryFactory(const Request& req, const State& origin, std::shared_ptr<Trajectory6DOFLimits> limits);
      35             : 
      36             :     /**
      37             :      * @brief Sets the origin twist norm threshold below which the initial twist is zeroed
      38             :      *
      39             :      * @param max_norm [in] Threshold on the L2 norm of the origin twist
      40             :      *
      41             :      * @return Reference to this factory for method chaining
      42             :      */
      43           1 :     EncircleTrajectoryFactory& with_origin_twist_threshold(double max_norm);
      44             : 
      45             :     /**
      46             :      * @brief Sets the TF world frame ID embedded in visualisation messages
      47             :      *
      48             :      * @param frame_id [in] TF frame ID string
      49             :      *
      50             :      * @return Reference to this factory for method chaining
      51             :      */
      52           1 :     EncircleTrajectoryFactory& with_frame_id(const std::string& frame_id);
      53             : 
      54             :     /**
      55             :      * @brief Builds and returns the configured encircle trajectory
      56             :      *
      57             :      * @return Unique pointer to the constructed EncircleTrajectory, or nullptr on failure
      58             :      */
      59           1 :     std::unique_ptr<TrajectoryBase> build() override;
      60             : 
      61             : private:
      62             :     State origin_;
      63             :     std::shared_ptr<Trajectory6DOFLimits> limits_;
      64             :     double radius_                      = 0.0;
      65             :     double turn_angle_                  = 0.0;
      66             :     double tangent_offset_              = 0.0;
      67             :     double spiral_proportion_           = 1.0;
      68             :     double origin_twist_norm_threshold_ = 0.03;
      69             :     std::string frame_id_               = "world_ned";
      70             :     bool is_valid_                      = true;
      71             : };
      72             : 
      73             : #endif  // ENCIRCLE_TRAJECTORY_FACTORY_H