Saturate wrench msg and cleanup

motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp

@@ -28,7 +28,7 @@ class ThrusterAllocator : public rclcpp::Node {
    * between min and max values and publishes the thruster forces to the topic
    * "thrust/thruster_forces".
    */
-  void timer_callback();
+  void calculate_thrust_timer_cb();
 
 private:
   Eigen::MatrixXd thrust_configuration;
@@ -45,12 +45,12 @@ class ThrusterAllocator : public rclcpp::Node {
    * @param v The Eigen vector to check.
    * @return True if the vector is healthy, false otherwise.
    */
-  bool healthyWrench(const Eigen::VectorXd &v) const;
+  bool healthy_wrench(const Eigen::VectorXd &v) const;
   rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr
-      thrust_publisher_;
+      thruster_forces_publisher_;
   rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr
       wrench_subscriber_;
-  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::TimerBase::SharedPtr calculate_thrust_timer_;
   size_t count_;
   int num_dof_;
   int num_thrusters_;

motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp

@@ -22,7 +22,7 @@
  * @return true if the matrix has any NaN or INF elements, false otherwise.
  */
 template <typename Derived>
-inline bool isInvalidMatrix(const Eigen::MatrixBase<Derived> &M) {
+inline bool is_invalid_matrix(const Eigen::MatrixBase<Derived> &M) {
   bool has_nan = !(M.array() == M.array()).all();
   bool has_inf = M.array().isInf().any();
   return has_nan || has_inf;
@@ -35,7 +35,7 @@ inline bool isInvalidMatrix(const Eigen::MatrixBase<Derived> &M) {
  * @param M The matrix to print.
  * @return std::stringstream The string stream containing the matrix.
  */
-inline std::stringstream printMatrix(std::string name,
+inline std::stringstream print_matrix(std::string name,
                                      const Eigen::MatrixXd &M) {
   std::stringstream ss;
   ss << std::endl << name << " = " << std::endl << M;
@@ -49,10 +49,10 @@ inline std::stringstream printMatrix(std::string name,
  * @throws char* if the pseudoinverse is invalid.
  * @return The pseudoinverse of the given matrix.
  */
-inline Eigen::MatrixXd calculateRightPseudoinverse(const Eigen::MatrixXd &T) {
+inline Eigen::MatrixXd calculate_right_pseudoinverse(const Eigen::MatrixXd &T) {
   Eigen::MatrixXd pseudoinverse = T.transpose() * (T * T.transpose()).inverse();
   // pseudoinverse.completeOrthogonalDecomposition().pseudoInverse();
-  if (isInvalidMatrix(pseudoinverse)) {
+  if (is_invalid_matrix(pseudoinverse)) {
     throw std::runtime_error("Invalid Psuedoinverse Calculated");
   }
   return pseudoinverse;
@@ -68,7 +68,7 @@ inline Eigen::MatrixXd calculateRightPseudoinverse(const Eigen::MatrixXd &T) {
  * @return True if all vector values are within the given range, false
  * otherwise.
  */
-inline bool saturateVectorValues(Eigen::VectorXd &vec, double min, double max) {
+inline bool saturate_vector_values(Eigen::Vector3d &vec, double min, double max) {
   bool all_values_in_range =
       std::all_of(vec.begin(), vec.end(),
                   [min, max](double val) { return val >= min && val <= max; });
@@ -90,7 +90,7 @@ inline bool saturateVectorValues(Eigen::VectorXd &vec, double min, double max) {
  */
 #include <std_msgs/msg/float32_multi_array.hpp>
 
-inline void arrayEigenToMsg(const Eigen::VectorXd &u,
+inline void array_eigen_to_msg(const Eigen::VectorXd &u,
                             std_msgs::msg::Float32MultiArray &msg) {
   msg.data.assign(u.data(), u.data() + u.size());
 }
@@ -104,7 +104,7 @@ inline void arrayEigenToMsg(const Eigen::VectorXd &u,
  * @return The resulting Eigen matrix.
  */
 inline Eigen::MatrixXd
-doubleArrayToEigenMatrix(const std::vector<double> &matrix, int rows,
+double_array_to_eigen_matrix(const std::vector<double> &matrix, int rows,
                          int cols) {
   return Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
                                         Eigen::RowMajor>>(matrix.data(), rows,

motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp

@@ -30,7 +30,7 @@ class PseudoinverseAllocator {
    * @param tau The input torques as a vector.
    * @return The allocated thrust as a vector.
    */
-  Eigen::VectorXd calculateAllocatedThrust(const Eigen::VectorXd &tau);
+  Eigen::VectorXd calculate_allocated_thrust(const Eigen::VectorXd &tau);
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   Eigen::MatrixXd T_pinv;

motion/thruster_allocator/src/allocator_ros.cpp

@@ -15,18 +15,15 @@ ThrusterAllocator::ThrusterAllocator()
   declare_parameter("propulsion.thrusters.num", 4);
   declare_parameter("propulsion.thrusters.min", -100);
   declare_parameter("propulsion.thrusters.max", 100);
-  declare_parameter("propulsion.thrusters.direction", std::vector<int64_t>{0});
   declare_parameter("propulsion.thrusters.configuration_matrix",
                     std::vector<double>{0});
 
   num_dof_ = get_parameter("propulsion.dofs.num").as_int();
   num_thrusters_ = get_parameter("propulsion.thrusters.num").as_int();
   min_thrust_ = get_parameter("propulsion.thrusters.min").as_int();
   max_thrust_ = get_parameter("propulsion.thrusters.max").as_int();
-  direction_ =
-      get_parameter("propulsion.thrusters.direction").as_integer_array();
 
-  thrust_configuration = doubleArrayToEigenMatrix(
+  thrust_configuration = double_array_to_eigen_matrix(
       get_parameter("propulsion.thrusters.configuration_matrix")
           .as_double_array(),
       num_dof_, num_thrusters_);
@@ -36,55 +33,49 @@ ThrusterAllocator::ThrusterAllocator()
       std::bind(&ThrusterAllocator::wrench_callback, this,
                 std::placeholders::_1));
 
-  thrust_publisher_ = this->create_publisher<std_msgs::msg::Float32MultiArray>(
+  thruster_forces_publisher_ = this->create_publisher<std_msgs::msg::Float32MultiArray>(
       "thrust/thruster_forces", 1);
 
-  timer_ = this->create_wall_timer(
-      100ms, std::bind(&ThrusterAllocator::timer_callback, this));
+  calculate_thrust_timer_ = this->create_wall_timer(
+      100ms, std::bind(&ThrusterAllocator::calculate_thrust_timer_cb, this));
 
   pseudoinverse_allocator_.T_pinv =
-      calculateRightPseudoinverse(thrust_configuration);
+      calculate_right_pseudoinverse(thrust_configuration);
 
   body_frame_forces_.setZero();
 }
 
-void ThrusterAllocator::timer_callback() {
-  Eigen::VectorXd thruster_forces =
-      pseudoinverse_allocator_.calculateAllocatedThrust(body_frame_forces_);
+void ThrusterAllocator::calculate_thrust_timer_cb() {
+  Eigen::Vector3d thruster_forces =
+      pseudoinverse_allocator_.calculate_allocated_thrust(body_frame_forces_);
 
-  if (isInvalidMatrix(thruster_forces)) {
-    RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid");
+  if (is_invalid_matrix(thruster_forces)) {
+    RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid, ignoring");
     return;
   }
 
-  if (!saturateVectorValues(thruster_forces, min_thrust_, max_thrust_)) {
+  if (!saturate_vector_values(thruster_forces, min_thrust_, max_thrust_)) {
     RCLCPP_WARN(get_logger(), "Thruster forces vector required saturation.");
   }
 
   std_msgs::msg::Float32MultiArray msg_out;
-  arrayEigenToMsg(thruster_forces, msg_out);
-  thrust_publisher_->publish(msg_out);
+  array_eigen_to_msg(thruster_forces, msg_out);
+  thruster_forces_publisher_->publish(msg_out);
 }
 
 void ThrusterAllocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) {
   Eigen::Vector3d msg_vector;
   msg_vector(0) = msg.force.x;  // surge
   msg_vector(1) = msg.force.y;  // sway
   msg_vector(2) = msg.torque.z; // yaw
-  if (!healthyWrench(msg_vector)) {
+  if (is_invalid_matrix(msg_vector)) {
     RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring.");
     return;
   }
-  std::swap(msg_vector, body_frame_forces_);
-}
-
-bool ThrusterAllocator::healthyWrench(const Eigen::VectorXd &v) const {
-  if (isInvalidMatrix(v))
-    return false;
 
-  bool within_max_thrust = std::none_of(v.begin(), v.end(), [this](double val) {
-    return std::abs(val) > max_thrust_;
-  });
+  if (!saturate_vector_values(msg_vector, min_thrust_, max_thrust_)) {
+    RCLCPP_WARN(get_logger(), "ASV wrench vector required saturation.");
+  }
 
-  return within_max_thrust;
+  std::swap(msg_vector, body_frame_forces_);
 }

motion/thruster_allocator/src/pseudoinverse_allocator.cpp

@@ -4,7 +4,7 @@ PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv)
     : T_pinv(T_pinv) {}
 
 Eigen::VectorXd
-PseudoinverseAllocator::calculateAllocatedThrust(const Eigen::VectorXd &tau) {
+PseudoinverseAllocator::calculate_allocated_thrust(const Eigen::VectorXd &tau) {
   Eigen::VectorXd u = T_pinv * tau;
   return u;
 }