ThrustAllocator.h

Go to the documentation of this file.

#ifndef CONTROLS_ALLOCATOR_THRUST_ALLOCATOR_H
#define CONTROLS_ALLOCATOR_THRUST_ALLOCATOR_H
 
#include <allocator/config/ThrusterLayout.h>
#include <allocator/config/ThrusterLookupInterface.h>
#include <allocator/solver/ThrustAllocationSolverInterface.h>
#include <utils/Transforms.h>
#include <yaml-cpp/yaml.h>
 
#include <bb_controls_msgs/msg/thruster_forces.hpp>
#include <bb_controls_msgs/msg/thrusters.hpp>
#include <geometry_msgs/msg/wrench_stamped.hpp>
 
class ThrustAllocator {
    static constexpr double QP_WEIGHT_XY    = 1.0;
    static constexpr double QP_WEIGHT_Z     = 2.0;
    static constexpr double QP_WEIGHT_ROLL  = 8.0;
    static constexpr double QP_WEIGHT_PITCH = 8.0;
    static constexpr double QP_WEIGHT_YAW   = 6.0;
 
public:
    ThrustAllocator(std::unique_ptr<ThrusterLookupInterface> thruster_lookup,
                    std::unique_ptr<ThrustAllocationSolverInterface> solver, const std::string& yaml_config_filename);
 
    std::tuple<bb_controls_msgs::msg::Thrusters, bb_controls_msgs::msg::ThrusterForces, Vector6d> allocate(
        const Vector6d& force, float battery_voltage);
 
    void update_cg_offset(const Vector3d& updated_cg_offset);
 
private:
    ThrusterLayout thruster_layout_{};
 
    std::unique_ptr<ThrusterLookupInterface> thruster_lookup_;
    std::unique_ptr<ThrustAllocationSolverInterface> solver_;
 
    Vector3d cg_offset_{};
    bool cg_offset_changed_{false};
 
    void load_configuration(const std::string& yaml_config_filename);
 
    void init_allocator();
};
 
#endif  // CONTROLS_ALLOCATOR_THRUST_ALLOCATOR_H