From 037765984f09cf7457e2f702006168fd49b646b9 Mon Sep 17 00:00:00 2001 From: Aldokan Date: Mon, 8 Apr 2024 20:29:50 +0200 Subject: [PATCH] fixed saturate_vector_values bug --- .../include/thruster_allocator/allocator_utils.hpp | 2 +- motion/thruster_allocator/src/allocator_ros.cpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp index 0e69bcff..0da09bed 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp +++ b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp @@ -68,7 +68,7 @@ inline Eigen::MatrixXd calculate_right_pseudoinverse(const Eigen::MatrixXd &T) { * @return True if all vector values are within the given range, false * otherwise. */ -inline bool saturate_vector_values(Eigen::Vector3d &vec, double min, +inline bool saturate_vector_values(Eigen::VectorXd &vec, double min, double max) { bool all_values_in_range = std::all_of(vec.begin(), vec.end(), diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp index 21e777f4..012ccb47 100644 --- a/motion/thruster_allocator/src/allocator_ros.cpp +++ b/motion/thruster_allocator/src/allocator_ros.cpp @@ -33,9 +33,8 @@ ThrusterAllocator::ThrusterAllocator() std::bind(&ThrusterAllocator::wrench_callback, this, std::placeholders::_1)); - thruster_forces_publisher_ = - this->create_publisher( - "thrust/thruster_forces", 1); + thruster_forces_publisher_ = this->create_publisher( + "thrust/thruster_forces", 1); calculate_thrust_timer_ = this->create_wall_timer( 100ms, std::bind(&ThrusterAllocator::calculate_thrust_timer_cb, this));