Merge branch 'thrust_allocator_fix' into development

auv_setup/launch/orca.launch.py

@@ -13,10 +13,10 @@ def generate_launch_description():
         value='[${severity}] [${time}] [${node}]: ${message}')
 
     # Thruster Allocator launch
-    thruster_allocator_launch = IncludeLaunchDescription(
+    thrust_allocator_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(get_package_share_directory('thruster_allocator_auv'),
-                         'launch', 'thruster_allocator_auv.launch.py')))
+            os.path.join(get_package_share_directory('thrust_allocator_auv'),
+                         'launch', 'thrust_allocator_auv.launch.py')))
 
     #Thruster Interface launch
     thruster_interface_launch = IncludeLaunchDescription(
@@ -26,4 +26,4 @@ def generate_launch_description():
 
     # Return launch description
     return LaunchDescription(
-        [set_env_var, thruster_allocator_launch, thruster_interface_launch])
+        [set_env_var, thrust_allocator_launch, thruster_interface_launch])

motion/thruster_allocator_auv/CMakeLists.txt → motion/thrust_allocator_auv/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(thruster_allocator_auv)
+project(thrust_allocator_auv)
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
@@ -20,8 +20,8 @@ find_package(Eigen3 REQUIRED)
 include_directories(include)
 
 add_executable(${PROJECT_NAME}_node 
-  src/thruster_allocator_auv_node.cpp 
-  src/thruster_allocator_ros.cpp 
+  src/thrust_allocator_auv_node.cpp 
+  src/thrust_allocator_ros.cpp 
   src/pseudoinverse_allocator.cpp
 )
 

motion/thruster_allocator_auv/include/thruster_allocator_auv/thruster_allocator_ros.hpp → motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp

@@ -1,22 +1,22 @@
 /**
- * @file thruster_allocator_ros.hpp
+ * @file thrust_allocator_ros.hpp
  * @brief Contains the ROS logic for the thruster allocator node.
  */
 
 #ifndef VORTEX_ALLOCATOR_ROS_HPP
 #define VORTEX_ALLOCATOR_ROS_HPP
 
-#include "thruster_allocator_auv/eigen_vector6d_typedef.hpp"
-#include "thruster_allocator_auv/pseudoinverse_allocator.hpp"
-#include "thruster_allocator_auv/thruster_allocator_utils.hpp"
+#include "thrust_allocator_auv/eigen_vector6d_typedef.hpp"
+#include "thrust_allocator_auv/pseudoinverse_allocator.hpp"
+#include "thrust_allocator_auv/thrust_allocator_utils.hpp"
 #include <eigen3/Eigen/Eigen>
 #include <geometry_msgs/msg/wrench.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <vortex_msgs/msg/thruster_forces.hpp>
 
