Line data    Source code

       1             : #ifndef CONTROLS_VEHICLE_VEHICLE_H
       2             : #define CONTROLS_VEHICLE_VEHICLE_H
       3             : 
       4             : #include <allocator/ThrustAllocator.h>
       5             : #include <controller/ControllerConfig.h>
       6             : #include <controller/VehicleControllerInterface.h>
       7             : #include <dynamics/SystemDynamicsBase.h>
       8             : #include <trajectory/Trajectory6DOFLimits.h>
       9             : #include <trajectory/TrajectoryBase.h>
      10             : #include <utils/Transforms.h>
      11             : #include <utils/concurrentqueue.h>
      12             : #include <vehicle/Request.h>
      13             : 
      14             : #include <atomic>
      15             : #include <boost/uuid/name_generator.hpp>
      16             : #include <boost/uuid/random_generator.hpp>
      17             : #include <boost/uuid/uuid.hpp>
      18             : #include <boost/uuid/uuid_generators.hpp>
      19             : #include <future>
      20             : #include <limits>
      21             : #include <memory>
      22             : #include <mutex>
      23             : #include <shared_mutex>
      24             : 
      25             : /**
      26             :  * @brief Central vehicle abstraction managing the controller state machine, trajectory
      27             :  * execution, force computation, and thrust allocation
      28             :  *
      29             :  * Exposes two distinct APIs:
      30             :  *  - **Producer API** (called from ROS service/action callbacks): submits
      31             :  *    DISABLE, STATIONKEEP, or TRAJECTORY requests to a priority queue.
      32             :  *  - **Consumer API** (called exclusively from the control loop): drains the
      33             :  *    request queue, samples the active trajectory, runs the controller, and
      34             :  *    allocates forces to thrusters.
      35             :  *
      36             :  * Thread safety: state and battery voltage are protected by shared mutexes.
      37             :  * The request queue is protected by a plain mutex.  All Consumer API methods
      38             :  * must be called from a single thread.
      39             :  */
      40           1 : class Vehicle {
      41             :     /** @brief Minimum force component magnitude to consider a thruster axis saturated */
      42             :     static constexpr double SATURATION_THRESHOLD = 1e-3;
      43             : 
      44             : public:
      45             :     /** @brief Possible operational states of the controller */
      46           1 :     enum class ControllerState { DISABLED, STATIONKEEP, RUNNING };
      47             : 
      48             :     /**
      49             :      * @brief Snapshot of controller status exposed to external callers
      50             :      */
      51           1 :     struct ControllerStatus {
      52             :         /** @brief Current controller state */
      53             :         ControllerState state;
      54             : 
      55             :         /** @brief UUID of the most recently activated request */
      56             :         boost::uuids::uuid request_uuid;
      57             : 
      58             :         /** @brief ROS time at which this status was last updated */
      59             :         double timestamp;
      60             : 
      61             :         /** @brief Fractional trajectory progress in [0, 1], or -1 if not running */
      62             :         double progress;
      63             : 
      64             :         /** @brief Thruster slowdown scale factor */
      65             :         uint8_t risk_factor;
      66             : 
      67             :         /** @brief Most recent vehicle pose */
      68             :         Pose current_pose;
      69             : 
      70             :         /** @brief Most recent body-frame pose error [Δx, Δy, Δz, Δroll, Δpitch, Δyaw] */
      71             :         Vector6d body_pose_error;
      72             :     };
      73             : 
      74             :     /** @brief Shared vehicle dynamics model (set once in ctor, mutated via setters only) */
      75             :     std::shared_ptr<SystemDynamicsBase> system;
      76             : 
      77             :     /** @brief Shared controller tuning parameters (set once in ctor, fields mutated by param callback) */
      78             :     std::shared_ptr<ControllerConfig> controller_config;
      79             : 
      80             :     /** @brief Active vehicle controller implementation (set once in ctor, called from control loop only) */
      81             :     std::shared_ptr<VehicleControllerInterface> controller;
      82             : 
      83             :     /** @brief Thrust allocator (set once in ctor, called from control loop and teleop callback) */
      84             :     std::unique_ptr<ThrustAllocator> thrust_allocator;
      85             : 
      86             :     /** @brief Velocity and acceleration limits (set once in ctor, fields mutated by param callback) */
      87             :     std::shared_ptr<Trajectory6DOFLimits> vehicle_limits;
      88             : 
      89             :     /**
      90             :      * @brief Constructs the vehicle with all required subsystems
      91             :      *
      92             :      * @param system_dynamics      [in] Vehicle dynamics model
      93             :      * @param controller_config    [in] Controller tuning parameters
      94             :      * @param vehicle_controller   [in] Controller implementation
      95             :      * @param thrust_allocator     [in] Thrust allocation subsystem
      96             :      */
      97           1 :     Vehicle(std::shared_ptr<SystemDynamicsBase> system_dynamics, std::shared_ptr<ControllerConfig> controller_config,
      98             :             std::shared_ptr<VehicleControllerInterface> vehicle_controller,
      99             :             std::unique_ptr<ThrustAllocator> thrust_allocator);
     100             : 
     101             :     /**
     102             :      * @brief Drains the pending request queue, fulfilling all unfulfilled promises
     103             :      *
     104             :      * Prevents broken promises if any futures are still alive
     105             :      * when the Vehicle is destroyed.
     106             :      */
     107           1 :     ~Vehicle();
     108             : 
     109             :     // Non-copyable and non-movable: mutex and unique_ptr members present
     110           0 :     Vehicle(const Vehicle&)            = delete;
     111           0 :     Vehicle& operator=(const Vehicle&) = delete;
     112           0 :     Vehicle(Vehicle&&)                 = delete;
     113           0 :     Vehicle& operator=(Vehicle&&)      = delete;
     114             : 
     115             :     /* ------------------------------------------------------------------ */
     116             :     /* Producer API (called from service/action callbacks)                 */
     117             :     /* ------------------------------------------------------------------ */
     118             : 
     119             :     /**
     120             :      * @brief Enqueues a request to enter the STATIONKEEP state
     121             :      *
     122             :      * @return Handle containing the request UUID and an activation future
     123             :      */
     124           1 :     request::RequestHandle<request::RequestHandleState::PENDING> request_stationkeep();
     125             : 
     126             :     /**
     127             :      * @brief Enqueues a request to enter the DISABLED state
     128             :      *
     129             :      * @return Handle containing the request UUID and an activation future
     130             :      */
     131           1 :     request::RequestHandle<request::RequestHandleState::PENDING> request_disable();
     132             : 
     133             :     /**
     134             :      * @brief Enqueues a request to execute a trajectory
     135             :      *
     136             :      * The request is rejected (future resolved to false) if the controller is
     137             :      * currently DISABLED when the control loop processes it.
     138             :      *
     139             :      * @param trajectory [in] Trajectory to execute; ownership is transferred
     140             :      *
     141             :      * @return Handle containing the request UUID and an activation future
     142             :      */
     143           1 :     request::RequestHandle<request::RequestHandleState::PENDING> request_trajectory(
     144             :         std::unique_ptr<TrajectoryBase> trajectory);
     145             : 
     146             :     /**
     147             :      * @brief Returns a thread-safe snapshot of the current controller status
     148             :      *
     149             :      * @return Current ControllerStatus
     150             :      */
     151           1 :     ControllerStatus get_controller_status() const;
     152             : 
     153             :     /* ------------------------------------------------------------------ */
     154             :     /* Consumer API (called from control_loop only)                        */
     155             :     /* ------------------------------------------------------------------ */
     156             : 
     157             :     /**
     158             :      * @brief Returns true if the controller is currently in the DISABLED state
     159             :      *
     160             :      * @return True if disabled
     161             :      */
     162           1 :     bool is_disabled() const;
     163           0 :     bool is_stationkeeping() const;
     164             : 
     165             :     /**
     166             :      * @brief Drains the pending request queue and performs state transitions
     167             :      *
     168             :      * Priority: DISABLE > STATIONKEEP > TRAJECTORY.  Automatically transitions
     169             :      * a RUNNING trajectory to STATIONKEEP when the trajectory finishes.
     170             :      *
     171             :      * @param t             [in] Current ROS time in seconds
     172             :      * @param current_state [in] Most recent vehicle state
     173             :      * @param debug_queue   [in,out] Queue to which trajectory visualisation messages are pushed
     174             :      */
     175           1 :     void process_pending(double t, const State& current_state, moodycamel::ConcurrentQueue<DebugMessage>& debug_queue);
     176             : 
     177             :     /**
     178             :      * @brief Samples the active trajectory and runs the controller to compute a body force
     179             :      *
     180             :      * @param t             [in] Current ROS time in seconds
     181             :      * @param dt            [in] Time step in seconds since the last control tick
     182             :      * @param current_state [in] Current vehicle state
     183             :      *
     184             :      * @return 6-element body force/torque vector in N and Nm
     185             :      */
     186           1 :     Vector6d get_force(double t, double dt, const State& current_state);
     187             : 
     188             :     /**
     189             :      * @brief Allocates a body force vector to per-thruster PWM commands
     190             :      *
     191             :      * @param force [in] 6-element body force/torque vector in N and Nm
     192             :      *
     193             :      * @return Tuple of (PWM command message, per-thruster force message, achieved body force)
     194             :      */
     195           1 :     std::tuple<bb_controls_msgs::msg::Thrusters, bb_controls_msgs::msg::ThrusterForces, Vector6d> allocate_force(
     196             :         const Vector6d& force) const;
     197             : 
     198             :     /**
     199             :      * @brief Writes a new controller status snapshot
     200             :      *
     201             :      * @param status [in] Status snapshot to store
     202             :      */
     203           1 :     void update_controller_status(const ControllerStatus& status);
     204             : 
     205             :     /* ------------------------------------------------------------------ */
     206             :     /* Robot state (thread-safe)                                           */
     207             :     /* ------------------------------------------------------------------ */
     208             : 
     209             :     /**
     210             :      * @brief Returns the most recently stored vehicle state and its timestamp
     211             :      *
     212             :      * @return Tuple of (State, timestamp in seconds)
     213             :      */
     214           1 :     std::tuple<State, double> get_state() const;
     215             : 
     216             :     /**
     217             :      * @brief Stores a new vehicle state with its timestamp
     218             :      *
     219             :      * @param state [in] New vehicle state
     220             :      * @param t     [in] State timestamp in seconds
     221             :      */
     222           1 :     void set_state(const State& state, double t);
     223             : 
     224             :     /* ------------------------------------------------------------------ */
     225             :     /* Battery voltage (thread-safe)                                       */
     226             :     /* ------------------------------------------------------------------ */
     227             : 
     228             :     /**
     229             :      * @brief Returns the most recently stored battery voltage
     230             :      *
     231             :      * @return Battery voltage in volts, or FLT_MAX if not yet set
     232             :      */
     233           1 :     float get_battery_voltage() const;
     234             : 
     235             :     /**
     236             :      * @brief Stores the current battery voltage
     237             :      *
     238             :      * @param battery_voltage [in] Battery voltage in volts
     239             :      */
     240           1 :     void set_battery_voltage(float battery_voltage);
     241             : 
     242             :     /* ------------------------------------------------------------------ */
     243             :     /* Trajectory queries                                                  */
     244             :     /* ------------------------------------------------------------------ */
     245             : 
     246             :     /**
     247             :      * @brief Returns fractional trajectory progress for the active trajectory
     248             :      *
     249             :      * @param t [in] Current ROS time in seconds
     250             :      *
     251             :      * @return Progress in [0, 1], or -1 if no trajectory is running
     252             :      */
     253           1 :     double get_trajectory_progress(double t) const;
     254             : 
     255             :     /* ------------------------------------------------------------------ */
     256             :     /* Controller                                                          */
     257             :     /* ------------------------------------------------------------------ */
     258             : 
     259             :     /**
     260             :      * @brief Returns the most recent body-frame pose error from the active controller
     261             :      *
     262             :      * @return 6-element body-frame pose error vector
     263             :      */
     264           1 :     Vector6d get_body_pose_error() const;
     265             : 
     266             :     /**
     267             :      * @brief Updates the per-axis thruster saturation cost used for integral anti-windup
     268             :      *
     269             :      * @param yaw_xy [in] Saturation cost for the X, Y, and Yaw axes
     270             :      * @param z_rp   [in] Saturation cost for the Z, Roll, and Pitch axes
     271             :      */
     272           1 :     void update_saturation_cost(double yaw_xy, double z_rp);
     273             : 
     274             : private:
     275             :     std::atomic<ControllerState> controller_state_{ControllerState::DISABLED};
     276             :     std::unique_ptr<TrajectoryBase> trajectory_;
     277             :     request::Request<request::RequestState::ACTIVE> active_request_
     278             :         = request::Request<request::RequestState::ACTIVE>::make_initial();
     279             :     State model_{};
     280             : 
     281             :     double state_time_ = 0.0;
     282             :     State state_{};
     283             :     mutable std::shared_mutex state_mutex_;
     284             : 
     285             :     float battery_voltage_ = std::numeric_limits<float>::max();
     286             :     mutable std::mutex battery_voltage_mutex_;
     287             : 
     288             :     ControllerStatus controller_status_{};
     289             :     mutable std::shared_mutex controller_status_mutex_;
     290             : 
     291             :     mutable std::mutex pending_request_mutex_;
     292             :     std::vector<request::Request<request::RequestState::PENDING>> request_queue_;
     293             :     boost::uuids::random_generator uuid_generator_;
     294             : 
     295             :     double saturation_cost_yaw_xy_ = 0.0;
     296             :     double saturation_cost_z_rp_   = 0.0;
     297             : 
     298             :     /**
     299             :      * @brief Syncs the internal model pose to the current vehicle state
     300             :      *
     301             :      * Only the yaw component of the orientation is preserved (roll and pitch are
     302             :      * zeroed via extractYaw).
     303             :      *
     304             :      * @param current_state               [in] Current vehicle state to copy from
     305             :      * @param preserve_twist_acceleration [in] If true, copies twist and acceleration; otherwise zeros them
     306             :      */
     307             :     void update_model_to_state(const State& current_state, bool preserve_twist_acceleration);
     308             : 
     309             :     request::RequestHandle<request::RequestHandleState::PENDING> request_controller(
     310             :         request::RequestType type, std::unique_ptr<TrajectoryBase> trajectory = nullptr);
     311             : };
     312             : 
     313             : #endif  // CONTROLS_VEHICLE_VEHICLE_H