Cleanup, add launch file, saturate output thrust

vortexntnu · Oct 14, 2023 · e8d245f · e8d245f
2 parents 8c752ae + 03654d0
commit e8d245f
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 9 deletions.
diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp
@@ -17,12 +17,10 @@ class Allocator : public rclcpp::Node {
 
 private:
   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(); 
-          // Hardcoded thruster config matrix for T_pinv
+      (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();
+  // Hardcoded thruster config matrix for T_pinv
 
   void wrench_callback(const geometry_msgs::msg::Wrench &msg);
   bool healthyWrench(const Eigen::VectorXd &v) const;

diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp
@@ -78,15 +78,15 @@ Eigen::VectorXd Allocator::wrenchMsgToEigen(const float force_x,
   return body_frame_forces;
 }
 
- bool Allocator::healthyWrench(const Eigen::VectorXd &v) const {
+bool Allocator::healthyWrench(const Eigen::VectorXd &v) const {
   // Check for NaN/Inf
   if (isInvalidMatrix(v))
     return false;
 
   // Check reasonableness
   for (unsigned i = 0; i < v.size(); ++i)
-    if (std::abs(v[i]) > 100) //c_force_range_limit
+    if (std::abs(v[i]) > 100) // c_force_range_limit
       return false;
 
   return true;
- }
+ }