Merge commit '7a0fff71a67b25ccfff64de2a99ca38cef989662' into HEAD

asv_setup/config/robots/freya.yaml

@@ -32,15 +32,17 @@
           yaw:   true
       thrusters:
         num: 4
+        min: -100
+        max: 100
         configuration_matrix: #NED
-         "[[ 0.70711,   0.70711,   0.70711,   0.70711], # Surge
-          [ -0.70711,  0.70711,   -0.70711,  0.70711], # Sway
-          [ 0.27738,   0.27738,  -0.27738,  -0.27738]]" # Yaw
+          [0.70711, 0.70711, 0.70711, 0.70711, 
+          -0.70711, 0.70711, -0.70711, 0.70711, 
+          0.27738, 0.27738, -0.27738, -0.27738]
 
         rate_of_change:
           max: 0 # Maximum rate of change in newton per second for a thruster
         thruster_to_pin_map: [1, 3, 2, 0] # I.e. if thruster_to_pin = [1, 3, 2, 0] then thruster 0 is pin 1 etc..
-        direction: [1,  1,  1, 1] # Disclose during thruster mapping
+        direction: [1, 1, 1, 1] # Disclose during thruster mapping
         offset: [0, 0, 0, 0] # Disclose during thruster mapping
       command:
         wrench:

motion/thruster_allocator/CMakeLists.txt

@@ -41,11 +41,30 @@ install(TARGETS
   ${PROJECT_NAME}_node
   DESTINATION lib/${PROJECT_NAME})
 
