diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml index 28f17a2e..7ec64e1e 100644 --- a/asv_setup/config/robots/freya.yaml +++ b/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: diff --git a/motion/thruster_allocator/CMakeLists.txt b/motion/thruster_allocator/CMakeLists.txt index f2265f87..8556ff1e 100644 --- a/motion/thruster_allocator/CMakeLists.txt +++ b/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 + $ + $ + ) + ament_target_dependencies(${PROJECT_NAME}_test + rclcpp + geometry_msgs + vortex_msgs + ) endif() # Install launch files. diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp index 9a291583..35bcd54b 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp +++ b/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 direction_ = {1, 1, 1, 1}; + std::vector direction_; PseudoinverseAllocator pseudoinverse_allocator_; }; diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp index d78f355a..9d9c477a 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp +++ b/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 &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 diff --git a/motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp b/motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp index b0d6cc2e..e5f256f8 100644 --- a/motion/thruster_allocator/include/thruster_allocator/pseudoinverse_allocator.hpp +++ b/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 +/** + * @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; }; diff --git a/motion/thruster_allocator/package.xml b/motion/thruster_allocator/package.xml index 0ab2d25f..6f3f3a38 100644 --- a/motion/thruster_allocator/package.xml +++ b/motion/thruster_allocator/package.xml @@ -11,7 +11,10 @@ geometry_msgs vortex_msgs eigen - + + ament_cmake_gtest + ament_lint_auto + ament_lint_common ament_cmake diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp index 77239e92..71da83a5 100644 --- a/motion/thruster_allocator/src/allocator_ros.cpp +++ b/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 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{0}); + declare_parameter("propulsion.thrusters.configuration_matrix", std::vector{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( "thrust/wrench_input", 1, std::bind(&ThrusterAllocator::wrench_callback, this, diff --git a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp index 1e16a726..4ffd93be 100644 --- a/motion/thruster_allocator/src/pseudoinverse_allocator.cpp +++ b/motion/thruster_allocator/src/pseudoinverse_allocator.cpp @@ -1,20 +1,8 @@ #include -/** - * @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;