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             :      * @return True if a new setpoint was established from the current state (traj or stationkeep accepted)
     176             :      */
     177           1 :     bool process_pending(double t, const State& current_state, moodycamel::ConcurrentQueue<DebugMessage>& debug_queue);
     178             : 
     179             :     /**
     180             :      * @brief Samples the active trajectory and runs the controller to compute a body force
     181             :      *
     182             :      * @param t               [in] Current ROS time in seconds
     183             :      * @param dt              [in] Time step in seconds since the last control tick
     184             :      * @param current_state   [in] Current vehicle state
     185             :      * @param setpoint_offset [in] World-frame position offset added to the setpoint (m)
     186             :      *
     187             :      * @return 6-element body force/torque vector in N and Nm
     188             :      */
     189           1 :     Vector6d get_force(double t, double dt, const State& current_state, const Vector3d& setpoint_offset);
     190             : 
     191             :     /**
     192             :      * @brief Allocates a body force vector to per-thruster PWM commands
     193             :      *
     194             :      * @param force [in] 6-element body force/torque vector in N and Nm
     195             :      *
     196             :      * @return Tuple of (PWM command message, per-thruster force message, achieved body force)
     197             :      */
     198           1 :     std::tuple<bb_controls_msgs::msg::Thrusters, bb_controls_msgs::msg::ThrusterForces, Vector6d> allocate_force(
     199             :         const Vector6d& force) const;
     200             : 
     201             :     /**
     202             :      * @brief Writes a new controller status snapshot
     203             :      *
     204             :      * @param status [in] Status snapshot to store
     205             :      */
     206           1 :     void update_controller_status(const ControllerStatus& status);
     207             : 
     208             :     /* ------------------------------------------------------------------ */
     209             :     /* Robot state (thread-safe)                                           */
     210             :     /* ------------------------------------------------------------------ */
     211             : 
     212             :     /**
     213             :      * @brief Returns the most recently stored vehicle state and its timestamp
     214             :      *
     215             :      * @return Tuple of (State, timestamp in seconds)
     216             :      */
     217           1 :     std::tuple<State, double> get_state() const;
     218             : 
     219             :     /**
     220             :      * @brief Returns the setpoint offset accumulated from rejected odometry jumps
     221             :      *
     222             :      * @return World-frame position offset in m
     223             :      */
     224           1 :     Vector3d get_setpoint_offset() const;
     225             : 
     226             :     /**
     227             :      * @brief Accumulates a world-frame position offset onto the setpoint offset
     228             :      *
     229             :      * @param delta [in] World-frame displacement in m
     230             :      */
     231           1 :     void add_setpoint_offset(const Vector3d& delta);
     232             : 
     233             :     /**
     234             :      * @brief Resets the setpoint offset to zero
     235             :      */
     236           1 :     void reset_setpoint_offset();
     237             : 
     238             :     /**
     239             :      * @brief Stores a new vehicle state with its timestamp
     240             :      *
     241             :      * @param state [in] New vehicle state
     242             :      * @param t     [in] State timestamp in seconds
     243             :      */
     244           1 :     void set_state(const State& state, double t);
     245             : 
     246             :     /* ------------------------------------------------------------------ */
     247             :     /* Battery voltage (thread-safe)                                       */
     248             :     /* ------------------------------------------------------------------ */
     249             : 
     250             :     /**
     251             :      * @brief Returns the most recently stored battery voltage
     252             :      *
     253             :      * @return Battery voltage in volts, or FLT_MAX if not yet set
     254             :      */
     255           1 :     float get_battery_voltage() const;
     256             : 
     257             :     /**
     258             :      * @brief Stores the current battery voltage
     259             :      *
     260             :      * @param battery_voltage [in] Battery voltage in volts
     261             :      */
     262           1 :     void set_battery_voltage(float battery_voltage);
     263             : 
     264             :     /* ------------------------------------------------------------------ */
     265             :     /* Trajectory queries                                                  */
     266             :     /* ------------------------------------------------------------------ */
     267             : 
     268             :     /**
     269             :      * @brief Returns fractional trajectory progress for the active trajectory
     270             :      *
     271             :      * @param t [in] Current ROS time in seconds
     272             :      *
     273             :      * @return Progress in [0, 1], or -1 if no trajectory is running
     274             :      */
     275           1 :     double get_trajectory_progress(double t) const;
     276             : 
     277             :     /* ------------------------------------------------------------------ */
     278             :     /* Controller                                                          */
     279             :     /* ------------------------------------------------------------------ */
     280             : 
     281             :     /**
     282             :      * @brief Returns the most recent body-frame pose error from the active controller
     283             :      *
     284             :      * @return 6-element body-frame pose error vector
     285             :      */
     286           1 :     Vector6d get_body_pose_error() const;
     287             : 
     288             :     /**
     289             :      * @brief Updates the per-axis thruster saturation cost used for integral anti-windup
     290             :      *
     291             :      * @param yaw_xy [in] Saturation cost for the X, Y, and Yaw axes
     292             :      * @param z_rp   [in] Saturation cost for the Z, Roll, and Pitch axes
     293             :      */
     294           1 :     void update_saturation_cost(double yaw_xy, double z_rp);
     295             : 
     296             : private:
     297             :     std::atomic<ControllerState> controller_state_{ControllerState::DISABLED};
     298             :     std::unique_ptr<TrajectoryBase> trajectory_;
     299             :     request::Request<request::RequestState::ACTIVE> active_request_
     300             :         = request::Request<request::RequestState::ACTIVE>::make_initial();
     301             :     State model_{};
     302             : 
     303             :     double state_time_ = 0.0;
     304             :     State state_{};
     305             :     Vector3d setpoint_offset_ = Vector3d::Zero();
     306             :     mutable std::shared_mutex state_mutex_;
     307             : 
     308             :     float battery_voltage_ = std::numeric_limits<float>::max();
     309             :     mutable std::mutex battery_voltage_mutex_;
     310             : 
     311             :     ControllerStatus controller_status_{};
     312             :     mutable std::shared_mutex controller_status_mutex_;
     313             : 
     314             :     mutable std::mutex pending_request_mutex_;
     315             :     std::vector<request::Request<request::RequestState::PENDING>> request_queue_;
     316             :     boost::uuids::random_generator uuid_generator_;
     317             : 
     318             :     double saturation_cost_yaw_xy_ = 0.0;
     319             :     double saturation_cost_z_rp_   = 0.0;
     320             : 
     321             :     /**
     322             :      * @brief Syncs the internal model pose to the current vehicle state
     323             :      *
     324             :      * Only the yaw component of the orientation is preserved (roll and pitch are
     325             :      * zeroed via extract_yaw).
     326             :      *
     327             :      * @param current_state               [in] Current vehicle state to copy from
     328             :      * @param preserve_twist_acceleration [in] If true, copies twist and acceleration; otherwise zeros them
     329             :      */
     330             :     void update_model_to_state(const State& current_state, bool preserve_twist_acceleration);
     331             : 
     332             :     request::RequestHandle<request::RequestHandleState::PENDING> request_controller(
     333             :         request::RequestType type, std::unique_ptr<TrajectoryBase> trajectory = nullptr);
     334             : };
     335             : 
     336             : #endif  // CONTROLS_VEHICLE_VEHICLE_H