Updated and removed unused variables

vortexntnu · Feb 18, 2024 · a8cc6ed · a8cc6ed
1 parent 96c3154
commit a8cc6ed
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 19 deletions.
diff --git a/auv_setup/config/robots/new_auv.yaml b/auv_setup/config/robots/new_auv.yaml
@@ -19,13 +19,6 @@
     propulsion:
       dofs:
         num: 6
-        which:
-          surge: true
-          sway:  true
-          heave: true
-          roll:  true
-          pitch: true
-          yaw:   true
       thrusters:
         num: 8
         min: -100

diff --git a/...thruster_allocator_auv/eigen_typedefs.hpp → ..._allocator_auv/eigen_vector6d_typedef.hpp b/...thruster_allocator_auv/eigen_typedefs.hpp → ..._allocator_auv/eigen_vector6d_typedef.hpp
@@ -1,5 +1,5 @@
 /**
- * @file eigen_typedefs.hpp
+ * @file eigen_vector6d_typedef.hpp
  * @brief Contains the typedef for a 6x1 Eigen vector.
  */
 

diff --git a/motion/thruster_allocator_auv/include/thruster_allocator_auv/thruster_allocator_ros.hpp b/motion/thruster_allocator_auv/include/thruster_allocator_auv/thruster_allocator_ros.hpp
@@ -6,7 +6,7 @@
 #ifndef VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP
 #define VORTEX_ALLOCATOR_ALLOCATOR_ROS_HPP
 
-#include "thruster_allocator_auv/eigen_typedefs.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 <eigen3/Eigen/Eigen>
@@ -31,7 +31,7 @@ class ThrusterAllocator : public rclcpp::Node {
 
   /**
    * @brief Callback function for the wrench input subscription. Extracts the
-   * surge, sway and yaw values from the received wrench msg
+   * surge, sway, heave, roll, pitch and yaw values from the received wrench msg
    * and stores them in the body_frame_forces_ Eigen vector.
    * @param msg The received geometry_msgs::msg::Wrench message.
    */
@@ -44,16 +44,15 @@ class ThrusterAllocator : public rclcpp::Node {
    */
   bool healthy_wrench(const Eigen::VectorXd &v) const;
 
-  rclcpp::Publisher<vortex_msgs::msg::ThrusterForces>::SharedPtr publisher_;
-  rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr subscription_;
-  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<vortex_msgs::msg::ThrusterForces>::SharedPtr thruster_forces_publisher_;
+  rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr wrench_subscriber_;
+  rclcpp::TimerBase::SharedPtr calculate_thrust_timer_;
   size_t count_;
   int num_dof_;
   int num_thrusters_;
   int min_thrust_;
   int max_thrust_;
   Eigen::Vector6d body_frame_forces_;
-  std::vector<int64_t> direction_;
   PseudoinverseAllocator pseudoinverse_allocator_;
 };
 

diff --git a/motion/thruster_allocator_auv/include/thruster_allocator_auv/thruster_allocator_utils.hpp b/motion/thruster_allocator_auv/include/thruster_allocator_auv/thruster_allocator_utils.hpp
@@ -37,7 +37,6 @@ inline bool is_invalid_matrix(const Eigen::MatrixBase<Derived> &M) {
  */
 inline Eigen::MatrixXd calculate_right_pseudoinverse(const Eigen::MatrixXd &T) {
   Eigen::MatrixXd pseudoinverse = T.transpose() * (T * T.transpose()).inverse();
-  // pseudoinverse.completeOrthogonalDecomposition().pseudoInverse();
   if (is_invalid_matrix(pseudoinverse)) {
     throw std::runtime_error("Invalid Psuedoinverse Calculated");
   }

diff --git a/motion/thruster_allocator_auv/src/thruster_allocator_ros.cpp b/motion/thruster_allocator_auv/src/thruster_allocator_ros.cpp
@@ -27,14 +27,14 @@ ThrusterAllocator::ThrusterAllocator()
           .as_double_array(),
       num_dof_, num_thrusters_);
 
-  subscription_ = this->create_subscription<geometry_msgs::msg::Wrench>(
+  wrench_subscriber_ = this->create_subscription<geometry_msgs::msg::Wrench>(
       "thrust/wrench_input", 1,
       std::bind(&ThrusterAllocator::wrench_cb, this, std::placeholders::_1));
 
-  publisher_ = this->create_publisher<vortex_msgs::msg::ThrusterForces>(
+  thruster_forces_publisher_ = this->create_publisher<vortex_msgs::msg::ThrusterForces>(
       "thrust/thruster_forces", 1);
 
-  timer_ = this->create_wall_timer(
+  calculate_thrust_timer_ = this->create_wall_timer(
       100ms, std::bind(&ThrusterAllocator::calculate_thrust_timer_cb, this));
 
   pseudoinverse_allocator_.T_pinv =
@@ -58,7 +58,7 @@ void ThrusterAllocator::calculate_thrust_timer_cb() {
 
   vortex_msgs::msg::ThrusterForces msg_out;
   array_eigen_to_msg(thruster_forces, msg_out);
-  publisher_->publish(msg_out);
+  thruster_forces_publisher_->publish(msg_out);
 }
 
 void ThrusterAllocator::wrench_cb(const geometry_msgs::msg::Wrench &msg) {