Add runtime_error for calculateRightPseudoinverse and renamed Allocat…

…or to ThrusterAllocator

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:

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;
 }

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<geometry_msgs::msg::Wrench>(
       "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<vortex_msgs::msg::ThrusterForces>(
       "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;

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<Allocator>();
+  auto allocator = std::make_shared<ThrusterAllocator>();
   RCLCPP_INFO(allocator->get_logger(), "Thruster allocator initiated");
   rclcpp::spin(allocator);
   rclcpp::shutdown();