Merge branch 'thrust_allocation_ROS2' of github.com:vortexntnu/vortex…

…-asv into thrust_allocation_ROS2

motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp

@@ -1,8 +1,8 @@
 #ifndef VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP
 #define VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP
 
-#include <rclcpp/rclcpp.hpp>
 #include <eigen3/Eigen/Eigen>
+#include <rclcpp/rclcpp.hpp>
 #include <thruster_allocator/allocator_utils.hpp>
 #include <thruster_allocator/pseudoinverse_allocator.hpp>
 
@@ -22,11 +22,11 @@ class ThrusterAllocator : public rclcpp::Node {
   explicit ThrusterAllocator();
 
   /**
- * @brief Calculates the allocated
- * thrust based on the body frame forces. It then saturates the output vector
- * between min and max values and publishes the thruster forces to the topic
- * "thrust/thruster_forces".
- */
+   * @brief Calculates the allocated
+   * thrust based on the body frame forces. It then saturates the output vector
+   * between min and max values and publishes the thruster forces to the topic
+   * "thrust/thruster_forces".
+   */
   void timer_callback();
 
 private:
@@ -41,18 +41,18 @@ class ThrusterAllocator : public rclcpp::Node {
   // clang-format on
 
   /**
- * @brief Callback function for the wrench input subscription. Extracts the
- * surge, sway and yaw values from the received wrench msg
- * and stores them in the body_frame_forces_ Eigen vector.
- * @param msg The received geometry_msgs::msg::Wrench message.
- */
+   * @brief Callback function for the wrench input subscription. Extracts the
+   * surge, sway and yaw values from the received wrench msg
+   * and stores them in the body_frame_forces_ Eigen vector.
+   * @param msg The received geometry_msgs::msg::Wrench message.
+   */
   void wrench_callback(const geometry_msgs::msg::Wrench &msg);
 
   /**
- * @brief Checks if the given Eigen vector contains any NaN or Inf values
- * @param v The Eigen vector to check.
- * @return True if the vector is healthy, false otherwise.
- */
+   * @brief Checks if the given Eigen vector contains any NaN or Inf values
+   * @param v The Eigen vector to check.
+   * @return True if the vector is healthy, false otherwise.
+   */
   bool healthyWrench(const Eigen::VectorXd &v) const;
   rclcpp::Publisher<vortex_msgs::msg::ThrusterForces>::SharedPtr publisher_;
   rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr subscription_;

motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp

@@ -60,12 +60,14 @@ inline void calculateRightPseudoinverse(const Eigen::MatrixXd &M,
 }
 
 /**
- * @brief Saturates the values of a given Eigen vector between a minimum and maximum value.
- * 
+ * @brief Saturates the values of a given Eigen vector between a minimum and
+ * maximum value.
+ *
  * @param vec The Eigen vector to be saturated.
  * @param min The minimum value to saturate the vector values to.
  * @param max The maximum value to saturate the vector values to.
- * @return True if all vector values are within the given range, false otherwise.
+ * @return True if all vector values are within the given range, false
+ * otherwise.
  */
 inline bool saturateVectorValues(Eigen::VectorXd &vec, double min, double max) {
   bool vector_in_range = true;