Cleanup, add launch file, thrust saturation

motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp

@@ -29,7 +29,6 @@ class Allocator : public rclcpp::Node {
   Eigen::VectorXd wrenchMsgToEigen(const geometry_msgs::msg::Wrench &msg) const;
   Eigen::VectorXd wrenchMsgToEigen(const float force_x, const float force_y,
                                    const float torque) const;
-  rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Publisher<vortex_msgs::msg::ThrusterForces>::SharedPtr publisher_;
   rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr subscription_;
   size_t count_;

motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp

@@ -7,71 +7,56 @@
 #include <string>
 #include <vector>
 
-// Return true if X has any nan or inf elements.
+// Return true if M has any NaN or INF elements.
 template <typename Derived>
-inline bool isInvalidMatrix(const Eigen::MatrixBase<Derived> &X) {
-  bool has_nan = !(X.array() == X.array()).all();
-  bool has_inf = !((X - X).array() == (X - X).array()).all();
+inline bool isInvalidMatrix(const Eigen::MatrixBase<Derived> &M) {
+  bool has_nan = !(M.array() == M.array()).all();
+  bool has_inf = M.array().isInf().any();
   return has_nan || has_inf;
 }
 
-inline void printMatrix(std::string name, const Eigen::MatrixXd &X) {
+inline void printMatrix(std::string name, const Eigen::MatrixXd &M) {
   std::stringstream ss;
-  ss << std::endl << name << " = " << std::endl << X;
+  ss << std::endl << name << " = " << std::endl << M;
   RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), ss.str());
 }
 
-inline void printVector(std::string name, const Eigen::VectorXd &X) {
+inline void printVector(std::string name, const Eigen::VectorXd &M) {
   std::stringstream ss;
-  ss << std::endl << name << " = " << std::endl << X;
+  ss << std::endl << name << " = " << std::endl << M;
   RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), ss.str());
 }
 
-// Calculate the pseudoinverse matrix of the matrix X.
+// Calculate the pseudoinverse matrix of the matrix M.
 // Return false if the calculations fails.
