@@ -83,6 +83,80 @@ std::map<int8_t, std::string> pbsrv_enum2str = {{0, "OK"},
8383 {-4 , " BUSY" }};
8484
8585
86+ /* *
87+ * @brief ROS 2 Interface node for commanding and subscribing to buoy controllers and sensors.
88+ *
89+ * This template class uses the Curiously Recurring Template Pattern (CRTP) to provide a
90+ * compile-time polymorphic interface for creating node-based controllers. By using CRTP,
91+ * derived classes can override callback methods and parameter-setting behavior without incurring
92+ * the overhead of virtual dispatch. The derived class must pass itself as the template parameter
93+ * to `Interface`, enabling the base class to call into user-defined implementations.
94+ *
95+ * Provides service clients and functions to send commands to and receive telemetry from the
96+ * MBARI WEC controllers:
97+ *
98+ * - AHRS
99+ * - Power
100+ * - Spring
101+ * - Battery
102+ * - Trefoil
103+ *
104+ * If the user has overridden one of these callbacks in their derived class, the corresponding
105+ * topic subscriber will be set up and routed to their implementation. If not, that topic will
106+ * not be subscribed to. The relevant callbacks include:
107+ *
108+ * - ahrs_callback
109+ * - battery_callback
110+ * - spring_callback
111+ * - power_callback
112+ * - trefoil_callback
113+ * - powerbuoy_callback
114+ *
115+ * @tparam ControllerImplCRTP The concrete controller class that inherits from this interface.
116+ * It must implement any callbacks or parameter-setting routines
117+ * it needs, marked as `final`.
118+ *
119+ * ## How to Use
120+ *
121+ * 1. Include the header for `Interface`:
122+ * @code{.cpp}
123+ * #include <buoy_api/interface.hpp>
124+ * @endcode
125+ *
126+ * 2. Forward-declare any policies or helper classes you will use:
127+ * @code{.cpp}
128+ * struct PBTorqueControlPolicy; // defined by user in torque_control_policy.hpp
129+ * @endcode
130+ *
131+ * 3. Define your controller class by inheriting from `buoy_api::Interface<YourClass>`
132+ * and adding `friend CRTP`:
133+ * @code{.cpp}
134+ * class PBTorqueController final : public buoy_api::Interface<PBTorqueController>
135+ * {
136+ * public:
137+ * explicit PBTorqueController(const std::string & node_name);
138+ * ~PBTorqueController() = default;
139+ *
140+ * private:
141+ * friend CRTP; // Enables base to access overrides.
142+ * void set_params() final;
143+ * void power_callback(const buoy_interfaces::msg::PCRecord & data);
144+ * std::unique_ptr<PBTorqueControlPolicy> policy_;
145+ * };
146+ * @endcode
147+ *
148+ * 4. Implement `set_params()` to declare or update ROS2 parameters to update your policy class.
149+ *
150+ * 5. Override any telemetry callbacks to process incoming data.
151+ *
152+ * 6. Construct your controller in your `main()` function, passing in the desired node name.
153+ * The base `Interface` handles common setup: parameters, services, publishers, and subscribers.
154+ *
155+ * ### Benefits of CRTP in This Context
156+ * - **Zero-cost abstraction**: No virtual table; callbacks are resolved at compile time.
157+ * - **Flexible overrides**: Only override what you use.
158+ * - **Simplified boilerplate**: Base class manages ROS2 setup.
159+ */
86160template <class ControllerImplCRTP >
87161class Interface : public rclcpp ::Node
88162{
@@ -94,6 +168,13 @@ class Interface : public rclcpp::Node
94168 {
95169 }
96170
171+ /* *
172+ * @brief Initialize the Interface node.
173+ *
174+ * @param node_name Name of the ROS2 node.
175+ * @param _wait_for_services If true and check_for_services, block until services are available.
176+ * @param _check_for_services If true, attempt to verify service availability before use.
177+ */
97178 explicit Interface (
98179 const std::string & node_name,
99180 const bool _wait_for_services,
@@ -320,7 +401,11 @@ class Interface : public rclcpp::Node
320401 }
321402 }
322403
323- // Setup node clock to use sim time from /clock
404+ /* *
405+ * @brief Enable/Disable using sim time in Node clock from /clock.
406+ *
407+ * @param enable True to use /clock, False to use system time.
408+ */
324409 void use_sim_time (bool enable = true )
325410 {
326411 this ->set_parameter (
@@ -329,7 +414,11 @@ class Interface : public rclcpp::Node
329414 enable));
330415 }
331416
332- // set publish rate of PC Microcontroller telemetry
417+ /* *
418+ * @brief Set publish rate of PC Microcontroller telemetry.
419+ *
420+ * @param rate_hz Desired publish rate in Hz.
421+ */
333422 void set_pc_pack_rate (const uint8_t & rate_hz = 50 )
334423 {
335424 auto request = std::make_shared<buoy_interfaces::srv::PCPackRateCommand::Request>();
@@ -342,7 +431,11 @@ class Interface : public rclcpp::Node
342431 auto response = pc_pack_rate_client_->async_send_request (request, pc_pack_rate_callback);
343432 }
344433
345- // set publish rate of SC Microcontroller telemetry
434+ /* *
435+ * @brief Set publish rate of SC Microcontroller telemetry.
436+ *
437+ * @param rate_hz Desired publish rate in Hz.
438+ */
346439 void set_sc_pack_rate (const uint8_t & rate_hz = 50 )
347440 {
348441 auto request = std::make_shared<buoy_interfaces::srv::SCPackRateCommand::Request>();
@@ -355,7 +448,11 @@ class Interface : public rclcpp::Node
355448 auto response = sc_pack_rate_client_->async_send_request (request, sc_pack_rate_callback);
356449 }
357450
358- // set publish rate of PC Microcontroller telemetry
451+ /* *
452+ * @brief Set publish rate of PC Microcontroller telemetry via parameter server.
453+ *
454+ * @param rate_hz Desired publish rate in Hz.
455+ */
359456 void set_pc_pack_rate_param (const double & rate_hz = 50.0 )
360457 {
361458 std::vector<rclcpp::Parameter> params = {rclcpp::Parameter{" publish_rate" ,
@@ -372,7 +469,11 @@ class Interface : public rclcpp::Node
372469 }
373470 }
374471
375- // set publish rate of SC Microcontroller telemetry
472+ /* *
473+ * @brief Set publish rate of SC Microcontroller telemetry via parameter server.
474+ *
475+ * @param rate_hz Desired publish rate in Hz.
476+ */
376477 void set_sc_pack_rate_param (const double & rate_hz = 50.0 )
377478 {
378479 std::vector<rclcpp::Parameter> params = {rclcpp::Parameter{" publish_rate" ,
@@ -443,6 +544,12 @@ class Interface : public rclcpp::Node
443544 using TFResetServiceResponseFuture =
444545 rclcpp::Client<buoy_interfaces::srv::TFResetCommand>::SharedFuture;
445546
547+ /* *
548+ * @brief Turn valve on for a duration to lower mean piston position.
549+ *
550+ * @param duration_sec Valve on duration in seconds.
551+ * @return A future containing the service response.
552+ */
446553 ValveServiceResponseFuture send_valve_command (const uint16_t & duration_sec)
447554 {
448555 auto request = std::make_shared<buoy_interfaces::srv::ValveCommand::Request>();
@@ -454,6 +561,12 @@ class Interface : public rclcpp::Node
454561 return valve_response_future;
455562 }
456563
564+ /* *
565+ * @brief Turn pump on for a duration to raise mean piston position.
566+ *
567+ * @param duration_mins Pump on duration in minutes.
568+ * @return A future containing the service response.
569+ */
457570 PumpServiceResponseFuture send_pump_command (const float & duration_mins)
458571 {
459572 auto request = std::make_shared<buoy_interfaces::srv::PumpCommand::Request>();
@@ -465,6 +578,12 @@ class Interface : public rclcpp::Node
465578 return pump_response_future;
466579 }
467580
581+ /* *
582+ * @brief Set winding current setpoint to control piston damping.
583+ *
584+ * @param wind_curr Wind current setpoint in Amps.
585+ * @return A future containing the service response.
586+ */
468587 PCWindCurrServiceResponseFuture send_pc_wind_curr_command (const float & wind_curr)
469588 {
470589 auto request = std::make_shared<buoy_interfaces::srv::PCWindCurrCommand::Request>();
@@ -476,6 +595,14 @@ class Interface : public rclcpp::Node
476595 return pc_wind_curr_response_future;
477596 }
478597
598+ /* *
599+ * @brief Set bias current setpoint to control piston damping offset.
600+ *
601+ * A high bias in either direction will move the piston back and forth.
602+ *
603+ * @param bias_curr Bias current setpoint in Amps.
604+ * @return A future containing the service response.
605+ */
479606 PCBiasCurrServiceResponseFuture send_pc_bias_curr_command (const float & bias_curr)
480607 {
481608 auto request = std::make_shared<buoy_interfaces::srv::PCBiasCurrCommand::Request>();
@@ -487,6 +614,12 @@ class Interface : public rclcpp::Node
487614 return pc_bias_curr_response_future;
488615 }
489616
617+ /* *
618+ * @brief Set damping gain.
619+ *
620+ * @param scale Damping gain.
621+ * @return A future containing the service response.
622+ */
490623 PCScaleServiceResponseFuture send_pc_scale_command (const float & scale)
491624 {
492625 auto request = std::make_shared<buoy_interfaces::srv::PCScaleCommand::Request>();
@@ -498,6 +631,12 @@ class Interface : public rclcpp::Node
498631 return pc_scale_response_future;
499632 }
500633
634+ /* *
635+ * @brief Set additional damping gain in the piston retract direction.
636+ *
637+ * @param retract Additional damping gain for retraction.
638+ * @return A future containing the service response.
639+ */
501640 PCRetractServiceResponseFuture send_pc_retract_command (const float & retract)
502641 {
503642 auto request = std::make_shared<buoy_interfaces::srv::PCRetractCommand::Request>();
@@ -513,12 +652,52 @@ class Interface : public rclcpp::Node
513652 virtual ~Interface () = default ;
514653
515654 // set_params and callbacks optionally defined by user
655+
656+ /* *
657+ * @brief Set user-defined Node parameters (e.g., custom controller gains).
658+ */
516659 virtual void set_params () {}
660+
661+ /* *
662+ * @brief Override this function to subscribe to /ahrs_data to receive XBRecord telemetry.
663+ *
664+ * @param data Incoming XBRecord.
665+ */
517666 void ahrs_callback (const buoy_interfaces::msg::XBRecord &) {}
667+
668+ /* *
669+ * @brief Override this function to subscribe to /battery_data to receive BCRecord telemetry.
670+ *
671+ * @param data Incoming BCRecord.
672+ */
518673 void battery_callback (const buoy_interfaces::msg::BCRecord &) {}
674+
675+ /* *
676+ * @brief Override this function to subscribe to /spring_data to receive SCRecord telemetry.
677+ *
678+ * @param data Incoming SCRecord.
679+ */
519680 void spring_callback (const buoy_interfaces::msg::SCRecord &) {}
681+
682+ /* *
683+ * @brief Override this function to subscribe to /power_data to receive PCRecord telemetry.
684+ *
685+ * @param data Incoming PCRecord.
686+ */
520687 void power_callback (const buoy_interfaces::msg::PCRecord &) {}
688+
689+ /* *
690+ * @brief Override this function to subscribe to /trefoil_data to receive TFRecord telemetry.
691+ *
692+ * @param data Incoming TFRecord.
693+ */
521694 void trefoil_callback (const buoy_interfaces::msg::TFRecord &) {}
695+
696+ /* *
697+ * @brief Override this function to subscribe to /powerbuoy_data to receive PBRecord telemetry.
698+ *
699+ * @param data Incoming PBRecord containing a slice of all microcontroller telemetry data.
700+ */
522701 void powerbuoy_callback (const buoy_interfaces::msg::PBRecord &) {}
523702
524703 // abbrv callback types
0 commit comments