diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp index 64a7d969..4ecc7d4f 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp +++ b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp @@ -11,9 +11,10 @@ using namespace std::chrono_literals; -class Allocator : public rclcpp::Node { +// TODO: Rename to ThrusterAllocator +class ThrusterAllocator : public rclcpp::Node { public: - explicit Allocator(); + explicit ThrusterAllocator(); void timer_callback(); private: diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp index 9fad061b..525f5e37 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp +++ b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp @@ -56,7 +56,7 @@ inline void calculateRightPseudoinverse(const Eigen::MatrixXd &M, Eigen::MatrixXd pseudoinverse = M.transpose() * (M * M.transpose()).inverse(); // pseudoinverse.completeOrthogonalDecomposition().pseudoInverse(); if (isInvalidMatrix(pseudoinverse)) { - throw "Invalid pseudoinverse calculated"; + throw std::runtime_error("Invalid Psuedoinverse Calculated"); } M_pinv = pseudoinverse; } diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp index 3fd10a4d..6fce539d 100644 --- a/motion/thruster_allocator/src/allocator_ros.cpp +++ b/motion/thruster_allocator/src/allocator_ros.cpp @@ -15,18 +15,18 @@ using namespace std::chrono_literals; -Allocator::Allocator() +ThrusterAllocator::ThrusterAllocator() : Node("thruster_allocator_node"), pseudoinverse_allocator_(Eigen::MatrixXd::Zero(3, 4)) { subscription_ = this->create_subscription( "thrust/wrench_input", 1, - std::bind(&Allocator::wrench_callback, this, std::placeholders::_1)); + std::bind(&ThrusterAllocator::wrench_callback, this, std::placeholders::_1)); publisher_ = this->create_publisher( "thrust/thruster_forces", 1); timer_ = this->create_wall_timer(100ms, - std::bind(&Allocator::timer_callback, this)); + std::bind(&ThrusterAllocator::timer_callback, this)); Eigen::MatrixXd thrust_configuration_pseudoinverse; calculateRightPseudoinverse(thrust_configuration, @@ -47,7 +47,7 @@ Allocator::Allocator() * saturation, it logs a warning message. */ -void Allocator::timer_callback() { +void ThrusterAllocator::timer_callback() { Eigen::VectorXd thruster_forces = pseudoinverse_allocator_.calculateAllocatedThrust(body_frame_forces_); @@ -78,7 +78,7 @@ void Allocator::timer_callback() { * returns. * @param msg The received geometry_msgs::msg::Wrench message. */ -void Allocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) { +void ThrusterAllocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) { body_frame_forces_(0) = msg.force.x; // surge body_frame_forces_(1) = msg.force.y; // sway body_frame_forces_(2) = msg.torque.z; // yaw @@ -95,7 +95,7 @@ void Allocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) { * @param v The Eigen vector to check. * @return True if the vector is healthy, false otherwise. */ -bool Allocator::healthyWrench(const Eigen::VectorXd &v) const { +bool ThrusterAllocator::healthyWrench(const Eigen::VectorXd &v) const { // Check for NaN/Inf if (isInvalidMatrix(v)) return false; diff --git a/motion/thruster_allocator/src/thruster_allocator_node.cpp b/motion/thruster_allocator/src/thruster_allocator_node.cpp index 146ae471..73875a04 100644 --- a/motion/thruster_allocator/src/thruster_allocator_node.cpp +++ b/motion/thruster_allocator/src/thruster_allocator_node.cpp @@ -3,7 +3,7 @@ int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto allocator = std::make_shared(); + auto allocator = std::make_shared(); RCLCPP_INFO(allocator->get_logger(), "Thruster allocator initiated"); rclcpp::spin(allocator); rclcpp::shutdown();