-inline bool calculatePseudoinverse(const Eigen::MatrixXd &X,
-                                   Eigen::MatrixXd *X_pinv) {
-  Eigen::MatrixXd pseudoinverse = X.transpose() * (X * X.transpose()).inverse();
+inline bool calculatePseudoinverse(const Eigen::MatrixXd &M,
+                                   Eigen::MatrixXd *M_pinv) {
+  Eigen::MatrixXd pseudoinverse = M.transpose() * (M * M.transpose()).inverse();
 
   if (isInvalidMatrix(pseudoinverse)) {
     return false;
   }
-  *X_pinv = pseudoinverse;
+  *M_pinv = pseudoinverse;
   return true;
 }
 
-//
-// TODO: Trengs disse funksjonene under?
-//
-
-// Read a matrix from the ROS parameter server.
-// Return false if unsuccessful.
-// Burde ikke ligge her!!
-// inline bool getMatrixParam(ros::NodeHandle nh, std::string name,
-//                            Eigen::MatrixXd *X) {
-//   XmlRpc::XmlRpcValue matrix;
-//   nh.getParam(name, matrix);
-
-//   try {
-//     const int rows = matrix.size();
-//     const int cols = matrix[0].size();
-//     X->setZero(rows, cols);
-//     for (int i = 0; i < rows; ++i)
-//       for (int j = 0; j < cols; ++j)
-//         (*X)(i, j) = matrix[i][j];
-//   } catch (...) {
-//     return false;
-//   }
-//   return true;
-// }
-
-// Return the 3-by-3 skew-symmetric matrix of the vector v.
-inline Eigen::Matrix3d createSkewSymmetricMatrix(const Eigen::Vector3d &v) {
-  Eigen::Matrix3d S;
-  S << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
-  return S;
+// Saturate all elements of vector v to within [min, max].
+// Return true if all elements already are within the range.
+inline bool saturateVectorValues(Eigen::VectorXd *vec, double min, double max) {
+  bool vector_in_range = true;
+  for (int i = 0; i < vec->size(); ++i) {
+    if ((*vec)(i) > max) {
+      (*vec)(i) = max;
+      vector_in_range = false;
+    } else if ((*vec)(i) < min) {
+      (*vec)(i) = min;
+      vector_in_range = false;
+    }
+  }
+  return vector_in_range;
 }
 
+// Copies vector elements into ThrusterForces message
 inline void arrayEigenToMsg(const Eigen::VectorXd &u,
                             vortex_msgs::msg::ThrusterForces *msg) {
   int r = u.size();
@@ -81,20 +66,12 @@ inline void arrayEigenToMsg(const Eigen::VectorXd &u,
   msg->thrust = u_vec;
 }
 
-// Saturate all elements of vector v to within [min, max].
-// Return true if all elements already are within the range.
-inline bool clampVectorValues(Eigen::VectorXd *v, double min, double max) {
-  bool vector_in_range = true;
-  for (int i = 0; i < v->size(); ++i) {
-    if ((*v)(i) > max) {
-      (*v)(i) = max;
-      vector_in_range = false;
-    } else if ((*v)(i) < min) {
-      (*v)(i) = min;
-      vector_in_range = false;
-    }
-  }
-  return vector_in_range;
+// Return the 3-by-3 skew-symmetric matrix of the vector v.
+// CURRENTLY UNUSED 
+inline Eigen::Matrix3d createSkewSymmetricMatrix(const Eigen::Vector3d &v) {
+  Eigen::Matrix3d S;
+  S << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
+  return S;
 }
 
 #endif // VORTEX_ALLOCATOR_ALLOCATOR_UTILS_HPP

motion/thruster_allocator/launch/thruster_allocator_launch.py

@@ -0,0 +1,11 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='thruster_allocator',
+            executable='thruster_allocator_node',
+            name='allocator'
+        )
+    ])

motion/thruster_allocator/src/allocator_ros.cpp

@@ -4,7 +4,6 @@
 #include "vortex_msgs/msg/thruster_forces.hpp"
 
 Allocator::Allocator() : Node("thrust_allocator_node") {
-
   subscription_ = this->create_subscription<geometry_msgs::msg::Wrench>(
       "thrust/wrench_input", 1,
       std::bind(&Allocator::wrench_callback, this, std::placeholders::_1));
@@ -13,13 +12,12 @@ Allocator::Allocator() : Node("thrust_allocator_node") {
       "thrust/thruster_forces", 1);
 
   Eigen::MatrixXd thrust_configuration_pseudoinverse;
-  if (!calculatePseudoinverse(thrust_configuration,
-                              &thrust_configuration_pseudoinverse)) {
-    RCLCPP_FATAL(get_logger(), "Failed to compute pseudoinverse of thrust "
-                               "config matrix. Killing node...");
+  if (!calculatePseudoinverse(thrust_configuration, &thrust_configuration_pseudoinverse)) {
+    RCLCPP_FATAL(get_logger(), "Failed to compute pseudoinverse of thrust config matrix. Killing node...");
     rclcpp::shutdown();
   }
 
+  // Set T_pinv in m_pseudoinverse_allocator
   m_pseudoinverse_allocator.reset(
       new PseudoinverseAllocator(thrust_configuration_pseudoinverse));
 }
@@ -28,44 +26,37 @@ void Allocator::spinOnce() {
   const Eigen::VectorXd body_frame_forces = wrenchMsgToEigen(
       body_frame_force_x, body_frame_force_y, body_frame_torque);
 
-
-  // printMatrix("T_pinv", m_pseudoinverse_allocator->T_pinv);
-
-  // u vector
   Eigen::VectorXd thruster_forces =
       m_pseudoinverse_allocator->calculateAllocatedThrust(body_frame_forces);
 
-  // TODO: Legg til isValicMatrix sjekk og clampVectorValues (saturateVector)
-
   if(isInvalidMatrix(thruster_forces)){
     RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid");
     return;
   }
 
+  // Saturates output vector between min and max values
+  if(!saturateVectorValues(&thruster_forces, -100, 100)){
+    RCLCPP_WARN(get_logger(), "Thruster forces vector required saturation.");
+  }
+
   vortex_msgs::msg::ThrusterForces msg_out;
-  //printVector("thruster_forces", thruster_forces);
   arrayEigenToMsg(thruster_forces, &msg_out);
-  for (int i = 0; i < 4; i++) // 4 thrusters
+  // Number of thrusters = 4
+  for (int i = 0; i < 4; i++){
     msg_out.thrust[i] *= m_direction[i];
-  //for (double d : msg_out.thrust){
-  //  RCLCPP_INFO(this->get_logger(), "msg_out-thrust: '%f'", d);
-  //}
-
+  }
   publisher_->publish(msg_out);
 }
 
 void Allocator::wrench_callback(const geometry_msgs::msg::Wrench &msg) {
-  const Eigen::VectorXd body_frame_forces = wrenchMsgToEigen(msg); // Wrench
-  // health check
+  const Eigen::VectorXd body_frame_forces = wrenchMsgToEigen(msg);
   if (!healthyWrench(body_frame_forces)) {
     RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring.");
     return;
   }
-
   body_frame_force_x = msg.force.x;
   body_frame_force_y = msg.force.y;
   body_frame_torque = msg.torque.z;
-  //RCLCPP_INFO(this->get_logger(), "I heard: '%f'", msg.force.x);
 }
 
 Eigen::VectorXd
@@ -98,15 +89,4 @@ Eigen::VectorXd Allocator::wrenchMsgToEigen(const float force_x,
       return false;
 
   return true;
- }
-
-// void Allocator::timer_callback() {
-//     auto message = geometry_msgs::msg::Wrench();
-//     message.force.x = 1;
-//     message.force.y = 1;
-//     message.force.z = 1;
-
-//     double surge = message.force.x;
-//     RCLCPP_INFO(this->get_logger(), "Publishing: '%d'", surge);
-//     publisher_->publish(message);
-// }
+ }

motion/thruster_allocator/src/main.cpp

@@ -6,6 +6,7 @@ int main(int argc, char **argv) {
   rclcpp::init(argc, argv);
   auto allocator = std::make_shared<Allocator>();
   rclcpp::Rate loop_rate(10);
+  RCLCPP_INFO(allocator->get_logger(), "Thruster allocator initiated");
 
   while (rclcpp::ok()) {
     allocator->spinOnce();