diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp index 53ea2d4a..36a7f7a5 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_ros.hpp +++ b/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::SharedPtr publisher_; rclcpp::Subscription::SharedPtr subscription_; size_t count_; diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp index 019a8628..f501a722 100644 --- a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp +++ b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp @@ -7,71 +7,56 @@ #include #include -// Return true if X has any nan or inf elements. +// Return true if M has any NaN or INF elements. template -inline bool isInvalidMatrix(const Eigen::MatrixBase &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 &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 diff --git a/motion/thruster_allocator/launch/thruster_allocator_launch.py b/motion/thruster_allocator/launch/thruster_allocator_launch.py new file mode 100644 index 00000000..1c79476b --- /dev/null +++ b/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' + ) + ]) \ No newline at end of file diff --git a/motion/thruster_allocator/src/allocator_ros.cpp b/motion/thruster_allocator/src/allocator_ros.cpp index 61728abb..f0d47636 100644 --- a/motion/thruster_allocator/src/allocator_ros.cpp +++ b/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( "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); -// } \ No newline at end of file + } \ No newline at end of file diff --git a/motion/thruster_allocator/src/main.cpp b/motion/thruster_allocator/src/main.cpp index 5fa35176..cf32fcd4 100644 --- a/motion/thruster_allocator/src/main.cpp +++ b/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(); rclcpp::Rate loop_rate(10); + RCLCPP_INFO(allocator->get_logger(), "Thruster allocator initiated"); while (rclcpp::ok()) { allocator->spinOnce();