-class ThrusterAllocator : public rclcpp::Node {
+class ThrustAllocator : public rclcpp::Node {
 public:
-  explicit ThrusterAllocator();
+  explicit ThrustAllocator();
 
   /**
    * @brief Calculates the allocated
@@ -27,8 +27,6 @@ class ThrusterAllocator : public rclcpp::Node {
   void calculate_thrust_timer_cb();
 
 private:
-  Eigen::MatrixXd thrust_configuration;
-
   /**
    * @brief Callback function for the wrench input subscription. Extracts the
    * surge, sway, heave, roll, pitch and yaw values from the received wrench msg

motion/thruster_allocator_auv/include/thruster_allocator_auv/thruster_allocator_utils.hpp → motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp

@@ -1,5 +1,5 @@
 /**
- * @file thruster_allocator_utils.hpp
+ * @file thrust_allocator_utils.hpp
  * @brief This file contains utility functions for the thruster allocator
  * module.
  */

motion/thruster_allocator_auv/launch/thruster_allocator_auv.launch.py → motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py

@@ -5,14 +5,14 @@
 
 
 def generate_launch_description():
-    thruster_allocator_auv_node = Node(
-        package='thruster_allocator_auv',
-        executable='thruster_allocator_auv_node',
-        name='thruster_allocator_auv_node',
+    thrust_allocator_auv_node = Node(
+        package='thrust_allocator_auv',
+        executable='thrust_allocator_auv_node',
+        name='thrust_allocator_auv_node',
         parameters=[
             path.join(get_package_share_directory('auv_setup'), 'config',
                       'robots', 'orca.yaml')
         ],
         output='screen',
     )
-    return LaunchDescription([thruster_allocator_auv_node])
+    return LaunchDescription([thrust_allocator_auv_node])

motion/thruster_allocator_auv/package.xml → motion/thrust_allocator_auv/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>thruster_allocator_auv</name>
+  <name>thrust_allocator_auv</name>
   <version>0.0.0</version>
   <description>Thruster allocator for AUV</description>
   <maintainer email="[email protected]">alice</maintainer>

motion/thruster_allocator_auv/src/pseudoinverse_allocator.cpp → motion/thrust_allocator_auv/src/pseudoinverse_allocator.cpp

@@ -1,4 +1,4 @@
-#include "thruster_allocator_auv/pseudoinverse_allocator.hpp"
+#include "thrust_allocator_auv/pseudoinverse_allocator.hpp"
 
 PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv)
     : T_pinv(T_pinv) {}

motion/thrust_allocator_auv/src/thrust_allocator_auv_node.cpp

@@ -0,0 +1,11 @@
+#include "thrust_allocator_auv/thrust_allocator_ros.hpp"
+#include "thrust_allocator_auv/thrust_allocator_utils.hpp"
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto allocator = std::make_shared<ThrustAllocator>();
+  RCLCPP_INFO(allocator->get_logger(), "Thrust allocator initiated");
+  rclcpp::spin(allocator);
+  rclcpp::shutdown();
+  return 0;
+}

motion/thruster_allocator_auv/src/thruster_allocator_ros.cpp → motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp

@@ -1,15 +1,15 @@
-#include "thruster_allocator_auv/thruster_allocator_ros.hpp"
-#include "thruster_allocator_auv/pseudoinverse_allocator.hpp"
-#include "thruster_allocator_auv/thruster_allocator_utils.hpp"
+#include "thrust_allocator_auv/thrust_allocator_ros.hpp"
+#include "thrust_allocator_auv/pseudoinverse_allocator.hpp"
+#include "thrust_allocator_auv/thrust_allocator_utils.hpp"
 #include <vortex_msgs/msg/thruster_forces.hpp>
 
 #include <chrono>
 #include <functional>
 
 using namespace std::chrono_literals;
 
-ThrusterAllocator::ThrusterAllocator()
-    : Node("thruster_allocator_node"),
+ThrustAllocator::ThrustAllocator()
+    : Node("thrust_allocator_node"),
       pseudoinverse_allocator_(Eigen::MatrixXd::Zero(6, 8)) {
   declare_parameter("physical.center_of_mass", std::vector<double>{0});
   declare_parameter("propulsion.dimensions.num", 3);
@@ -47,23 +47,23 @@ ThrusterAllocator::ThrusterAllocator()
 
   wrench_subscriber_ = this->create_subscription<geometry_msgs::msg::Wrench>(
       "thrust/wrench_input", 1,
-      std::bind(&ThrusterAllocator::wrench_cb, this, std::placeholders::_1));
+      std::bind(&ThrustAllocator::wrench_cb, this, std::placeholders::_1));
 
   thruster_forces_publisher_ =
       this->create_publisher<vortex_msgs::msg::ThrusterForces>(
           "thrust/thruster_forces", 5);
 
   calculate_thrust_timer_ = this->create_wall_timer(
       thrust_update_period_,
-      std::bind(&ThrusterAllocator::calculate_thrust_timer_cb, this));
+      std::bind(&ThrustAllocator::calculate_thrust_timer_cb, this));
 
   pseudoinverse_allocator_.T_pinv =
-      calculate_right_pseudoinverse(thrust_configuration);
+      calculate_right_pseudoinverse(thrust_configuration_);
 
   body_frame_forces_.setZero();
 }
 
-void ThrusterAllocator::calculate_thrust_timer_cb() {
+void ThrustAllocator::calculate_thrust_timer_cb() {
   Eigen::VectorXd thruster_forces =
       pseudoinverse_allocator_.calculate_allocated_thrust(body_frame_forces_);
 
@@ -81,7 +81,7 @@ void ThrusterAllocator::calculate_thrust_timer_cb() {
   thruster_forces_publisher_->publish(msg_out);
 }
 
-void ThrusterAllocator::wrench_cb(const geometry_msgs::msg::Wrench &msg) {
+void ThrustAllocator::wrench_cb(const geometry_msgs::msg::Wrench &msg) {
   Eigen::Vector6d msg_vector;
   msg_vector(0) = msg.force.x;  // surge
   msg_vector(1) = msg.force.y;  // sway
@@ -97,7 +97,7 @@ void ThrusterAllocator::wrench_cb(const geometry_msgs::msg::Wrench &msg) {
   std::swap(msg_vector, body_frame_forces_);
 }
 
-bool ThrusterAllocator::healthy_wrench(const Eigen::VectorXd &v) const {
+bool ThrustAllocator::healthy_wrench(const Eigen::VectorXd &v) const {
   if (is_invalid_matrix(v))
     return false;