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
     152             :         bool buoyancy_adaptive              = false;
     153             :         double buoyancy_adaptive_max_scale  = 5.0;
     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             :         // System dynamics parameters
     159             :         double mass            = 36.10;
     160             :         double buoyancy_factor = 1.0;
     161             :         double i_xx            = 0.7699579;
     162             :         double i_yy            = 0.7699579;
     163             :         double i_zz            = 1.38624;
     164             :         double i_xy            = 0.0;
     165             :         double i_xz            = 0.0;
     166             :         double i_yz            = 0.0;
     167             :         double ma_x            = 12.19;
     168             :         double ma_y            = 17.44;
     169             :         double ma_z            = 26.47;
     170             :         double ma_roll         = 0.15;
     171             :         double ma_pitch        = 0.26;
     172             :         double ma_yaw          = 0.19;
     173             :         double dl_x            = 3.8;
     174             :         double dl_y            = 4.4;
     175             :         double dl_z            = 34.8;
     176             :         double dl_roll         = 0.131;
     177             :         double dl_pitch        = 0.201;
     178             :         double dl_yaw          = 0.077;
     179             :         double dnl_x           = 38.7;
     180             :         double dnl_y           = 54.6;
     181             :         double dnl_z           = 78.9;
     182             :         double dnl_roll        = 0.342;
     183             :         double dnl_pitch       = 0.465;
     184             :         double dnl_yaw         = 0.451;
     185             :         double rbb_x           = 0.0;
     186             :         double rbb_y           = 0.0;
     187             :         double rbb_z           = -0.05593619792;
     188             :         double rgb_x           = 0.0;
     189             :         double rgb_y           = 0.0;
     190             :         double rgb_z           = 0.0;
     191             :         double cg_offset_x     = 0.0;
     192             :         double cg_offset_y     = 0.0;
     193             :         double cg_offset_z     = 0.0;
     194             :     };
     195             : 
     196             :     // clang-format off
     197             :     /** @brief Maps ROS parameter names to their corresponding Params member pointers */
     198             :     static inline const std::unordered_map<std::string_view, double Params::*> DOUBLE_PARAM_MAP = {
     199             :         {"ff_gain", &Params::ff_gain},
     200             :         {"fb_gain", &Params::fb_gain},
     201             :         {"buoyancy_gain", &Params::buoyancy_gain},
     202             :         {"surface_threshold", &Params::surface_threshold},
     203             :         {"axis_gain_x", &Params::axis_gain_x},
     204             :         {"axis_gain_y", &Params::axis_gain_y},
     205             :         {"axis_gain_z", &Params::axis_gain_z},
     206             :         {"axis_gain_roll", &Params::axis_gain_roll},
     207             :         {"axis_gain_pitch", &Params::axis_gain_pitch},
     208             :         {"axis_gain_yaw", &Params::axis_gain_yaw},
     209             :         {"kp_x", &Params::kp_x},
     210             :         {"ki_x", &Params::ki_x},
     211             :         {"kd_x", &Params::kd_x},
     212             :         {"kp_y", &Params::kp_y},
     213             :         {"ki_y", &Params::ki_y},
     214             :         {"kd_y", &Params::kd_y},
     215             :         {"kp_z", &Params::kp_z},
     216             :         {"ki_z", &Params::ki_z},
     217             :         {"kd_z", &Params::kd_z},
     218             :         {"kp_roll", &Params::kp_roll},
     219             :         {"ki_roll", &Params::ki_roll},
     220             :         {"kd_roll", &Params::kd_roll},
     221             :         {"kp_pitch", &Params::kp_pitch},
     222             :         {"ki_pitch", &Params::ki_pitch},
     223             :         {"kd_pitch", &Params::kd_pitch},
     224             :         {"kp_yaw", &Params::kp_yaw},
     225             :         {"ki_yaw", &Params::ki_yaw},
     226             :         {"kd_yaw", &Params::kd_yaw},
     227             :         {"kp_x_surface", &Params::kp_x_surface},
     228             :         {"ki_x_surface", &Params::ki_x_surface},
     229             :         {"kd_x_surface", &Params::kd_x_surface},
     230             :         {"kp_y_surface", &Params::kp_y_surface},
     231             :         {"ki_y_surface", &Params::ki_y_surface},
     232             :         {"kd_y_surface", &Params::kd_y_surface},
     233             :         {"kp_z_surface", &Params::kp_z_surface},
     234             :         {"ki_z_surface", &Params::ki_z_surface},
     235             :         {"kd_z_surface", &Params::kd_z_surface},
     236             :         {"kp_roll_surface", &Params::kp_roll_surface},
     237             :         {"ki_roll_surface", &Params::ki_roll_surface},
     238             :         {"kd_roll_surface", &Params::kd_roll_surface},
     239             :         {"kp_pitch_surface", &Params::kp_pitch_surface},
     240             :         {"ki_pitch_surface", &Params::ki_pitch_surface},
     241             :         {"kd_pitch_surface", &Params::kd_pitch_surface},
     242             :         {"kp_yaw_surface", &Params::kp_yaw_surface},
     243             :         {"ki_yaw_surface", &Params::ki_yaw_surface},
     244             :         {"kd_yaw_surface", &Params::kd_yaw_surface},
     245             :         {"buoyancy_adaptive_max_scale", &Params::buoyancy_adaptive_max_scale},
     246             :         {"buoyancy_adaptive_clamp_pct", &Params::buoyancy_adaptive_clamp_pct},
     247             :         {"buoyancy_adaptive_pos_thresh", &Params::buoyancy_adaptive_pos_thresh},
     248             :         {"buoyancy_adaptive_vel_thresh", &Params::buoyancy_adaptive_vel_thresh},
     249             :         {"slow_down_gain", &Params::slow_down_gain},
     250             :         {"max_xy_vel", &Params::max_xy_vel},
     251             :         {"max_xy_acc", &Params::max_xy_acc},
     252             :         {"max_xy_jerk", &Params::max_xy_jerk},
     253             :         {"max_z_vel", &Params::max_z_vel},
     254             :         {"max_z_acc", &Params::max_z_acc},
     255             :         {"max_z_jerk", &Params::max_z_jerk},
     256             :         {"max_yaw_vel", &Params::max_yaw_vel},
     257             :         {"max_yaw_acc", &Params::max_yaw_acc},
     258             :         {"max_yaw_jerk", &Params::max_yaw_jerk},
     259             :         {"mass", &Params::mass},
     260             :         {"buoyancy_factor", &Params::buoyancy_factor},
     261             :         {"i_xx", &Params::i_xx},
     262             :         {"i_yy", &Params::i_yy},
     263             :         {"i_zz", &Params::i_zz},
     264             :         {"i_xy", &Params::i_xy},
     265             :         {"i_xz", &Params::i_xz},
     266             :         {"i_yz", &Params::i_yz},
     267             :         {"ma_x", &Params::ma_x},
     268             :         {"ma_y", &Params::ma_y},
     269             :         {"ma_z", &Params::ma_z},
     270             :         {"ma_roll", &Params::ma_roll},
     271             :         {"ma_pitch", &Params::ma_pitch},
     272             :         {"ma_yaw", &Params::ma_yaw},
     273             :         {"dl_x", &Params::dl_x},
     274             :         {"dl_y", &Params::dl_y},
     275             :         {"dl_z", &Params::dl_z},
     276             :         {"dl_roll", &Params::dl_roll},
     277             :         {"dl_pitch", &Params::dl_pitch},
     278             :         {"dl_yaw", &Params::dl_yaw},
     279             :         {"dnl_x", &Params::dnl_x},
     280             :         {"dnl_y", &Params::dnl_y},
     281             :         {"dnl_z", &Params::dnl_z},
     282             :         {"dnl_roll", &Params::dnl_roll},
     283             :         {"dnl_pitch", &Params::dnl_pitch},
     284             :         {"dnl_yaw", &Params::dnl_yaw},
     285             :         {"rbb_x", &Params::rbb_x},
     286             :         {"rbb_y", &Params::rbb_y},
     287             :         {"rbb_z", &Params::rbb_z},
     288             :         {"rgb_x", &Params::rgb_x},
     289             :         {"rgb_y", &Params::rgb_y},
     290             :         {"rgb_z", &Params::rgb_z},
     291             :         {"cg_offset_x", &Params::cg_offset_x},
     292             :         {"cg_offset_y", &Params::cg_offset_y},
     293             :         {"cg_offset_z", &Params::cg_offset_z},
     294             :     };
     295             : 
     296             :     static inline const std::unordered_map<std::string_view, bool Params::*> BOOL_PARAM_MAP = {
     297             :         {"surface_gain_scheduling", &Params::surface_gain_scheduling},
     298             :         {"buoyancy_adaptive", &Params::buoyancy_adaptive},
     299             :     };
     300             :     // clang-format on
     301             : 
     302             : protected:
     303             :     /** @brief Control loop frequency in Hz */
     304             :     static constexpr int CONTROL_LOOP_RATE = 20;
     305             : 
     306             :     /** @brief Minimum depth value clamp to avoid near-zero depth noise (m) */
     307             :     static constexpr double MIN_DEPTH_CLAMP = 0.00;
     308             : 
     309             :     /** @brief Position jump threshold for outlier rejection (m) */
     310             :     static constexpr double POSITION_JUMP_THRESHOLD = 0.5;
     311             : 
     312             :     /** @brief Flag for starting control loop **/
     313             :     std::once_flag control_loop_started_;
     314             : 
     315             :     /** @brief TF child_frame_id of the vehicle body frame */
     316             :     const std::string vehicle_frame_id_;
     317             : 
     318             :     /** @brief TF frame_id of the world frame */
     319             :     const std::string world_frame_id_;
     320             : 
     321             :     /** @brief Encapsulated vehicle control stack */
     322             :     std::unique_ptr<Vehicle> auv_;
     323             : 
     324             :     // ROS subscribers
     325             :     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr world_sub_;
     326             :     rclcpp::Subscription<bb_sensor_msgs::msg::BatteriesWithId>::SharedPtr battery_sub_;
     327             :     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr teleop_force_sub_;
     328             : 
     329             :     // ROS publishers
     330             :     rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr pub_controller_force_;
     331             :     rclcpp::Publisher<bb_controls_msgs::msg::ControllerStatus>::SharedPtr pub_status_;
     332             :     rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pub_traj_viz_;
     333             :     rclcpp::Publisher<bb_controls_msgs::msg::Thrusters>::SharedPtr pub_thruster_commands_;
     334             :     rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr pub_allocator_force_;
     335             :     rclcpp::Publisher<bb_controls_msgs::msg::ThrusterForces>::SharedPtr pub_thruster_forces_;
     336             : 
     337             :     rclcpp::TimerBase::SharedPtr control_loop_timer_;
     338             :     rclcpp::TimerBase::SharedPtr debug_publish_timer_;
     339             : 
     340             :     /** @brief Lock-free queue through which the control loop passes debug messages to the publish timer */
     341             :     moodycamel::ConcurrentQueue<DebugMessage> debug_queue_;
     342             : 
     343             :     rclcpp::Service<bb_controls_msgs::srv::Controller>::SharedPtr controller_srv_;
     344             :     rclcpp::Service<bb_controls_msgs::srv::SplineTraj>::SharedPtr spline_traj_srv_;
     345             :     rclcpp::Service<bb_controls_msgs::srv::EncircleTraj>::SharedPtr encircle_traj_srv_;
     346             : 
     347             :     rclcpp_action::Server<Locomotion>::SharedPtr poly_action_server_;
     348             :     std::mutex action_goal_mutex_;
     349             :     std::shared_ptr<GoalHandleLocomotion> current_action_goal_;
     350             : 
     351             :     /** @brief Signals the action execution thread to exit its loop */
     352             :     std::atomic<bool> shutdown_requested_{false};
     353             : 
     354             :     /** @brief Thread running action execute method; joined before destruction */
     355             :     std::thread action_execute_thread_;
     356             : 
     357             :     OnSetParametersCallbackHandle::SharedPtr callback_handle_;
     358             : 
     359             :     /** @brief Local snapshot of ROS parameters, pushed to Vehicle subsystems on change */
     360             :     Params config_;
     361             : 
     362             :     /** @brief ROS timestamp of the previous control loop iteration */
     363             :     double prev_time_ = 0.0;
     364             : 
     365             :     rclcpp::CallbackGroup::SharedPtr odom_cb_group_, control_loop_cb_group_, services_cb_group_, actions_cb_group_,
     366             :         aux_cb_group_;
     367             : 
     368             :     /**
     369             :      * @brief Executes one iteration of the 20 Hz control loop
     370             :      *
     371             :      * Calls process_pending, get_force, allocate_force, and publishes thruster
     372             :      * commands.  Enqueues debug data for the background publish timer.
     373             :      */
     374           1 :     void control_loop();
     375             : 
     376             :     /**
     377             :      * @brief Publishes a ControllerStatus message from the given status snapshot
     378             :      *
     379             :      * @param status [in] Controller status snapshot to publish
     380             :      */
     381           1 :     void publish_controller_status(const Vehicle::ControllerStatus& status);
     382             : 
     383             :     /**
     384             :      * @brief Drains the debug queue and publishes all pending messages
     385             :      *
     386             :      * Also publishes the current controller status.  Called at 20 Hz by a
     387             :      * separate timer on the aux callback group.
     388             :      */
     389           1 :     void debug_publish_loop();
     390             : 
     391             :     /**
     392             :      * @brief Callback invoked by the ROS parameter system when any parameter changes
     393             :      *
     394             :      * @param parameters [in] List of parameters that were set
     395             :      *
     396             :      * @return Result indicating success or failure
     397             :      */
     398           1 :     rcl_interfaces::msg::SetParametersResult parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
     399             : 
     400             :     /**
     401             :      * @brief Registers all entries in DOUBLE_PARAM_MAP / BOOL_PARAM_MAP with the ROS parameter server
     402             :      */
     403           1 :     void declare_controller_params();
     404             : 
     405             :     /**
     406             :      * @brief Reads all declared ROS parameters into config_
     407             :      *
     408             :      * Only valid when values have already been committed to the server (i.e. at
     409             :      * startup, after declare_controller_params).  Must NOT be called from inside
     410             :      * a set-parameter callback, where the new values are not yet committed.
     411             :      */
     412           1 :     void load_params_from_server();
     413             : 
     414             :     /**
     415             :      * @brief Applies config_ to the Vehicle's dynamics model, controller gains, and limits
     416             :      */
     417           1 :     void apply_config_to_vehicle();
     418             : 
     419             :     /**
     420             :      * @brief Odometry subscriber callback — updates vehicle state and starts the control loop
     421             :      *
     422             :      * @param m [in] Incoming odometry message
     423             :      */
     424           1 :     void sub_callback_world_position(const nav_msgs::msg::Odometry::ConstSharedPtr& m);
     425             : 
     426             :     /**
     427             :      * @brief Battery subscriber callback — forwards the minimum cell voltage to the Vehicle
     428             :      *
     429             :      * @param msg [in] Incoming battery message containing per-cell voltages
     430             :      */
     431           1 :     void sub_callback_battery_voltage(const bb_sensor_msgs::msg::BatteriesWithId::ConstSharedPtr& msg);
     432             : 
     433             :     /**
     434             :      * @brief Teleop subscriber callback — allocates the raw force directly if the controller is disabled
     435             :      *
     436             :      * If the controller is not disabled, issues a disable request and ignores the message.
     437             :      *
     438             :      * @param msg [in] Twist message interpreted as a 6DOF body force [Fx, Fy, Fz, Tx, Ty, Tz]
     439             :      */
     440           1 :     void sub_callback_teleop_force(const geometry_msgs::msg::Twist::ConstSharedPtr& msg);
     441             : 
     442             :     /**
     443             :      * @brief Service callback to enable (stationkeep) or disable the controller
     444             :      *
     445             :      * @param req [in]  Request with an enable flag
     446             :      * @param res [out] Response with a status flag (always true)
     447             :      */
     448           1 :     void srv_callback_controller(const std::shared_ptr<bb_controls_msgs::srv::Controller::Request> req,
     449             :                                  std::shared_ptr<bb_controls_msgs::srv::Controller::Response> res);
     450             : 
     451             :     /**
     452             :      * @brief Service callback to request a straight XYZ or multi-waypoint spline trajectory
     453             :      *
     454             :      * @param req [in]  SplineTraj request containing waypoints and relative-coordinate flags
     455             :      * @param res [out] Response populated with the assigned trajectory UUID, or empty on failure
     456             :      */
     457           1 :     void srv_callback_trajectory_xyz(const std::shared_ptr<bb_controls_msgs::srv::SplineTraj::Request> req,
     458             :                                      std::shared_ptr<bb_controls_msgs::srv::SplineTraj::Response> res);
     459             : 
     460             :     /**
     461             :      * @brief Service callback to request a circular arc (encircle) trajectory
     462             :      *
     463             :      * @param req [in]  EncircleTraj request containing radius, turn angle, and spiral parameters
     464             :      * @param res [out] Response populated with the assigned trajectory UUID, or empty on failure
     465             :      */
     466           1 :     void srv_callback_encircle_traj(const std::shared_ptr<bb_controls_msgs::srv::EncircleTraj::Request> req,
     467             :                                     std::shared_ptr<bb_controls_msgs::srv::EncircleTraj::Response> res);
     468             : 
     469             :     /**
     470             :      * @brief Action server goal handler — rejects goals when the controller is disabled
     471             :      *
     472             :      * @param uuid [in] Goal UUID assigned by the action client
     473             :      * @param goal [in] Locomotion goal
     474             :      *
     475             :      * @return ACCEPT_AND_EXECUTE if the controller is enabled; REJECT otherwise
     476             :      */
     477           1 :     rclcpp_action::GoalResponse poly_handle_goal(const rclcpp_action::GoalUUID& uuid,
     478             :                                                  std::shared_ptr<const Locomotion::Goal> goal);
     479             : 
     480             :     /**
     481             :      * @brief Action server cancel handler — always accepts cancel requests
     482             :      *
     483             :      * @param goal_handle [in] Handle to the goal being cancelled
     484             :      *
     485             :      * @return CancelResponse::ACCEPT
     486             :      */
     487           1 :     rclcpp_action::CancelResponse poly_handle_cancel(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
     488             : 
     489             :     /**
     490             :      * @brief Action server accepted handler — preempts any active goal and starts a new execute thread
     491             :      *
     492             :      * @param goal_handle [in] Handle to the newly accepted goal
     493             :      */
     494           1 :     void poly_handle_accepted(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
     495             : 
     496             :     /**
     497             :      * @brief Executes a Locomotion action: plans trajectory, waits for completion, checks tolerances
     498             :      *
     499             :      * Runs in action_execute_thread_.  Publishes periodic feedback and resolves the
     500             :      * action result to SUCCESS when pose errors fall within the goal tolerances,
     501             :      * or to ABORTED/FAILURE on preemption or planning errors.
     502             :      *
     503             :      * @param goal_handle [in] Handle used to publish feedback and set the final result
     504             :      */
     505           1 :     void poly_execute_action(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
     506             : };
     507             : 
     508             : #endif  // CONTROLS_NODES_CONTROLSROS2WRAPPER_H