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 #include -#include +#include 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::SharedPtr + rclcpp::Publisher::SharedPtr thruster_forces_publisher_; rclcpp::Subscription::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 #include #include - -#include +#include /** * @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 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 +#include #include #include @@ -34,7 +34,7 @@ ThrusterAllocator::ThrusterAllocator() std::placeholders::_1)); thruster_forces_publisher_ = - this->create_publisher( + this->create_publisher( "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); }