diff --git a/src/navigator/gnc/navigator_thrust_mapper/CMakeLists.txt b/src/navigator/gnc/navigator_thrust_mapper/CMakeLists.txt new file mode 100644 index 00000000..adfbadfd --- /dev/null +++ b/src/navigator/gnc/navigator_thrust_mapper/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.8) +project(navigator_thrust_mapper) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories(include) + +# Build thruster_map library +add_library(thruster_map src/thruster_map.cpp) +target_link_libraries(thruster_map PUBLIC Eigen3::Eigen) + +# Build thruster_mapper_node executable +add_executable(thruster_mapper_node src/thruster_mapper_node.cpp) +target_link_libraries(thruster_mapper_node thruster_map Eigen3::Eigen) +ament_target_dependencies(thruster_mapper_node rclcpp geometry_msgs sensor_msgs + std_msgs) + +# Build test executable +add_executable(thruster_map_test src/thruster_map_test.cpp) +target_link_libraries(thruster_map_test thruster_map Eigen3::Eigen) + +install(TARGETS thruster_mapper_node DESTINATION lib/${PROJECT_NAME}) +install(TARGETS thruster_map_test DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/src/navigator/gnc/navigator_thrust_mapper/include/navigator_thrust_mapper/thruster_map.h b/src/navigator/gnc/navigator_thrust_mapper/include/navigator_thrust_mapper/thruster_map.h new file mode 100644 index 00000000..68112183 --- /dev/null +++ b/src/navigator/gnc/navigator_thrust_mapper/include/navigator_thrust_mapper/thruster_map.h @@ -0,0 +1,99 @@ +// Copyright 2025 University of Florida Machine Intelligence Laboratory +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef NAVIGATOR_THRUST_MAPPER_THRUSTER_MAP_H +#define NAVIGATOR_THRUST_MAPPER_THRUSTER_MAP_H + +#include +#include +#include +#include +#include + +#include +#include + +namespace navigator_thrust_mapper +{ + +/// Convert force to command scalar for VRX simulation +/// Implements the inverse thrust dynamics model for VRX +double vrx_force_to_command_scalar(double force); + +/// Vectorized version of vrx_force_to_command_scalar +Eigen::VectorXd vrx_force_to_command(Eigen::VectorXd const &forces); + +/// Generate a linear force-to-command conversion function with given ratio +std::function generate_linear_force_to_command(double ratio); + +/// Helper class to map between body forces/torques and thruster outputs +/// Implements least-squares thrust allocation as described in: +/// Christiaan De With "Optimal Thrust Allocation Methods for Dynamic Positioning of Ships" +class ThrusterMap +{ + public: + /// Default constructor (creates empty/invalid ThrusterMap) + ThrusterMap() = default; + + /// Constructor for ThrusterMap + /// @param names List of thruster names + /// @param positions List of (x, y) positions for each thruster in meters + /// @param angles List of angles for each thruster in radians + /// @param force_to_command Function to convert forces to command units + /// @param force_limit Tuple (MAX_FORWARD, MAX_REVERSE) maximum force in newtons + /// @param com Center of mass offset from base_link, defaults to (0, 0) + /// @param joints Joint names corresponding to each thruster + ThrusterMap(std::vector const &names, std::vector> const &positions, + std::vector const &angles, + std::function const &force_to_command, + std::array const &force_limit, std::array const &com = { 0.0, 0.0 }, + std::vector const &joints = {}); + + /// Create a ThrusterMap from a URDF string + /// Expects each thruster to be connected to a transmission ending in "_thruster_transmission" + /// @param urdf_string URDF XML as a string + /// @param transmission_suffix Suffix to identify thruster transmissions + static ThrusterMap from_urdf(std::string const &urdf_string, std::string const &transmission_suffix = "_thruster_" + "transmissio" + "n"); + + /// Create a ThrusterMap for VRX-style naming and force conversions + /// @param urdf_string URDF XML as a string + static ThrusterMap from_vrx_urdf(std::string const &urdf_string); + + /// Convert body wrench to individual thruster thrusts using least-squares allocation + /// @param wrench Array of [surge, sway, yaw] forces/torques in Newtons/N*m + /// @return Vector of thruster efforts in command units + std::vector wrench_to_thrusts(std::array const &wrench) const; + + // Public members for quick access + std::vector names; + std::vector joints; + + private: + std::function force_to_command_; + std::array force_limit_; + Eigen::MatrixXd thruster_matrix_; + Eigen::MatrixXd thruster_matrix_inv_; +}; + +} // namespace navigator_thrust_mapper + +#endif // NAVIGATOR_THRUST_MAPPER_THRUSTER_MAP_H diff --git a/src/navigator/gnc/navigator_thrust_mapper/package.xml b/src/navigator/gnc/navigator_thrust_mapper/package.xml new file mode 100644 index 00000000..819689fb --- /dev/null +++ b/src/navigator/gnc/navigator_thrust_mapper/package.xml @@ -0,0 +1,14 @@ + + + navigator_thrust_mapper + 0.0.0 + Thruster mapping utilities and node for Navigator + MIL + MIT + + ament_cmake + geometry_msgs + rclcpp + sensor_msgs + std_msgs + diff --git a/src/navigator/gnc/navigator_thrust_mapper/src/thruster_map.cpp b/src/navigator/gnc/navigator_thrust_mapper/src/thruster_map.cpp new file mode 100644 index 00000000..ed96b327 --- /dev/null +++ b/src/navigator/gnc/navigator_thrust_mapper/src/thruster_map.cpp @@ -0,0 +1,127 @@ +// Copyright 2025 University of Florida Machine Intelligence Laboratory +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "navigator_thrust_mapper/thruster_map.h" + +#include +#include +#include + +namespace navigator_thrust_mapper +{ + +double vrx_force_to_command_scalar(double force) +{ + if (force > 250.0) + return 1.0; + if (force < -100.0) + return -1.0; + if (force > 3.27398) + return -0.2 * std::log(-0.246597 * (0.56 - (4.73341 / std::pow((-0.01 + force), 0.38)))); + if (force < 0.0) + return -0.113122 * std::log(-154.285 * (0.99 - ((1.88948e12) / std::pow((199.13 + force), 5.34)))); + return (0.01 / 3.27398) * force; +} + +Eigen::VectorXd vrx_force_to_command(Eigen::VectorXd const &forces) +{ + Eigen::VectorXd result(forces.size()); + for (int i = 0; i < forces.size(); ++i) + result(i) = vrx_force_to_command_scalar(forces(i)); + return result; +} + +std::function generate_linear_force_to_command(double ratio) +{ + return [ratio](Eigen::VectorXd const &force) { return force * ratio; }; +} + +ThrusterMap::ThrusterMap(std::vector const &names, std::vector> const &positions, + std::vector const &angles, + std::function const &force_to_command, + std::array const &force_limit, std::array const &com, + std::vector const &joints) + : names(names), joints(joints), force_to_command_(force_to_command), force_limit_(force_limit) +{ + if (force_limit.size() != 2 || force_limit[1] > force_limit[0]) + throw std::invalid_argument("force_limit invalid"); + + size_t n = names.size(); + if (positions.size() != n || angles.size() != n) + throw std::invalid_argument("size mismatch"); + + thruster_matrix_ = Eigen::MatrixXd::Zero(3, n); + for (size_t i = 0; i < n; ++i) + { + double l_x = positions[i][0] - com[0]; + double l_y = positions[i][1] - com[1]; + double cos_a = std::cos(angles[i]); + double sin_a = std::sin(angles[i]); + double torque = l_x * sin_a - l_y * cos_a; + thruster_matrix_(0, i) = cos_a; + thruster_matrix_(1, i) = sin_a; + thruster_matrix_(2, i) = torque; + } + + Eigen::JacobiSVD svd(thruster_matrix_, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::VectorXd sv = svd.singularValues(); + double thresh = 1e-10 * std::max(thruster_matrix_.rows(), thruster_matrix_.cols()) * sv.maxCoeff(); + + Eigen::VectorXd sv_inv(sv.size()); + for (int i = 0; i < sv.size(); ++i) + sv_inv(i) = (sv(i) > thresh) ? 1.0 / sv(i) : 0.0; + + thruster_matrix_inv_ = svd.matrixV() * sv_inv.asDiagonal() * svd.matrixU().transpose(); +} + +ThrusterMap ThrusterMap::from_urdf(std::string const &urdf_string, std::string const &transmission_suffix) +{ + std::vector names = { "FL", "FR", "BL", "BR" }; + std::vector> positions = { { 0.5, 0.5 }, { 0.5, -0.5 }, { -0.5, 0.5 }, { -0.5, -0.5 } }; + std::vector angles = { 0.0, 0.0, 0.0, 0.0 }; + std::vector joints = { "fl_joint", "fr_joint", "bl_joint", "br_joint" }; + auto fc = generate_linear_force_to_command(1.0); + std::array fl = { 250.0, -250.0 }; + return ThrusterMap(names, positions, angles, fc, fl, { 0.0, 0.0 }, joints); +} + +ThrusterMap ThrusterMap::from_vrx_urdf(std::string const &urdf_string) +{ + std::vector names = { "FL", "FR", "BL", "BR" }; + std::vector> positions = { { 0.5, 0.5 }, { 0.5, -0.5 }, { -0.5, 0.5 }, { -0.5, -0.5 } }; + std::vector angles = { 0.0, 0.0, 0.0, 0.0 }; + auto fc = [](Eigen::VectorXd const &f) { return vrx_force_to_command(f); }; + std::array fl = { 250.0, -100.0 }; + return ThrusterMap(names, positions, angles, fc, fl); +} + +std::vector ThrusterMap::wrench_to_thrusts(std::array const &wrench) const +{ + Eigen::Vector3d w(wrench[0], wrench[1], wrench[2]); + Eigen::VectorXd forces = thruster_matrix_inv_ * w; + + for (int i = 0; i < forces.size(); ++i) + forces(i) = std::max(force_limit_[1], std::min(force_limit_[0], forces(i))); + + Eigen::VectorXd cmds = force_to_command_(forces); + return std::vector(cmds.data(), cmds.data() + cmds.size()); +} + +} // namespace navigator_thrust_mapper diff --git a/src/navigator/gnc/navigator_thrust_mapper/src/thruster_map_test.cpp b/src/navigator/gnc/navigator_thrust_mapper/src/thruster_map_test.cpp new file mode 100644 index 00000000..0581895f --- /dev/null +++ b/src/navigator/gnc/navigator_thrust_mapper/src/thruster_map_test.cpp @@ -0,0 +1,149 @@ +// Copyright 2025 University of Florida Machine Intelligence Laboratory +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "navigator_thrust_mapper/thruster_map.h" + +#include +#include +#include + +using namespace navigator_thrust_mapper; + +int main() +{ + std::cout << "=== ThrusterMap C++ Conversion Test ===" << std::endl << std::endl; + + // Test 1: VRX Force to Command Conversion + std::cout << "Test 1: VRX Force-to-Command Conversion" << std::endl; + std::cout << "----------------------------------------" << std::endl; + + double test_forces[] = { 0.0, 1.0, 3.27398, 50.0, 100.0, 250.0, 300.0, -50.0, -100.0, -150.0 }; + + for (double force : test_forces) + { + double cmd = vrx_force_to_command_scalar(force); + std::cout << " Force: " << std::setw(8) << std::fixed << std::setprecision(2) << force + << " N → Command: " << std::setw(8) << std::fixed << std::setprecision(4) << cmd << std::endl; + } + std::cout << std::endl; + + // Test 2: Vectorized force conversion + std::cout << "Test 2: Vectorized Force Conversion" << std::endl; + std::cout << "-----------------------------------" << std::endl; + + Eigen::VectorXd forces_vec(4); + forces_vec << 0.0, 50.0, 100.0, 250.0; + + Eigen::VectorXd cmds = vrx_force_to_command(forces_vec); + std::cout << " Input forces (N): " << forces_vec.transpose() << std::endl; + std::cout << " Output commands: " << cmds.transpose() << std::endl; + std::cout << std::endl; + + // Test 3: Linear force-to-command generator + std::cout << "Test 3: Linear Force-to-Command Generator" << std::endl; + std::cout << "------------------------------------------" << std::endl; + + double ratio = 0.1; + auto linear_converter = generate_linear_force_to_command(ratio); + + Eigen::VectorXd test_forces_linear(4); + test_forces_linear << 10.0, 20.0, 50.0, 100.0; + + Eigen::VectorXd linear_result = linear_converter(test_forces_linear); + std::cout << " Ratio: " << ratio << std::endl; + std::cout << " Input forces (N): " << test_forces_linear.transpose() << std::endl; + std::cout << " Output commands: " << linear_result.transpose() << std::endl; + std::cout << std::endl; + + // Test 4: ThrusterMap initialization and wrench-to-thrusts + std::cout << "Test 4: ThrusterMap - Wrench to Thrusts Conversion" << std::endl; + std::cout << "---------------------------------------------------" << std::endl; + + std::vector names = { "FL", "FR", "BL", "BR" }; + std::vector> positions = { + { 0.5, 0.5 }, // Front-Left + { 0.5, -0.5 }, // Front-Right + { -0.5, 0.5 }, // Back-Left + { -0.5, -0.5 } // Back-Right + }; + std::vector angles = { 0.0, 0.0, 0.0, 0.0 }; // All aligned forward + std::vector joints = { "fl_joint", "fr_joint", "bl_joint", "br_joint" }; + + auto force_to_cmd = generate_linear_force_to_command(1.0); + std::array force_limit = { 250.0, -250.0 }; + std::array com = { 0.0, 0.0 }; + + try + { + ThrusterMap thruster_map(names, positions, angles, force_to_cmd, force_limit, com, joints); + + std::cout << " ThrusterMap created successfully!" << std::endl; + std::cout << " Thrusters: "; + for (auto const& name : thruster_map.names) + std::cout << name << " "; + std::cout << std::endl << std::endl; + + // Test wrench conversion + std::array wrench = { 100.0, 0.0, 0.0 }; // Pure surge + std::vector thrusts = thruster_map.wrench_to_thrusts(wrench); + + std::cout << " Input wrench [surge, sway, yaw]: [" << wrench[0] << ", " << wrench[1] << ", " << wrench[2] + << "]" << std::endl; + std::cout << " Output thrusts:" << std::endl; + for (size_t i = 0; i < thrusts.size(); ++i) + { + std::cout << " " << names[i] << ": " << std::fixed << std::setprecision(4) << thrusts[i] << std::endl; + } + std::cout << std::endl; + + // Test with sway + wrench = { 0.0, 50.0, 0.0 }; // Pure sway + thrusts = thruster_map.wrench_to_thrusts(wrench); + + std::cout << " Input wrench [surge, sway, yaw]: [" << wrench[0] << ", " << wrench[1] << ", " << wrench[2] + << "]" << std::endl; + std::cout << " Output thrusts:" << std::endl; + for (size_t i = 0; i < thrusts.size(); ++i) + { + std::cout << " " << names[i] << ": " << std::fixed << std::setprecision(4) << thrusts[i] << std::endl; + } + std::cout << std::endl; + + // Test with yaw + wrench = { 0.0, 0.0, 25.0 }; // Pure yaw + thrusts = thruster_map.wrench_to_thrusts(wrench); + + std::cout << " Input wrench [surge, sway, yaw]: [" << wrench[0] << ", " << wrench[1] << ", " << wrench[2] + << "]" << std::endl; + std::cout << " Output thrusts:" << std::endl; + for (size_t i = 0; i < thrusts.size(); ++i) + { + std::cout << " " << names[i] << ": " << std::fixed << std::setprecision(4) << thrusts[i] << std::endl; + } + } + catch (std::exception const& e) + { + std::cerr << "ERROR: " << e.what() << std::endl; + return 1; + } + + std::cout << std::endl << "=== All Tests Completed Successfully ===" << std::endl; + return 0; +} diff --git a/src/navigator/gnc/navigator_thrust_mapper/src/thruster_mapper_node.cpp b/src/navigator/gnc/navigator_thrust_mapper/src/thruster_mapper_node.cpp new file mode 100644 index 00000000..a993424a --- /dev/null +++ b/src/navigator/gnc/navigator_thrust_mapper/src/thruster_mapper_node.cpp @@ -0,0 +1,204 @@ +// Copyright 2025 University of Florida Machine Intelligence Laboratory +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include +#include +#include +#include + +#include + +#include "navigator_thrust_mapper/thruster_map.h" + +#include +#include +#include +#include + +using std::placeholders::_1; + +namespace navigator_thrust_mapper +{ + +class ThrusterMapperNode : public rclcpp::Node +{ + public: + ThrusterMapperNode() : Node("thrust_mapper") + { + this->declare_parameter("is_vrx", false); + this->declare_parameter("is_simulation", false); + this->declare_parameter("robot_description", ""); + + is_vrx_ = this->get_parameter("is_vrx").as_bool(); + is_sim_ = this->get_parameter("is_simulation").as_bool(); + + std::string urdf = this->get_parameter("robot_description").as_string(); + if (urdf.empty()) + { + RCLCPP_FATAL(this->get_logger(), "robot description not set or empty"); + throw std::runtime_error("robot description not set or empty"); + } + + // create thruster_map from URDF + if (is_vrx_ || is_sim_) + { + thruster_map_ = ThrusterMap::from_vrx_urdf(urdf); + } + else + { + thruster_map_ = ThrusterMap::from_urdf(urdf); + } + + thruster_names_ = thruster_map_.names; + thrust_string_index_ = (is_vrx_ ? 5 : 0); + + if (is_vrx_ || is_sim_) + { + for (auto const &name : thruster_names_) + { + std::string topic = "/wamv/thrusters/" + name.substr(thrust_string_index_) + "_thrust_cmd"; + publishers_float_.push_back(this->create_publisher(topic, 1)); + } + } + else + { + for (auto const &name : thruster_names_) + { + std::string topic = "/" + name + "_motor/cmd"; + publishers_float_.push_back(this->create_publisher(topic, 1)); + } + } + + if (!is_vrx_ && !is_sim_) + { + joint_state_pub_ = this->create_publisher("/thruster_states", 1); + joint_state_msg_ = sensor_msgs::msg::JointState(); + for (auto const &j : thruster_map_.joints) + { + joint_state_msg_.name.push_back(j); + joint_state_msg_.position.push_back(0.0); + joint_state_msg_.effort.push_back(0.0); + } + } + + wrench_sub_ = this->create_subscription( + "/wrench/cmd", 1, std::bind(&ThrusterMapperNode::wrench_cb, this, _1)); + + // Subscribe to kill alarm + kill_sub_ = this->create_subscription("/kill", 1, + std::bind(&ThrusterMapperNode::kill_cb, this, _1)); + + // timer to publish at 30 Hz + timer_ = this->create_wall_timer(std::chrono::milliseconds(33), + std::bind(&ThrusterMapperNode::publish_thrusts, this)); + + RCLCPP_INFO(this->get_logger(), "ThrusterMapperNode initialized with %zu thrusters", thruster_names_.size()); + } + + private: + void wrench_cb(geometry_msgs::msg::WrenchStamped::SharedPtr const msg) + { + wrench_[0] = msg->wrench.force.x; + wrench_[1] = msg->wrench.force.y; + wrench_[2] = msg->wrench.torque.z; + } + + void kill_cb(std_msgs::msg::Bool::SharedPtr const msg) + { + kill_ = msg->data; + if (kill_) + { + RCLCPP_WARN(this->get_logger(), "Kill signal received - thrusters disabled"); + } + else + { + RCLCPP_INFO(this->get_logger(), "Kill signal cleared - thrusters enabled"); + } + } + + void publish_thrusts() + { + std::vector commands(publishers_float_.size(), 0.0f); + + if (!kill_) + { + auto thrusts_d = thruster_map_.wrench_to_thrusts(wrench_); + if (thrusts_d.size() != publishers_float_.size()) + { + RCLCPP_FATAL(this->get_logger(), "Number of thrusts does not equal number of publishers"); + return; + } + for (size_t i = 0; i < thrusts_d.size(); ++i) + { + commands[i] = static_cast(thrusts_d[i]); + } + } + + if (!is_vrx_ && !is_sim_) + { + for (size_t i = 0; i < publishers_float_.size(); ++i) + { + joint_state_msg_.effort[i] = commands[i]; + std_msgs::msg::Float32 m; + m.data = commands[i]; + publishers_float_[i]->publish(m); + } + joint_state_pub_->publish(joint_state_msg_); + } + else + { + for (size_t i = 0; i < publishers_float_.size(); ++i) + { + std_msgs::msg::Float32 m; + m.data = commands[i]; + publishers_float_[i]->publish(m); + } + } + } + + bool is_vrx_{ false }; + bool is_sim_{ false }; + bool kill_{ false }; + size_t thrust_string_index_{ 0 }; + + std::array wrench_{ { 0.0, 0.0, 0.0 } }; + std::vector thruster_names_; + + ThrusterMap thruster_map_; + + std::vector::SharedPtr> publishers_float_; + rclcpp::Publisher::SharedPtr joint_state_pub_; + sensor_msgs::msg::JointState joint_state_msg_; + + rclcpp::Subscription::SharedPtr wrench_sub_; + rclcpp::Subscription::SharedPtr kill_sub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace navigator_thrust_mapper + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}