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 <controls/controls_parameters.hpp>
      23             : #include <memory>
      24             : #include <mutex>
      25             : #include <rclcpp_action/rclcpp_action.hpp>
      26             : #include <rclcpp_action/server.hpp>
      27             : #include <std_msgs/msg/bool.hpp>
      28             : #include <string>
      29             : #include <thread>
      30             : 
      31             : /**
      32             :  * @brief ROS2 node that wraps the Vehicle control stack and exposes its interface to the rest of the system
      33             :  *
      34             :  * Manages all ROS publishers, subscribers, services, and actions.  Runs the
      35             :  * 20 Hz control loop (started on receipt of the first odometry message), drains
      36             :  * a lock-free debug queue at 20 Hz on a separate timer, and forwards parameter
      37             :  * updates to the Vehicle and SystemDynamicsBase objects at runtime.
      38             :  *
      39             :  * Services:
      40             :  *   - in/controller        — Enable (stationkeep) or disable the controller
      41             :  *   - in/xyzTraj           — Request a waypoint/spline trajectory
      42             :  *   - in/encircleTraj      — Request a circular arc trajectory
      43             :  *
      44             :  * Actions:
      45             :  *   - in/locomotion/poly — Execute a waypoint trajectory with tolerance-based success checking
      46             :  *
      47             :  * Subscriptions:
      48             :  *   - in/odom_ned   — Vehicle odometry (nav_msgs/Odometry)
      49             :  *   - in/batteries  — Battery voltages
      50             :  *   - in/teleop     — Direct force commands (disables the controller if running, then allocates directly)
      51             :  *
      52             :  * Publications:
      53             :  *   - out/force              — Controller body force (WrenchStamped)
      54             :  *   - out/force/allocated    — Achieved body force after allocation (WrenchStamped)
      55             :  *   - out/trajectory/status  — Trajectory and controller status
      56             :  *   - out/trajectory/viz     — Sampled trajectory poses for RViz
      57             :  *   - out/thrusters/input    — Per-thruster PWM commands
      58             :  *   - out/thrusters/forces   — Per-thruster forces in Newtons
      59             :  */
      60           1 : class ControlsROS2Wrapper : public rclcpp::Node {
      61             : public:
      62             :     /**
      63             :      * @brief Constructs the node, initialises all ROS interfaces, and loads parameters
      64             :      *
      65             :      * @param name              [in] ROS node name
      66             :      * @param robot             [in] Fully constructed Vehicle instance
      67             :      * @param body_frame_tf_name  [in] TF frame ID of the vehicle body frame
      68             :      * @param world_frame_tf_name [in] TF frame ID of the world frame
      69             :      */
      70           1 :     explicit ControlsROS2Wrapper(const std::string& name, std::unique_ptr<Vehicle> robot,
      71             :                                  std::string body_frame_tf_name = "base_link", std::string world_frame_tf_name = "map");
      72             : 
      73           0 :     ~ControlsROS2Wrapper() override;
      74             : 
      75             :     // Non-copyable and non-movable: mutex, atomic, unique_ptr, and thread members
      76           0 :     ControlsROS2Wrapper(const ControlsROS2Wrapper&)            = delete;
      77           0 :     ControlsROS2Wrapper& operator=(const ControlsROS2Wrapper&) = delete;
      78           0 :     ControlsROS2Wrapper(ControlsROS2Wrapper&&)                 = delete;
      79           0 :     ControlsROS2Wrapper& operator=(ControlsROS2Wrapper&&)      = delete;
      80             : 
      81           0 :     using Locomotion           = bb_controls_msgs::action::Locomotion;
      82           0 :     using GoalHandleLocomotion = rclcpp_action::ServerGoalHandle<Locomotion>;
      83             : 
      84             : protected:
      85             :     /** @brief Control loop frequency in Hz */
      86             :     static constexpr int CONTROL_LOOP_RATE = 20;
      87             : 
      88             :     /** @brief Minimum depth value clamp to avoid near-zero depth noise (m) */
      89             :     static constexpr double MIN_DEPTH_CLAMP = 0.00;
      90             : 
      91             :     /** @brief Max plausible vehicle speed for odometry jump rejection (m/s) */
      92             :     static constexpr double MAX_VEL = 1.0;
      93             : 
      94             :     /** @brief Odometry gap (s) beyond which the twist low-pass filter re-initialises */
      95             :     static constexpr double TWIST_LPF_RESET_DT = 0.5;
      96             : 
      97             :     /** @brief Flag for starting control loop **/
      98             :     std::once_flag control_loop_started_;
      99             : 
     100             :     /** @brief TF child_frame_id of the vehicle body frame */
     101             :     const std::string vehicle_frame_id_;
     102             : 
     103             :     /** @brief TF frame_id of the world frame */
     104             :     const std::string world_frame_id_;
     105             : 
     106             :     /** @brief Encapsulated vehicle control stack */
     107             :     std::unique_ptr<Vehicle> auv_;
     108             : 
     109             :     // ROS subscribers
     110             :     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr world_sub_;
     111             :     rclcpp::Subscription<bb_sensor_msgs::msg::BatteriesWithId>::SharedPtr battery_sub_;
     112             :     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr teleop_force_sub_;
     113             : 
     114             :     // ROS publishers
     115             :     rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr pub_controller_force_;
     116             :     rclcpp::Publisher<bb_controls_msgs::msg::ControllerStatus>::SharedPtr pub_status_;
     117             :     rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pub_traj_viz_;
     118             :     rclcpp::Publisher<bb_controls_msgs::msg::Thrusters>::SharedPtr pub_thruster_commands_;
     119             :     rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr pub_allocator_force_;
     120             :     rclcpp::Publisher<bb_controls_msgs::msg::ThrusterForces>::SharedPtr pub_thruster_forces_;
     121             : 
     122             :     rclcpp::TimerBase::SharedPtr control_loop_timer_;
     123             :     rclcpp::TimerBase::SharedPtr debug_publish_timer_;
     124             : 
     125             :     /** @brief Lock-free queue through which the control loop passes debug messages to the publish timer */
     126             :     moodycamel::ConcurrentQueue<DebugMessage> debug_queue_;
     127             : 
     128             :     rclcpp::Service<bb_controls_msgs::srv::Controller>::SharedPtr controller_srv_;
     129             :     rclcpp::Service<bb_controls_msgs::srv::SplineTraj>::SharedPtr spline_traj_srv_;
     130             :     rclcpp::Service<bb_controls_msgs::srv::EncircleTraj>::SharedPtr encircle_traj_srv_;
     131             : 
     132             :     rclcpp_action::Server<Locomotion>::SharedPtr poly_action_server_;
     133             :     std::mutex action_goal_mutex_;
     134             :     std::shared_ptr<GoalHandleLocomotion> current_action_goal_;
     135             : 
     136             :     /** @brief Signals the action execution thread to exit its loop */
     137             :     std::atomic<bool> shutdown_requested_{false};
     138             : 
     139             :     /** @brief Thread running action execute method; joined before destruction */
     140             :     std::thread action_execute_thread_;
     141             : 
     142             :     /** @brief Declares all parameters, validates updates, and exposes the generated Params struct */
     143             :     std::shared_ptr<controls::ParamListener> param_listener_;
     144             : 
     145             :     /** @brief Local snapshot of ROS parameters, pushed to Vehicle subsystems on change */
     146             :     controls::Params config_;
     147             : 
     148             :     /** @brief ROS timestamp of the previous control loop iteration */
     149             :     double prev_time_ = 0.0;
     150             : 
     151             :     /** @brief Per-axis twist low-pass time constants (s); 0 disables an axis */
     152             :     Array6d twist_lpf_tau_ = Array6d::Zero();
     153             : 
     154             :     /** @brief State of the per-axis first-order low-pass filter on the measured twist */
     155             :     Vector6d twist_lpf_state_ = Vector6d::Zero();
     156             : 
     157             :     /** @brief Whether twist_lpf_state_ holds a valid sample */
     158             :     bool twist_lpf_init_ = false;
     159             : 
     160             :     rclcpp::CallbackGroup::SharedPtr odom_cb_group_, control_loop_cb_group_, services_cb_group_, actions_cb_group_,
     161             :         aux_cb_group_;
     162             : 
     163             :     /**
     164             :      * @brief Executes one iteration of the 20 Hz control loop
     165             :      *
     166             :      * Calls process_pending, get_force, allocate_force, and publishes thruster
     167             :      * commands.  Enqueues debug data for the background publish timer.
     168             :      */
     169           1 :     void control_loop();
     170             : 
     171             :     /**
     172             :      * @brief Publishes a ControllerStatus message from the given status snapshot
     173             :      *
     174             :      * @param status [in] Controller status snapshot to publish
     175             :      */
     176           1 :     void publish_controller_status(const Vehicle::ControllerStatus& status);
     177             : 
     178             :     /**
     179             :      * @brief Drains the debug queue and publishes all pending messages
     180             :      *
     181             :      * Also publishes the current controller status.  Called at 20 Hz by a
     182             :      * separate timer on the aux callback group.
     183             :      */
     184           1 :     void debug_publish_loop();
     185             : 
     186             :     /** @brief Pulls the latest validated params from the ParamListener and applies them on change */
     187           1 :     void refresh_params_if_changed();
     188             : 
     189             :     /**
     190             :      * @brief Applies config_ to the Vehicle's dynamics model, controller gains, and limits
     191             :      */
     192           1 :     void apply_config_to_vehicle();
     193             : 
     194             :     /**
     195             :      * @brief Odometry subscriber callback — updates vehicle state and starts the control loop
     196             :      *
     197             :      * @param m [in] Incoming odometry message
     198             :      */
     199           1 :     void sub_callback_world_position(const nav_msgs::msg::Odometry::ConstSharedPtr& m);
     200             : 
     201             :     /**
     202             :      * @brief Battery subscriber callback — forwards the minimum cell voltage to the Vehicle
     203             :      *
     204             :      * @param msg [in] Incoming battery message containing per-cell voltages
     205             :      */
     206           1 :     void sub_callback_battery_voltage(const bb_sensor_msgs::msg::BatteriesWithId::ConstSharedPtr& msg);
     207             : 
     208             :     /**
     209             :      * @brief Teleop subscriber callback — allocates the raw force directly if the controller is disabled
     210             :      *
     211             :      * If the controller is not disabled, issues a disable request and ignores the message.
     212             :      *
     213             :      * @param msg [in] Twist message interpreted as a 6DOF body force [Fx, Fy, Fz, Tx, Ty, Tz]
     214             :      */
     215           1 :     void sub_callback_teleop_force(const geometry_msgs::msg::Twist::ConstSharedPtr& msg);
     216             : 
     217             :     /**
     218             :      * @brief Service callback to enable (stationkeep) or disable the controller
     219             :      *
     220             :      * @param req [in]  Request with an enable flag
     221             :      * @param res [out] Response with a status flag (always true)
     222             :      */
     223           1 :     void srv_callback_controller(const std::shared_ptr<bb_controls_msgs::srv::Controller::Request> req,
     224             :                                  std::shared_ptr<bb_controls_msgs::srv::Controller::Response> res);
     225             : 
     226             :     /**
     227             :      * @brief Service callback to request a straight XYZ or multi-waypoint spline trajectory
     228             :      *
     229             :      * @param req [in]  SplineTraj request containing waypoints and relative-coordinate flags
     230             :      * @param res [out] Response populated with the assigned trajectory UUID, or empty on failure
     231             :      */
     232           1 :     void srv_callback_trajectory_xyz(const std::shared_ptr<bb_controls_msgs::srv::SplineTraj::Request> req,
     233             :                                      std::shared_ptr<bb_controls_msgs::srv::SplineTraj::Response> res);
     234             : 
     235             :     /**
     236             :      * @brief Service callback to request a circular arc (encircle) trajectory
     237             :      *
     238             :      * @param req [in]  EncircleTraj request containing radius, turn angle, and spiral parameters
     239             :      * @param res [out] Response populated with the assigned trajectory UUID, or empty on failure
     240             :      */
     241           1 :     void srv_callback_encircle_traj(const std::shared_ptr<bb_controls_msgs::srv::EncircleTraj::Request> req,
     242             :                                     std::shared_ptr<bb_controls_msgs::srv::EncircleTraj::Response> res);
     243             : 
     244             :     /**
     245             :      * @brief Action server goal handler — rejects goals when the controller is disabled
     246             :      *
     247             :      * @param uuid [in] Goal UUID assigned by the action client
     248             :      * @param goal [in] Locomotion goal
     249             :      *
     250             :      * @return ACCEPT_AND_EXECUTE if the controller is enabled; REJECT otherwise
     251             :      */
     252           1 :     rclcpp_action::GoalResponse poly_handle_goal(const rclcpp_action::GoalUUID& uuid,
     253             :                                                  std::shared_ptr<const Locomotion::Goal> goal);
     254             : 
     255             :     /**
     256             :      * @brief Action server cancel handler — always accepts cancel requests
     257             :      *
     258             :      * @param goal_handle [in] Handle to the goal being cancelled
     259             :      *
     260             :      * @return CancelResponse::ACCEPT
     261             :      */
     262           1 :     rclcpp_action::CancelResponse poly_handle_cancel(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
     263             : 
     264             :     /**
     265             :      * @brief Action server accepted handler — preempts any active goal and starts a new execute thread
     266             :      *
     267             :      * @param goal_handle [in] Handle to the newly accepted goal
     268             :      */
     269           1 :     void poly_handle_accepted(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
     270             : 
     271             :     /**
     272             :      * @brief Executes a Locomotion action: plans trajectory, waits for completion, checks tolerances
     273             :      *
     274             :      * Runs in action_execute_thread_.  Publishes periodic feedback and resolves the
     275             :      * action result to SUCCESS when pose errors fall within the goal tolerances,
     276             :      * or to ABORTED/FAILURE on preemption or planning errors.
     277             :      *
     278             :      * @param goal_handle [in] Handle used to publish feedback and set the final result
     279             :      */
     280           1 :     void poly_execute_action(const std::shared_ptr<GoalHandleLocomotion> goal_handle);
     281             : };
     282             : 
     283             : #endif  // CONTROLS_NODES_CONTROLSROS2WRAPPER_H