Committing clang-format changes

motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp

@@ -17,15 +17,15 @@ class Allocator : public rclcpp::Node {
   void timer_callback();
 
 private:
-// Hardcoded thruster config matrix for T_pinv
-// clang-format off
+  // Hardcoded thruster config matrix for T_pinv
+  // clang-format off
   Eigen::MatrixXd thrust_configuration =
       (Eigen::MatrixXd(3, 4) << 
       0.70711, 0.70711, 0.70711, 0.70711, 
       -0.70711, 0.70711, -0.70711, 0.70711, 
       0.27738, 0.27738, -0.27738, -0.27738)
           .finished();
-// clang-format on
+  // clang-format on
 
   void wrench_callback(const geometry_msgs::msg::Wrench &msg);
   bool healthyWrench(const Eigen::VectorXd &v) const;

motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp

@@ -10,14 +10,15 @@
 
 /**
  * @file allocator_utils.hpp
- * @brief This file contains utility functions for the thruster allocator module.
+ * @brief This file contains utility functions for the thruster allocator
+ * module.
  */
 
 // Return true if M has any NaN or INF elements.
 
 /**
  * @brief Check if the matrix has any NaN or INF elements.
- * 
+ *
  * @tparam Derived The type of the matrix.
  * @param M The matrix to check.
  * @return true if the matrix has any NaN or INF elements, false otherwise.
@@ -31,25 +32,27 @@ inline bool isInvalidMatrix(const Eigen::MatrixBase<Derived> &M) {
 
 /**
  * @brief Returns a string stream containing the matrix with the given name.
- * 
+ *
  * @param name The name of the matrix.
  * @param M The matrix to print.
  * @return std::stringstream The string stream containing the matrix.
  */
-inline std::stringstream printMatrix(std::string name, const Eigen::MatrixXd &M) {
+inline std::stringstream printMatrix(std::string name,
+                                     const Eigen::MatrixXd &M) {
   std::stringstream ss;
   ss << std::endl << name << " = " << std::endl << M;
   return ss;
 }
 
 /**
  * @brief Calculates the right pseudoinverse of the given matrix.
- * 
+ *
  * @param M The matrix to calculate the pseudoinverse of.
  * @param M_pinv The resulting pseudoinverse matrix.
  * @throws char* if the pseudoinverse is invalid.
  */
-inline void calculateRightPseudoinverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_pinv) {
+inline void calculateRightPseudoinverse(const Eigen::MatrixXd &M,
+                                        Eigen::MatrixXd &M_pinv) {
   Eigen::MatrixXd pseudoinverse = M.transpose() * (M * M.transpose()).inverse();
   // pseudoinverse.completeOrthogonalDecomposition().pseudoInverse();
   if (isInvalidMatrix(pseudoinverse)) {
@@ -76,12 +79,15 @@ inline bool saturateVectorValues(Eigen::VectorXd &vec, double min, double max) {
 
 // Copies vector elements into ThrusterForces message
 /**
- * @brief Converts an Eigen VectorXd to a vortex_msgs::msg::ThrusterForces message.
- * 
+ * @brief Converts an Eigen VectorXd to a vortex_msgs::msg::ThrusterForces
+ * message.
+ *
  * @param u The Eigen VectorXd to be converted.
- * @param msg The vortex_msgs::msg::ThrusterForces message to store the converted values.
+ * @param msg The vortex_msgs::msg::ThrusterForces message to store the
+ * converted values.
  */
-inline void arrayEigenToMsg(const Eigen::VectorXd &u, vortex_msgs::msg::ThrusterForces &msg) {
+inline void arrayEigenToMsg(const Eigen::VectorXd &u,
+                            vortex_msgs::msg::ThrusterForces &msg) {
   int r = u.size();
   std::vector<double> u_vec(r);
   for (int i = 0; i < r; ++i)

motion/thruster_allocator/src/allocator_ros.cpp

@@ -1,6 +1,8 @@
 /**
  * @file allocator_ros.cpp
- * @brief This file contains the implementation of the Allocator class, which allocates thrust to the ASV's thrusters based on the desired body frame forces.
+ * @brief This file contains the implementation of the Allocator class, which
+ * allocates thrust to the ASV's thrusters based on the desired body frame
+ * forces.
  */
 
 #include <thruster_allocator/allocator_ros.hpp>
@@ -13,22 +15,22 @@
 
 using namespace std::chrono_literals;
 
-Allocator::Allocator() : 
-  Node("thruster_allocator_node"),  
-  pseudoinverse_allocator_(Eigen::MatrixXd::Zero(3,4))
-{
+Allocator::Allocator()
+    : Node("thruster_allocator_node"),
+      pseudoinverse_allocator_(Eigen::MatrixXd::Zero(3, 4)) {
   subscription_ = this->create_subscription<geometry_msgs::msg::Wrench>(
       "thrust/wrench_input", 1,
       std::bind(&Allocator::wrench_callback, this, std::placeholders::_1));
 
   publisher_ = this->create_publisher<vortex_msgs::msg::ThrusterForces>(
       "thrust/thruster_forces", 1);
 
-  timer_ = this->create_wall_timer(
-      100ms, std::bind(&Allocator::timer_callback, this));
+  timer_ = this->create_wall_timer(100ms,
+                                   std::bind(&Allocator::timer_callback, this));
 
   Eigen::MatrixXd thrust_configuration_pseudoinverse;
-  calculateRightPseudoinverse(thrust_configuration, thrust_configuration_pseudoinverse);
+  calculateRightPseudoinverse(thrust_configuration,
+                              thrust_configuration_pseudoinverse);
 
   // Set T_pinv in pseudoinverse_allocator_
   pseudoinverse_allocator_.T_pinv = thrust_configuration_pseudoinverse;
@@ -37,10 +39,12 @@ Allocator::Allocator() :
 }
 
 /**
- * @brief This function is called by the timer and 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".
- * If the calculated thruster forces vector is invalid, it logs an error message.
- * If the thruster forces vector required saturation, it logs a warning message.
+ * @brief This function is called by the timer and 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". If the calculated thruster forces vector is
+ * invalid, it logs an error message. If the thruster forces vector required
+ * saturation, it logs a warning message.
  */
 
 void Allocator::timer_callback() {
@@ -67,14 +71,17 @@ void Allocator::timer_callback() {
 }
 
 /**
- * @brief Callback function for the wrench input subscription. It extracts the surge, sway and yaw values from the received geometry_msgs::msg::Wrench message and stores them in the body_frame_forces_ Eigen vector.
- * If the wrench vector is invalid (contains NaN or Inf values), it logs an error message and returns.
+ * @brief Callback function for the wrench input subscription. It extracts the
+ * surge, sway and yaw values from the received geometry_msgs::msg::Wrench
+ * message and stores them in the body_frame_forces_ Eigen vector. If the wrench
+ * vector is invalid (contains NaN or Inf values), it logs an error message and
+ * returns.
  * @param msg The received geometry_msgs::msg::Wrench message.
  */
 void Allocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) {
-  body_frame_forces_(0) = msg.force.x;   // surge
-  body_frame_forces_(1) = msg.force.y;   // sway
-  body_frame_forces_(2) = msg.torque.z;  // yaw
+  body_frame_forces_(0) = msg.force.x;  // surge
+  body_frame_forces_(1) = msg.force.y;  // sway
+  body_frame_forces_(2) = msg.torque.z; // yaw
   if (!healthyWrench(body_frame_forces_)) {
     RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring.");
     return;
@@ -83,7 +90,8 @@ void Allocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) {
 
 /**
  * @brief Checks if the given Eigen vector is a healthy wrench vector.
- * A healthy wrench vector is one that does not contain NaN or Inf values, and has reasonable values.
+ * A healthy wrench vector is one that does not contain NaN or Inf values, and
+ * has reasonable values.
  * @param v The Eigen vector to check.
  * @return True if the vector is healthy, false otherwise.
  */

motion/thruster_allocator/src/pseudoinverse_allocator.cpp

@@ -2,15 +2,16 @@
 
 /**
  * @brief Constructor for the PseudoinverseAllocator class.
- * 
+ *
  * @param T_pinv The pseudoinverse of the thruster configuration matrix.
  */
 PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv)
     : T_pinv(T_pinv) {}
 
 /**
- * @brief Calculates the allocated thrust given the input torques using the pseudoinverse allocator.
- * 
+ * @brief Calculates the allocated thrust given the input torques using the
+ * pseudoinverse allocator.
+ *
  * @param tau The input torques as a vector.
  * @return The allocated thrust as a vector.
  */