From 0b9d2462a06ad4f1eb03f9fb8d18067fba6622a0 Mon Sep 17 00:00:00 2001 From: Clang Robot Date: Sun, 15 Oct 2023 16:23:21 +0000 Subject: [PATCH] Committing clang-format changes --- .../thruster_allocator/allocator_ros.hpp | 6 +-- .../thruster_allocator/allocator_utils.hpp | 26 ++++++----- .../thruster_allocator/src/allocator_ros.cpp | 44 +++++++++++-------- .../src/pseudoinverse_allocator.cpp | 7 +-- 4 files changed, 49 insertions(+), 34 deletions(-) diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp index 8913a5fa..64a7d969 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp +++ b/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; diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp index 93eae2a2..9fad061b 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp +++ b/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,12 +32,13 @@ inline bool isInvalidMatrix(const Eigen::MatrixBase &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; @@ -44,12 +46,13 @@ inline std::stringstream printMatrix(std::string name, const Eigen::MatrixXd &M) /** * @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 u_vec(r); for (int i = 0; i < r; ++i) diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp index 2da11f27..3fd10a4d 100644 --- a/motion/thruster_allocator/src/allocator_ros.cpp +++ b/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 @@ -13,10 +15,9 @@ 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( "thrust/wrench_input", 1, std::bind(&Allocator::wrench_callback, this, std::placeholders::_1)); @@ -24,11 +25,12 @@ Allocator::Allocator() : publisher_ = this->create_publisher( "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. */ diff --git a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp index 75160c55..1e16a726 100644 --- a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp +++ b/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. */