Vehicle.h

Go to the documentation of this file.

#ifndef CONTROLS_VEHICLE_VEHICLE_H
#define CONTROLS_VEHICLE_VEHICLE_H
 
#include <allocator/ThrustAllocator.h>
#include <controller/ControllerConfig.h>
#include <controller/VehicleControllerInterface.h>
#include <dynamics/SystemDynamicsBase.h>
#include <trajectory/Trajectory6DOFLimits.h>
#include <trajectory/TrajectoryBase.h>
#include <utils/Transforms.h>
#include <utils/concurrentqueue.h>
#include <vehicle/Request.h>
 
#include <atomic>
#include <boost/uuid/name_generator.hpp>
#include <boost/uuid/random_generator.hpp>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <future>
#include <limits>
#include <memory>
#include <mutex>
#include <shared_mutex>
 
class Vehicle {
    static constexpr double SATURATION_THRESHOLD = 1e-3;
 
public:
    enum class ControllerState { DISABLED, STATIONKEEP, RUNNING };
 
    struct ControllerStatus {
        ControllerState state;
 
        boost::uuids::uuid request_uuid;
 
        double timestamp;
 
        double progress;
 
        uint8_t risk_factor;
 
        Pose current_pose;
 
        Vector6d body_pose_error;
    };
 
    std::shared_ptr<SystemDynamicsBase> system_;
 
    std::shared_ptr<ControllerConfig> controller_config_;
 
    std::shared_ptr<VehicleControllerInterface> controller_;
 
    std::unique_ptr<ThrustAllocator> thrust_allocator_;
 
    std::shared_ptr<Trajectory6DOFLimits> vehicle_limits_;
 
    Vehicle(std::shared_ptr<SystemDynamicsBase> system_dynamics, std::shared_ptr<ControllerConfig> controller_config,
            std::shared_ptr<VehicleControllerInterface> vehicle_controller,
            std::unique_ptr<ThrustAllocator> thrust_allocator);
 
    ~Vehicle();
 
    // Non-copyable and non-movable: mutex and unique_ptr members present
    Vehicle(const Vehicle&)            = delete;
    Vehicle& operator=(const Vehicle&) = delete;
    Vehicle(Vehicle&&)                 = delete;
    Vehicle& operator=(Vehicle&&)      = delete;
 
    /* ------------------------------------------------------------------ */
    /* Producer API (called from service/action callbacks)                 */
    /* ------------------------------------------------------------------ */
 
    request::RequestHandle<request::RequestHandleState::PENDING> request_stationkeep();
 
    request::RequestHandle<request::RequestHandleState::PENDING> request_disable();
 
    request::RequestHandle<request::RequestHandleState::PENDING> request_trajectory(
        std::unique_ptr<TrajectoryBase> trajectory);
 
    ControllerStatus get_controller_status() const;
 
    /* ------------------------------------------------------------------ */
    /* Consumer API (called from control_loop only)                        */
    /* ------------------------------------------------------------------ */
 
    bool is_disabled() const;
    bool is_stationkeeping() const;
 
    bool process_pending(double t, const State& current_state, moodycamel::ConcurrentQueue<DebugMessage>& debug_queue);
 
    Vector6d get_force(double t, double dt, const State& current_state, const Vector3d& setpoint_offset);
 
    std::tuple<bb_controls_msgs::msg::Thrusters, bb_controls_msgs::msg::ThrusterForces, Vector6d> allocate_force(
        const Vector6d& force) const;
 
    void update_controller_status(const ControllerStatus& status);
 
    /* ------------------------------------------------------------------ */
    /* Robot state (thread-safe)                                           */
    /* ------------------------------------------------------------------ */
 
    std::tuple<State, double> get_state() const;
 
    Vector3d get_setpoint_offset() const;
 
    void add_setpoint_offset(const Vector3d& delta);
 
    void reset_setpoint_offset();
 
    void set_state(const State& state, double t);
 
    /* ------------------------------------------------------------------ */
    /* Battery voltage (thread-safe)                                       */
    /* ------------------------------------------------------------------ */
 
    float get_battery_voltage() const;
 
    void set_battery_voltage(float battery_voltage);
 
    /* ------------------------------------------------------------------ */
    /* Trajectory queries                                                  */
    /* ------------------------------------------------------------------ */
 
    double get_trajectory_progress(double t) const;
 
    /* ------------------------------------------------------------------ */
    /* Controller                                                          */
    /* ------------------------------------------------------------------ */
 
    Vector6d get_body_pose_error() const;
 
    void update_saturation_cost(double yaw_xy, double z_rp);
 
private:
    std::atomic<ControllerState> controller_state_{ControllerState::DISABLED};
    std::unique_ptr<TrajectoryBase> trajectory_;
    request::Request<request::RequestState::ACTIVE> active_request_
        = request::Request<request::RequestState::ACTIVE>::make_initial();
    State model_{};
 
    double state_time_ = 0.0;
    State state_{};
    Vector3d setpoint_offset_ = Vector3d::Zero();
    mutable std::shared_mutex state_mutex_;
 
    float battery_voltage_ = std::numeric_limits<float>::max();
    mutable std::mutex battery_voltage_mutex_;
 
    ControllerStatus controller_status_{};
    mutable std::shared_mutex controller_status_mutex_;
 
    mutable std::mutex pending_request_mutex_;
    std::vector<request::Request<request::RequestState::PENDING>> request_queue_;
    boost::uuids::random_generator uuid_generator_;
 
    double saturation_cost_yaw_xy_ = 0.0;
    double saturation_cost_z_rp_   = 0.0;
 
    void update_model_to_state(const State& current_state, bool preserve_twist_acceleration);
 
    request::RequestHandle<request::RequestHandleState::PENDING> request_controller(
        request::RequestType type, std::unique_ptr<TrajectoryBase> trajectory = nullptr);
};
 
#endif  // CONTROLS_VEHICLE_VEHICLE_H