Line data    Source code

       1             : #ifndef CONTROLS_NODES_CONTROLSROS2WRAPPER_H
       2             : #define CONTROLS_NODES_CONTROLSROS2WRAPPER_H
       3             : 
       4             : #include <allocator/ThrustAllocator.h>
       5             : #include <utils/Types.h>
       6             : #include <utils/concurrentqueue.h>
       7             : #include <vehicle/Vehicle.h>
       8             : #include <yaml-cpp/yaml.h>
       9             : 
      10             : #include <atomic>
      11             : #include <bb_controls_msgs/action/locomotion.hpp>
      12             : #include <bb_controls_msgs/msg/controller_status.hpp>
      13             : #include <bb_controls_msgs/msg/thrusters.hpp>
      14             : #include <bb_controls_msgs/srv/controller.hpp>
      15             : #include <bb_controls_msgs/srv/encircle_traj.hpp>
      16             : #include <bb_controls_msgs/srv/spline_traj.hpp>
      17             : #include <bb_planner_msgs/srv/get_plan_through_poses.hpp>
      18             : #include <bb_sensor_msgs/msg/batteries_with_id.hpp>
      19             : #include <boost/uuid/uuid.hpp>
      20             : #include <boost/uuid/uuid_generators.hpp>
      21             : #include <boost/uuid/uuid_io.hpp>
      22             : #include <memory>
      23             : #include <mutex>
      24             : #include <rclcpp_action/rclcpp_action.hpp>
      25             : #include <rclcpp_action/server.hpp>
      26             : #include <std_msgs/msg/bool.hpp>
      27             : #include <string>
      28             : #include <string_view>
      29             : #include <thread>
      30             : #include <unordered_map>
      31             : 
      32             : /**
      33             :  * @brief ROS2 node that wraps the Vehicle control stack and exposes its interface to the rest of the system
      34             :  *
      35             :  * Manages all ROS publishers, subscribers, services, and actions.  Runs the
      36             :  * 20 Hz control loop (started on receipt of the first odometry message), drains
      37             :  * a lock-free debug queue at 20 Hz on a separate timer, and forwards parameter
      38             :  * updates to the Vehicle and SystemDynamicsBase objects at runtime.
      39             :  *
      40             :  * Services:
      41             :  *   - in/controller        — Enable (stationkeep) or disable the controller
      42             :  *   - in/xyzTraj           — Request a waypoint/spline trajectory
      43             :  *   - in/encircleTraj      — Request a circular arc trajectory
      44             :  *
      45             :  * Actions:
      46             :  *   - in/locomotion/poly — Execute a waypoint trajectory with tolerance-based success checking
      47             :  *
      48             :  * Subscriptions:
      49             :  *   - in/odom_ned   — Vehicle odometry (nav_msgs/Odometry)
      50             :  *   - in/batteries  — Battery voltages
      51             :  *   - in/teleop     — Direct force commands (disables the controller if running, then allocates directly)
      52             :  *
      53             :  * Publications:
      54             :  *   - out/force              — Controller body force (WrenchStamped)
      55             :  *   - out/force/allocated    — Achieved body force after allocation (WrenchStamped)
      56             :  *   - out/trajectory/status  — Trajectory and controller status
      57             :  *   - out/trajectory/viz     — Sampled trajectory poses for RViz
      58             :  *   - out/thrusters/input    — Per-thruster PWM commands
      59             :  *   - out/thrusters/forces   — Per-thruster forces in Newtons
      60             :  */
      61           1 : class ControlsROS2Wrapper : public rclcpp::Node {
      62             : public:
      63             :     /**
      64             :      * @brief Constructs the node, initialises all ROS interfaces, and loads parameters
      65             :      *
      66             :      * @param name              [in] ROS node name
      67             :      * @param robot             [in] Fully constructed Vehicle instance
      68             :      * @param body_frame_tf_name  [in] TF frame ID of the vehicle body frame
      69             :      * @param world_frame_tf_name [in] TF frame ID of the world frame
      70             :      */
      71           1 :     explicit ControlsROS2Wrapper(const std::string& name, std::unique_ptr<Vehicle> robot,
      72             :                                  std::string body_frame_tf_name = "base_link", std::string world_frame_tf_name = "map");
      73             : 
      74           0 :     ~ControlsROS2Wrapper() override;
      75             : 
      76             :     // Non-copyable and non-movable: mutex, atomic, unique_ptr, and thread members
      77           0 :     ControlsROS2Wrapper(const ControlsROS2Wrapper&)            = delete;
      78           0 :     ControlsROS2Wrapper& operator=(const ControlsROS2Wrapper&) = delete;
      79           0 :     ControlsROS2Wrapper(ControlsROS2Wrapper&&)                 = delete;
      80           0 :     ControlsROS2Wrapper& operator=(ControlsROS2Wrapper&&)      = delete;
      81             : 
      82           0 :     using Locomotion           = bb_controls_msgs::action::Locomotion;
      83           0 :     using GoalHandleLocomotion = rclcpp_action::ServerGoalHandle<Locomotion>;
      84             : 
      85             :     /**
      86             :      * @brief Internal representation of all ROS-declared controller and dynamics parameters
      87             :      *
      88             :      * Default values match the physical AUV4 vehicle.  Populated from the ROS
      89             :      * parameter server at startup and kept in sync via the set-parameter callback.
      90             :      * Pushed to the Vehicle subsystems by apply_config_to_vehicle().
      91             :      */
      92           1 :     struct Params {
      93             :         double ff_gain               = 0.0;
      94             :         double fb_gain               = 1.0;
      95             :         double buoyancy_gain         = 0.0;
      96             :         bool surface_gain_scheduling = false;
      97             :         double surface_threshold     = 0.1;
      98             :         double axis_gain_x           = 1.0;
      99             :         double axis_gain_y           = 1.0;
     100             :         double axis_gain_z           = 0.0;
     101             :         double axis_gain_roll        = 0.0;
     102             :         double axis_gain_pitch       = 0.0;
     103             :         double axis_gain_yaw         = 1.0;
     104             :         double kp_x                  = 1491.433;
     105             :         double ki_x                  = 35.628;
     106             :         double kd_x                  = 21.098;
     107             :         double kp_y                  = 600.345;
     108             :         double ki_y                  = 9.834;
     109             :         double kd_y                  = 330.864;
     110             :         double kp_z                  = 969.781;
     111             :         double ki_z                  = 474.207;
     112             :         double kd_z                  = 180.169;
     113             :         double kp_roll               = 79.701;
     114             :         double ki_roll               = 91.516;
     115             :         double kd_roll               = 10.975;
     116             :         double kp_pitch              = 117.765;
     117             :         double ki_pitch              = 135.223;
     118             :         double kd_pitch              = 16.209;
     119             :         double kp_yaw                = 150.113;
     120             :         double ki_yaw                = 10.038;
     121             :         double kd_yaw                = 54.212;
     122             :         double kp_x_surface          = 226.595;
     123             :         double ki_x_surface          = 4.124;
     124             :         double kd_x_surface          = 1643.310;
     125             :         double kp_y_surface          = 334.345;
     126             :         double ki_y_surface          = 9.834;
     127             :         double kd_y_surface          = 1589.864;
     128             :         double kp_z_surface          = 969.781;
     129             :         double ki_z_surface          = 474.207;
     130             :         double kd_z_surface          = 180.169;
     131             :         double kp_roll_surface       = 79.701;
     132             :         double ki_roll_surface       = 91.516;
     133             :         double kd_roll_surface       = 10.975;
     134             :         double kp_pitch_surface      = 117.765;
     135             :         double ki_pitch_surface      = 135.223;
     136             :         double kd_pitch_surface      = 16.209;
     137             :         double kp_yaw_surface        = 29.113;
     138             :         double ki_yaw_surface        = 1.038;
     139             :         double kd_yaw_surface        = 54.212;
     140             :         double slow_down_gain        = 0.0;
     141             :         double max_xy_vel            = 10.0;
     142             :         double max_xy_acc            = 5.0;
     143             :         double max_xy_jerk           = 0.8;
     144             :         double max_z_vel             = 0.0;
     145             :         double max_z_acc             = 0.0;
     146             :         double max_z_jerk            = 0.0;
     147             :         double max_yaw_vel           = 3.0;
     148             :         double max_yaw_acc           = 0.4;
     149             :         double max_yaw_jerk          = 0.3;
     150             : 
     151             :         // Adaptive buoyancy bias estimation (heave)
     152             :         bool buoyancy_adaptive_z            = false;
     153             :         double buoyancy_adaptive_rate       = 0.5;
     154             :         double buoyancy_adaptive_clamp_pct  = 0.15;
     155             :         double buoyancy_adaptive_pos_thresh = 0.15;
     156             :         double buoyancy_adaptive_vel_thresh = 0.05;
     157             : 
     158             :         // Adaptive buoyancy bias estimation (roll/pitch); thresholds in deg, deg/s
     159             :         bool buoyancy_adaptive_rp              = false;
     160             :         double buoyancy_adaptive_rate_rp       = 5.0;
     161             :         double buoyancy_adaptive_pos_thresh_rp = 0.2;
     162             :         double buoyancy_adaptive_vel_thresh_rp = 1.0;
     163             : 
     164             :         // System dynamics parameters
     165             :         double mass            = 36.10;
     166             :         double buoyancy_factor = 1.0;
     167             :         double i_xx            = 0.7699579;
     168             :         double i_yy            = 0.7699579;
     169             :         double i_zz            = 1.38624;
     170             :         double i_xy            = 0.0;
     171             :         double i_xz            = 0.0;
     172             :         double i_yz            = 0.0;
     173             :         double ma_x            = 12.19;
     174             :         double ma_y            = 17.44;
     175             :         double ma_z            = 26.47;
     176             :         double ma_roll         = 0.15;
     177             :         double ma_pitch        = 0.26;
     178             :         double ma_yaw          = 0.19;
     179             :         double dl_x            = 3.8;
     180             :         double dl_y            = 4.4;
     181             :         double dl_z            = 34.8;
     182             :         double dl_roll         = 0.131;
     183             :         double dl_pitch        = 0.201;
     184             :         double dl_yaw          = 0.077;
     185             :         double dnl_x           = 38.7;
     186             :         double dnl_y           = 54.6;
     187             :         double dnl_z           = 78.9;
     188             :         double dnl_roll        = 0.342;
     189             :         double dnl_pitch       = 0.465;
     190             :         double dnl_yaw         = 0.451;
     191             :         double rbb_x           = 0.0;
     192             :         double rbb_y           = 0.0;
     193             :         double rbb_z           = -0.05593619792;
     194             :         double rgb_x           = 0.0;
     195             :         double rgb_y           = 0.0;
     196             :         double rgb_z           = 0.0;
     197             :         double cg_offset_x     = 0.0;
     198             :         double cg_offset_y     = 0.0;
     199             :         double cg_offset_z     = 0.0;
     200             :     };
     201             : 
     202             :     // clang-format off
     203             :     /** @brief Maps ROS parameter names to their corresponding Params member pointers */
     204             :     static inline const std::unordered_map<std::string_view, double Params::*> DOUBLE_PARAM_MAP = {
     205             :         {"ff_gain", &Params::ff_gain},
     206             :         {"fb_gain", &Params::fb_gain},
     207             :         {"buoyancy_gain", &Params::buoyancy_gain},
     208             :         {"surface_threshold", &Params::surface_threshold},
     209             :         {"axis_gain_x", &Params::axis_gain_x},
     210             :         {"axis_gain_y", &Params::axis_gain_y},
     211             :         {"axis_gain_z", &Params::axis_gain_z},
     212             :         {"axis_gain_roll", &Params::axis_gain_roll},
     213             :         {"axis_gain_pitch", &Params::axis_gain_pitch},
     214             :         {"axis_gain_yaw", &Params::axis_gain_yaw},
     215             :         {"kp_x", &Params::kp_x},
     216             :         {"ki_x", &Params::ki_x},
     217             :         {"kd_x", &Params::kd_x},
     218             :         {"kp_y", &Params::kp_y},
     219             :         {"ki_y", &Params::ki_y},
     220             :         {"kd_y", &Params::kd_y},
     221             :         {"kp_z", &Params::kp_z},
     222             :         {"ki_z", &Params::ki_z},
     223             :         {"kd_z", &Params::kd_z},
     224             :         {"kp_roll", &Params::kp_roll},
     225             :         {"ki_roll", &Params::ki_roll},
     226             :         {"kd_roll", &Params::kd_roll},
     227             :         {"kp_pitch", &Params::kp_pitch},
     228             :         {"ki_pitch", &Params::ki_pitch},
     229             :         {"kd_pitch", &Params::kd_pitch},
     230             :         {"kp_yaw", &Params::kp_yaw},
     231             :         {"ki_yaw", &Params::ki_yaw},
     232             :         {"kd_yaw", &Params::kd_yaw},
     233             :         {"kp_x_surface", &Params::kp_x_surface},
     234             :         {"ki_x_surface", &Params::ki_x_surface},
     235             :         {"kd_x_surface", &Params::kd_x_surface},
     236             :         {"kp_y_surface", &Params::kp_y_surface},
     237             :         {"ki_y_surface", &Params::ki_y_surface},
     238             :         {"kd_y_surface", &Params::kd_y_surface},
     239             :         {"kp_z_surface", &Params::kp_z_surface},
     240             :         {"ki_z_surface", &Params::ki_z_surface},
     241             :         {"kd_z_surface", &Params::kd_z_surface},
     242             :         {"kp_roll_surface", &Params::kp_roll_surface},
     243             :         {"ki_roll_surface", &Params::ki_roll_surface},
     244             :         {"kd_roll_surface", &Params::kd_roll_surface},
     245             :         {"kp_pitch_surface", &Params::kp_pitch_surface},
     246             :         {"ki_pitch_surface", &Params::ki_pitch_surface},
     247             :         {"kd_pitch_surface", &Params::kd_pitch_surface},
     248             :         {"kp_yaw_surface", &Params::kp_yaw_surface},
     249             :         {"ki_yaw_surface", &Params::ki_yaw_surface},
     250             :         {"kd_yaw_surface", &Params::kd_yaw_surface},
     251             :         {"buoyancy_adaptive_rate", &Params::buoyancy_adaptive_rate},
     252             :         {"buoyancy_adaptive_clamp_pct", &Params::buoyancy_adaptive_clamp_pct},
     253             :         {"buoyancy_adaptive_pos_thresh", &Params::buoyancy_adaptive_pos_thresh},
     254             :         {"buoyancy_adaptive_vel_thresh", &Params::buoyancy_adaptive_vel_thresh},
     255             :         {"buoyancy_adaptive_rate_rp", &Params::buoyancy_adaptive_rate_rp},
     256             :         {"buoyancy_adaptive_pos_thresh_rp", &Params::buoyancy_adaptive_pos_thresh_rp},
     257             :         {"buoyancy_adaptive_vel_thresh_rp", &Params::buoyancy_adaptive_vel_thresh_rp},
     258             :         {"slow_down_gain", &Params::slow_down_gain},
     259             :         {"max_xy_vel", &Params::max_xy_vel},
     260             :         {"max_xy_acc", &Params::max_xy_acc},
     261             :         {"max_xy_jerk", &Params::max_xy_jerk},
     262             :         {"max_z_vel", &Params::max_z_vel},
     263             :         {"max_z_acc", &Params::max_z_acc},
     264             :         {"max_z_jerk", &Params::max_z_jerk},
     265             :         {"max_yaw_vel", &Params::max_yaw_vel},
     266             :         {"max_yaw_acc", &Params::max_yaw_acc},
     267             :         {"max_yaw_jerk", &Params::max_yaw_jerk},
     268             :         {"mass", &Params::mass},
     269             :         {"buoyancy_factor", &Params::buoyancy_factor},
     270             :         {"i_xx", &Params::i_xx},
     271             :         {"i_yy", &Params::i_yy},
     272             :         {"i_zz", &Params::i_zz},
     273             :         {"i_xy", &Params::i_xy},
     274             :         {"i_xz", &Params::i_xz},
     275             :         {"i_yz", &Params::i_yz},
     276             :         {"ma_x", &Params::ma_x},
     277             :         {"ma_y", &Params::ma_y},
     278             :         {"ma_z", &Params::ma_z},
     279             :         {"ma_roll", &Params::ma_roll},
     280             :         {"ma_pitch", &Params::ma_pitch},
     281             :         {"ma_yaw", &Params::ma_yaw},
     282             :         {"dl_x", &Params::dl_x},
     283             :         {"dl_y", &Params::dl_y},
     284             :         {"dl_z", &Params::dl_z},
     285             :         {"dl_roll", &Params::dl_roll},
     286             :         {"dl_pitch", &Params::dl_pitch},
     287             :         {"dl_yaw", &Params::dl_yaw},
     288             :         {"dnl_x", &Params::dnl_x},
     289             :         {"dnl_y", &Params::dnl_y},
     290             :         {"dnl_z", &Params::dnl_z},
     291             :         {"dnl_roll", &Params::dnl_roll},
     292             :         {"dnl_pitch", &Params::dnl_pitch},
     293             :         {"dnl_yaw", &Params::dnl_yaw},
     294             :         {"rbb_x", &Params::rbb_x},
     295             :         {"rbb_y", &Params::rbb_y},
     296             :         {"rbb_z", &Params::rbb_z},
     297             :         {"rgb_x", &Params::rgb_x},
     298             :         {"rgb_y", &Params::rgb_y},
     299             :         {"rgb_z", &Params::rgb_z},
     300             :         {"cg_offset_x", &Params::cg_offset_x},
     301             :         {"cg_offset_y", &Params::cg_offset_y},
     302             :         {"cg_offset_z", &Params::cg_offset_z},
     303             :     };
     304             : 
     305             :     static inline const std::unordered_map<std::string_view, bool Params::*> BOOL_PARAM_MAP = {
     306             :         {"surface_gain_scheduling", &Params::surface_gain_scheduling},
     307             :         {"buoyancy_adaptive_z", &Params::buoyancy_adaptive_z},
     308             :         {"buoyancy_adaptive_rp", &Params::buoyancy_adaptive_rp},
     309             :     };
     310             :     // clang-format on
     311             : 
     312             : protected:
     313             :     /** @brief Control loop frequency in Hz */
     314             :     static constexpr int CONTROL_LOOP_RATE = 20;
     315             : 
     316             :     /** @brief Minimum depth value clamp to avoid near-zero depth noise (m) */
     317             :     static constexpr double MIN_DEPTH_CLAMP = 0.00;
     318             : 
     319             :     /** @brief Position jump threshold for outlier rejection (m) */
     320             :     static constexpr double POSITION_JUMP_THRESHOLD = 0.5;
     321             : 
     322             :     /** @brief Flag for starting control loop **/
     323             :     std::once_flag control_loop_started_;
     324             : 
     325             :     /** @brief TF child_frame_id of the vehicle body frame */
     326             :     const std::string vehicle_frame_id_;
     327             : 
     328             :     /** @brief TF frame_id of the world frame */
     329             :     const std::string world_frame_id_;
     330             : 
     331             :     /** @brief Encapsulated vehicle control stack */
     332             :     std::unique_ptr<Vehicle> auv_;
     333             : 
     334             :     // ROS subscribers
     335             :     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr world_sub_;
     336             :     rclcpp::Subscription<bb_sensor_msgs::msg::BatteriesWithId>::SharedPtr battery_sub_;
     337             :     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr teleop_force_sub_;
     338             : 
     339             :     // ROS publishers
     340             :     rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr pub_controller_force_;
     341             :     rclcpp::Publisher<bb_controls_msgs::msg::ControllerStatus>::SharedPtr pub_status_;
     342             :     rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pub_traj_viz_;
     343             :     rclcpp::Publisher<bb_controls_msgs::msg::Thrusters>::SharedPtr pub_thruster_commands_;
     344             :     rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr pub_allocator_force_;
     345             :     rclcpp::Publisher<bb_controls_msgs::msg::ThrusterForces>::SharedPtr pub_thruster_forces_;
     346             : 
     347             :     rclcpp::TimerBase::SharedPtr control_loop_timer_;
     348             :     rclcpp::TimerBase::SharedPtr debug_publish_timer_;
     349             : 
     350             :     /** @brief Lock-free queue through which the control loop passes debug messages to the publish timer */
     351             :     moodycamel::ConcurrentQueue<DebugMessage> debug_queue_;
     352             : 
     353             :     rclcpp::Service<bb_controls_msgs::srv::Controller>::SharedPtr controller_srv_;
     354             :     rclcpp::Service<bb_controls_msgs::srv::SplineTraj>::SharedPtr spline_traj_srv_;
     355             :     rclcpp::Service<bb_controls_msgs::srv::EncircleTraj>::SharedPtr encircle_traj_srv_;
     356             : 
     357             :     rclcpp_action::Server<Locomotion>::SharedPtr poly_action_server_;
     358             :     std::mutex action_goal_mutex_;
     359             :     std::shared_ptr<GoalHandleLocomotion> current_action_goal_;
     360             : 
     361             :     /** @brief Signals the action execution thread to exit its loop */
     362             :     std::atomic<bool> shutdown_requested_{false};
     363             : 
     364             :     /** @brief Thread running action execute method; joined before destruction */
     365             :     std::thread action_execute_thread_;
     366             : 
     367             :     OnSetParametersCallbackHandle::SharedPtr callback_handle_;
     368             : 
     369             :     /** @brief Local snapshot of ROS parameters, pushed to Vehicle subsystems on change */
     370             :     Params config_;
     371             : 
     372             :     /** @brief ROS timestamp of the previous control loop iteration */
     373             :     double prev_time_ = 0.0;
     374             : 
     375             :     rclcpp::CallbackGroup::SharedPtr odom_cb_group_, control_loop_cb_group_, services_cb_group_, actions_cb_group_,
     376             :         aux_cb_group_;
     377             : 
     378             :     /**
     379             :      * @brief Executes one iteration of the 20 Hz control loop
     380             :      *
     381             :      * Calls process_pending, get_force, allocate_force, and publishes thruster
     382             :      * commands.  Enqueues debug data for the background publish timer.
     383             :      */
     384           1 :     void control_loop();
     385             : 
     386             :     /**
     387             :      * @brief Publishes a ControllerStatus message from the given status snapshot
     388             :      *
     389             :      * @param status [in] Controller status snapshot to publish
     390             :      */
     391           1 :     void publish_controller_status(const Vehicle::ControllerStatus& status);
     392             : 
     393             :     /**
     394             :      * @brief Drains the debug queue and publishes all pending messages
     395             :      *
     396             :      * Also publishes the current controller status.  Called at 20 Hz by a
     397             :      * separate timer on the aux callback group.
     398             :      */
     399           1 :     void debug_publish_loop();
     400             : 
     401             :     /**
     402             :      * @brief Callback invoked by the ROS parameter system when any parameter changes
     403             :      *
     404             :      * @param parameters [in] List of parameters that were set
     405             :      *
     406             :      * @return Result indicating success or failure
     407             :      */
     408           1 :     rcl_interfaces::msg::SetParametersResult parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
     409             : 
     410             :     /**
     411             :      * @brief Registers all entries in DOUBLE_PARAM_MAP / BOOL_PARAM_MAP with the ROS parameter server
     412             :      */
     413           1 :     void declare_controller_params();
     414             : 
     415             :     /**
     416             :      * @brief Reads all declared ROS parameters into config_
     417             :      *
     418             :      * Only valid when values have already been committed to the server (i.e. at
     419             :      * startup, after declare_controller_params).  Must NOT be called from inside
     420             :      * a set-parameter callback, where the new values are not yet committed.
     421             :      */
     422           1 :     void load_params_from_server();
     423             : 
     424             :     /**
     425             :      * @brief Applies config_ to the Vehicle's dynamics model, controller gains, and limits
     426             :      */
     427           1 :     void apply_config_to_vehicle();
     428             : 
     429             :     /**
     430             :      * @brief Odometry subscriber callback — updates vehicle state and starts the control loop
     431             :      *
     432             :      * @param m [in] Incoming odometry message
     433             :      */
     434           1 :     void sub_callback_world_position(const nav_msgs::msg::Odometry::ConstSharedPtr& m);
     435             : 
     436             :     /**
     437             :      * @brief Battery subscriber callback — forwards the minimum cell voltage to the Vehicle
     438             :      *
     439             :      * @param msg [in] Incoming battery message containing per-cell voltages
     440             :      */
     441           1 :     void sub_callback_battery_voltage(const bb_sensor_msgs::msg::BatteriesWithId::ConstSharedPtr& msg);
     442             : 
     443             :     /**
     444             :      * @brief Teleop subscriber callback — allocates the raw force directly if the controller is disabled
     445             :      *
     446             :      * If the controller is not disabled, issues a disable request and ignores the message.
     447             :      *
     448             :      * @param msg [in] Twist message interpreted as a 6DOF body force [Fx, Fy, Fz, Tx, Ty, Tz]
     449             :      */
     450           1 :     void sub_callback_teleop_force(const geometry_msgs::msg::Twist::ConstSharedPtr& msg);
     451             : 
     452             :     /**
     453             :      * @brief Service callback to enable (stationkeep) or disable the controller
     454             :      *
     455             :      * @param req [in]  Request with an enable flag
     456             :      * @param res [out] Response with a status flag (always true)
     457             :      */
     458           1 :     void srv_callback_controller(const std::shared_ptr<bb_controls_msgs::srv::Controller::Request> req,
     459             :                                  std::shared_ptr<bb_controls_msgs::srv::Controller::Response> res);
     460             : 
     461             :     /**
     462             :      * @brief Service callback to request a straight XYZ or multi-waypoint spline trajectory
     463             :      *
     464             :      * @param req [in]  SplineTraj request containing waypoints and relative-coordinate flags
     465             :      * @param res [out] Response populated with the assigned trajectory UUID, or empty on failure
     466             :      */
     467           1 :     void srv_callback_trajectory_xyz(const std::shared_ptr<bb_controls_msgs::srv::SplineTraj::Request> req,
     468             :                                      std::shared_ptr<bb_controls_msgs::srv::SplineTraj::Response> res);
     469             : 
     470             :     /**
     471             :      * @brief Service callback to request a circular arc (encircle) trajectory
     472             :      *
     473             :      * @param req [in]  EncircleTraj request containing radius, turn angle, and spiral parameters
     474             :      * @param res [out] Response populated with the assigned trajectory UUID, or empty on failure
     475             :      */
     476           1 :     void srv_callback_encircle_traj(const std::shared_ptr<bb_controls_msgs::srv::EncircleTraj::Request> req,
     477             :                                     std::shared_ptr<bb_controls_msgs::srv::EncircleTraj::Response> res);
     478             : 
     479             :     /**
     480             :      * @brief Action server goal handler — rejects goals when the controller is disabled
     481             :      *
     482             :      * @param uuid [in] Goal UUID assigned by the action client
     483             :      * @param goal [in] Locomotion goal
     484             :      *
     485             :      * @return ACCEPT_AND_EXECUTE if the controller is enabled; REJECT otherwise
     486             :      */
     487           1 :     rclcpp_action::GoalResponse poly_handle_goal(const rclcpp_action::GoalUUID& uuid,
     488             :                                                  std::shared_ptr<const Locomotion::Goal> goal);
     489             : 
     490             :     /**
     491             :      * @brief Action server cancel handler — always accepts cancel requests
     492             :      *
     493             :      * @param goal_handle [in] Handle to the goal being cancelled
     494             :      *
     495             :      * @return CancelResponse::ACCEPT
     496             :      */
     497           1 :     rclcpp_action::CancelResponse poly_handle_cancel(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
     498             : 
     499             :     /**
     500             :      * @brief Action server accepted handler — preempts any active goal and starts a new execute thread
     501             :      *
     502             :      * @param goal_handle [in] Handle to the newly accepted goal
     503             :      */
     504           1 :     void poly_handle_accepted(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
     505             : 
     506             :     /**
     507             :      * @brief Executes a Locomotion action: plans trajectory, waits for completion, checks tolerances
     508             :      *
     509             :      * Runs in action_execute_thread_.  Publishes periodic feedback and resolves the
     510             :      * action result to SUCCESS when pose errors fall within the goal tolerances,
     511             :      * or to ABORTED/FAILURE on preemption or planning errors.
     512             :      *
     513             :      * @param goal_handle [in] Handle used to publish feedback and set the final result
     514             :      */
     515           1 :     void poly_execute_action(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
     516             : };
     517             : 
     518             : #endif  // CONTROLS_NODES_CONTROLSROS2WRAPPER_H