From 8809896516aa380fa39d20fa74ce126d27ac1fe9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anders=20H=C3=B8gden?= <andeshog@stud.ntnu.no>
Date: Sun, 5 Jan 2025 22:06:06 +0100
Subject: [PATCH] refactor: change thrust output type to Float64MutliArray

---
 .../include/thruster_allocator/allocator_ros.hpp            | 4 ++--
 .../include/thruster_allocator/allocator_utils.hpp          | 6 ++----
 motion/thruster_allocator/src/allocator_ros.cpp             | 6 +++---
 3 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp
index 028ce5e5..fe275e7c 100644
--- a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp
+++ b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp
@@ -14,7 +14,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/wrench.hpp>
-#include <std_msgs/msg/float32_multi_array.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
 
 using namespace std::chrono_literals;
 
@@ -39,7 +39,7 @@ class ThrusterAllocator : public rclcpp::Node {
    * @param msg The received geometry_msgs::msg::Wrench message.
    */
   void wrench_callback(const geometry_msgs::msg::Wrench &msg);
-  rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr
+  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr
       thruster_forces_publisher_;
   rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr
       wrench_subscriber_;
diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp
index 0da09bed..1872bdcc 100644
--- a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp
+++ b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp
@@ -11,8 +11,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 #include <vector>
-
-#include <std_msgs/msg/float32_multi_array.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
 
 /**
  * @brief Check if the matrix has any NaN or INF elements.
@@ -89,10 +88,9 @@ inline bool saturate_vector_values(Eigen::VectorXd &vec, double min,
  * @param msg The std_msgs::msg::Float32MultiArray message to store the
  * converted values.
  */
-#include <std_msgs/msg/float32_multi_array.hpp>
 
 inline void array_eigen_to_msg(const Eigen::VectorXd &u,
-                               std_msgs::msg::Float32MultiArray &msg) {
+                               std_msgs::msg::Float64MultiArray &msg) {
   msg.data.assign(u.data(), u.data() + u.size());
 }
 
diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp
index 6eb4b9d9..c51069c1 100644
--- a/motion/thruster_allocator/src/allocator_ros.cpp
+++ b/motion/thruster_allocator/src/allocator_ros.cpp
@@ -1,7 +1,7 @@
 #include "thruster_allocator/allocator_ros.hpp"
 #include "thruster_allocator/allocator_utils.hpp"
 #include "thruster_allocator/pseudoinverse_allocator.hpp"
-#include <std_msgs/msg/float32_multi_array.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
 
 #include <chrono>
 #include <functional>
@@ -34,7 +34,7 @@ ThrusterAllocator::ThrusterAllocator()
                 std::placeholders::_1));
 
   thruster_forces_publisher_ =
-      this->create_publisher<std_msgs::msg::Float32MultiArray>(
+      this->create_publisher<std_msgs::msg::Float64MultiArray>(
           "freya/thrust/thruster_forces", 1);
 
   calculate_thrust_timer_ = this->create_wall_timer(
@@ -59,7 +59,7 @@ void ThrusterAllocator::calculate_thrust_timer_cb() {
     RCLCPP_WARN(get_logger(), "Thruster forces vector required saturation.");
   }
 
-  std_msgs::msg::Float32MultiArray msg_out;
+  std_msgs::msg::Float64MultiArray msg_out;
   array_eigen_to_msg(thruster_forces, msg_out);
   thruster_forces_publisher_->publish(msg_out);
 }