Line data    Source code

       1             : #ifndef CONTROLS_ALLOCATOR_THRUST_ALLOCATOR_H
       2             : #define CONTROLS_ALLOCATOR_THRUST_ALLOCATOR_H
       3             : 
       4             : #include <allocator/config/ThrusterLayout.h>
       5             : #include <allocator/config/ThrusterLookupInterface.h>
       6             : #include <allocator/solver/ThrustAllocationSolverInterface.h>
       7             : #include <utils/Transforms.h>
       8             : #include <yaml-cpp/yaml.h>
       9             : 
      10             : #include <bb_controls_msgs/msg/thruster_forces.hpp>
      11             : #include <bb_controls_msgs/msg/thrusters.hpp>
      12             : #include <geometry_msgs/msg/wrench_stamped.hpp>
      13             : 
      14             : /**
      15             :  * @brief Converts a 6DOF body force vector to per-thruster PWM commands
      16             :  *
      17             :  * Loads thruster geometry from a YAML configuration file, delegates force
      18             :  * allocation to a pluggable QP solver, and converts resulting thruster force
      19             :  * magnitudes to integer commands via a ThrusterLookupInterface.  The centre-
      20             :  * of-gravity offset can be updated at runtime and will trigger an automatic
      21             :  * re-initialisation on the next allocate() call.
      22             :  */
      23           1 : class ThrustAllocator {
      24             :     /** @brief QP slack weights for each DOF axis (higher = more expensive to violate) */
      25             :     static constexpr double QP_WEIGHT_XY    = 1.0;
      26             :     static constexpr double QP_WEIGHT_Z     = 2.0;
      27             :     static constexpr double QP_WEIGHT_ROLL  = 8.0;
      28             :     static constexpr double QP_WEIGHT_PITCH = 8.0;
      29             :     static constexpr double QP_WEIGHT_YAW   = 6.0;
      30             : 
      31             : public:
      32             :     /**
      33             :      * @brief Constructs the allocator, loads YAML config, and initialises the QP solver
      34             :      *
      35             :      * @param thruster_lookup      [in] Voltage-dependent thrust lookup table implementation
      36             :      * @param solver               [in] QP thrust allocation solver implementation
      37             :      * @param yaml_config_filename [in] Path to the YAML file describing the thruster layout
      38             :      *
      39             :      * @throw std::runtime_error if the YAML file cannot be parsed
      40             :      */
      41           1 :     ThrustAllocator(std::unique_ptr<ThrusterLookupInterface> thruster_lookup,
      42             :                     std::unique_ptr<ThrustAllocationSolverInterface> solver, const std::string& yaml_config_filename);
      43             : 
      44             :     /**
      45             :      * @brief Allocates a body force to per-thruster commands at the given battery voltage
      46             :      *
      47             :      * Re-initialises the solver if the CG offset has changed since the last call.
      48             :      *
      49             :      * @param force           [in] Desired 6DOF body force/torque vector in N and Nm
      50             :      * @param battery_voltage [in] Battery voltage in volts (used to look up force limits)
      51             :      *
      52             :      * @return Tuple of (PWM command message, per-thruster force message, achieved body force)
      53             :      */
      54           1 :     std::tuple<bb_controls_msgs::msg::Thrusters, bb_controls_msgs::msg::ThrusterForces, Vector6d> allocate(
      55             :         const Vector6d& force, float battery_voltage);
      56             : 
      57             :     /**
      58             :      * @brief Updates the centre-of-gravity offset applied to all thruster positions
      59             :      *
      60             :      * The solver is lazily re-initialised on the next allocate() call.
      61             :      *
      62             :      * @param updated_cg_offset [in] New CG offset in the body frame (m)
      63             :      */
      64           1 :     void update_cg_offset(const Vector3d& updated_cg_offset);
      65             : 
      66             : private:
      67             :     ThrusterLayout thruster_layout_{};
      68             : 
      69             :     std::unique_ptr<ThrusterLookupInterface> thruster_lookup_;
      70             :     std::unique_ptr<ThrustAllocationSolverInterface> solver_;
      71             : 
      72             :     Vector3d cg_offset_{};
      73             :     bool cg_offset_changed_{false};
      74             : 
      75             :     /**
      76             :      * @brief Parses the YAML config file and populates the thruster layout
      77             :      *
      78             :      * @param yaml_config_filename [in] Path to the YAML configuration file
      79             :      *
      80             :      * @throw std::runtime_error if the YAML file cannot be parsed
      81             :      */
      82             :     void load_configuration(const std::string& yaml_config_filename);
      83             : 
      84             :     /**
      85             :      * @brief Applies the current CG offset to all thrusters and rebuilds the QP solver
      86             :      */
      87             :     void init_allocator();
      88             : };
      89             : 
      90             : #endif  // CONTROLS_ALLOCATOR_THRUST_ALLOCATOR_H