+# if(BUILD_TESTING)
+#   find_package(ament_lint_auto REQUIRED)
+#   set(ament_cmake_copyright_FOUND TRUE)
+#   set(ament_cmake_cpplint_FOUND TRUE)
+#   ament_lint_auto_find_test_dependencies()
+# endif()
+
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  set(ament_cmake_copyright_FOUND TRUE)
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(${PROJECT_NAME}_test 
+    test/main.cpp
+    test/allocator_test.cpp
+  )
+
+
+  target_include_directories(${PROJECT_NAME}_test PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  ament_target_dependencies(${PROJECT_NAME}_test
+    rclcpp 
+    geometry_msgs 
+    vortex_msgs
+  )
 endif()
 
 # Install launch files.

motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp

@@ -30,16 +30,7 @@ class ThrusterAllocator : public rclcpp::Node {
   void timer_callback();
 
 private:
-  // Hardcoded thruster config matrix for T_pinv
-  // clang-format off
-  Eigen::MatrixXd thrust_configuration =
-      (Eigen::MatrixXd(3, 4) << 
-      0.70711, 0.70711, 0.70711, 0.70711, 
-      -0.70711, 0.70711, -0.70711, 0.70711, 
-      0.27738, 0.27738, -0.27738, -0.27738)
-          .finished();
-  // clang-format on
-
+  Eigen::MatrixXd thrust_configuration;
   /**
    * @brief Callback function for the wrench input subscription. Extracts the
    * surge, sway and yaw values from the received wrench msg
@@ -59,11 +50,11 @@ class ThrusterAllocator : public rclcpp::Node {
   rclcpp::TimerBase::SharedPtr timer_;
   size_t count_;
   int num_dof_;
-  int num_thrusters_ = 4;
-  int min_thrust_ = -100;
-  int max_thrust_ = 100;
+  int num_thrusters_;
+  int min_thrust_;
+  int max_thrust_;
   Eigen::Vector3d body_frame_forces_;
-  std::vector<int> direction_ = {1, 1, 1, 1};
+  std::vector<int64_t> direction_;
   PseudoinverseAllocator pseudoinverse_allocator_;
 };
 

motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp

@@ -100,4 +100,17 @@ inline void arrayEigenToMsg(const Eigen::VectorXd &u,
   msg.thrust = u_vec;
 }
 
+inline Eigen::MatrixXd doubleArrayToEigenMatrix(const std::vector<double> &matrix, int rows, int cols) {
+
+    Eigen::MatrixXd result(rows, cols);
+
+    for (int i = 0; i < rows; ++i) {
+        for (int j = 0; j < cols; ++j) {
+            result(i, j) = matrix[i * cols + j];
+        }
+    }
+
+    return result;
+}
+
 #endif // VORTEX_ALLOCATOR_ALLOCATOR_UTILS_HPP

motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp

@@ -1,16 +1,33 @@
-// Implement the unweighted pseudoinverse-based allocator described in e.g.
-// Fossen 2011 Handbook of Marine Craft Hydrodynamics and Motion Control
-// (chapter 12.3.2).
+/**
+ * @file pseudoinverse_allocator.hpp
+ * @brief Contains the PseudoinverseAllocator class, which implements the unweighted pseudoinverse-based allocator described in e.g. Fossen 2011 Handbook of Marine Craft Hydrodynamics and Motion Control (chapter 12.3.2).
+ */
 
 #ifndef VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP
 #define VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP
 
 #include <eigen3/Eigen/Eigen>
 
+/**
+ * @brief The PseudoinverseAllocator class calculates the allocated thrust given the input torques using the pseudoinverse allocator.
+ */
 class PseudoinverseAllocator {
 public:
+  /**
+   * @brief Constructor for the PseudoinverseAllocator class.
+   *
+   * @param T_pinv The pseudoinverse of the thruster configuration matrix.
+   */
   explicit PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv);
+
+  /**
+   * @brief Calculates the allocated thrust given the input torques using the pseudoinverse allocator.
+   *
+   * @param tau The input torques as a vector.
+   * @return The allocated thrust as a vector.
+   */
   Eigen::VectorXd calculateAllocatedThrust(const Eigen::VectorXd &tau);
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   Eigen::MatrixXd T_pinv;
 };

motion/thruster_allocator/package.xml

@@ -11,7 +11,10 @@
   <depend>geometry_msgs</depend>
   <depend>vortex_msgs</depend>
   <depend>eigen</depend>
-
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

motion/thruster_allocator/src/allocator_ros.cpp

@@ -10,7 +10,27 @@ using namespace std::chrono_literals;
 
 ThrusterAllocator::ThrusterAllocator()
     : Node("thruster_allocator_node"),
-      pseudoinverse_allocator_(Eigen::MatrixXd::Zero(3, 4)) {
+      pseudoinverse_allocator_(Eigen::MatrixXd::Zero(3, 4)) 
+{
+  //TEST
+  // std::vector<double> test = {0.70711, 0.70711, 0.70711, 0.70711, -0.70711, 0.70711, -0.70711, 0.70711, 0.27738, 0.27738, -0.27738, -0.27738};
+
+  declare_parameter("propulsion.dofs.num", 3);
+  declare_parameter("propulsion.thrusters.num", 4);
+  declare_parameter("propulsion.thrusters.min", -100);
+  declare_parameter("propulsion.thrusters.max", 100);
+  declare_parameter("propulsion.thrusters.direction", std::vector<int64_t>{0});
+  declare_parameter("propulsion.thrusters.configuration_matrix", std::vector<double>{0});
+
+  num_dof_ = get_parameter("propulsion.dofs.num").as_int();
+  num_thrusters_ = get_parameter("propulsion.thrusters.num").as_int();
+  min_thrust_ = get_parameter("propulsion.thrusters.min").as_int();
+  max_thrust_= get_parameter("propulsion.thrusters.max").as_int();
+  direction_ = get_parameter("propulsion.thrusters.direction").as_integer_array();
+  thrust_configuration = doubleArrayToEigenMatrix(get_parameter("propulsion.thrusters.configuration_matrix").as_double_array(), num_dof_, num_thrusters_);
+
+  RCLCPP_INFO(get_logger(), printMatrix("Test thrust_configuration test from ", thrust_configuration).str().c_str());
+
   subscription_ = this->create_subscription<geometry_msgs::msg::Wrench>(
       "thrust/wrench_input", 1,
       std::bind(&ThrusterAllocator::wrench_callback, this,

motion/thruster_allocator/src/pseudoinverse_allocator.cpp

@@ -1,20 +1,8 @@
 #include <thruster_allocator/pseudoinverse_allocator.hpp>
 
-/**
- * @brief Constructor for the PseudoinverseAllocator class.
- *
- * @param T_pinv The pseudoinverse of the thruster configuration matrix.
- */
 PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv)
     : T_pinv(T_pinv) {}
 
-/**
- * @brief Calculates the allocated thrust given the input torques using the
- * pseudoinverse allocator.
- *
- * @param tau The input torques as a vector.
- * @return The allocated thrust as a vector.
- */
 Eigen::VectorXd
 PseudoinverseAllocator::calculateAllocatedThrust(const Eigen::VectorXd &tau) {
   Eigen::VectorXd u = T_pinv * tau;