feat: added a new main.cpp file with ros2 node class

vortexntnu · Talhanc · Sep 27, 2024 · Oct 11, 2024 · Oct 11, 2024 · Oct 13, 2024
commit 76249d7552c3f054392c01a114cec1f404e540ac
diff --git a/control/dp_controller/include/dp_controller/main.hpp b/control/dp_controller/include/dp_controller/main.hpp
@@ -8,6 +8,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
 #include <iostream>
 
 class SMC_node : public rclcpp::Node {
@@ -20,6 +21,22 @@ class SMC_node : public rclcpp::Node {
 
   void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_subscription_;
+
+  // Helper functions
+  std::tuple<Eigen::VectorXd, Eigen::VectorXd> format_eta(const nav_msgs::msg::Odometry::SharedPtr msg);
+  Eigen::Vector3d quaternionToEuler(double w, double x, double y, double z);
+  double ssa(const double a2, const double a1);
+  Eigen::Vector3d ssa_vector(const Eigen::Vector3d &Desired, const Eigen::Vector3d &Current);
+  Eigen::VectorXd vector_difference(const Eigen::VectorXd &Current, const Eigen::VectorXd &Desired);
+  Eigen::VectorXd tau_PID(double dt, const Eigen::VectorXd &nu, const Eigen::VectorXd &eta, const Eigen::VectorXd &eta_d);
+  Eigen::VectorXd run_u(const nav_msgs::msg::Odometry::SharedPtr msg, const Eigen::VectorXd &d_eta, const Eigen::VectorXd &d_nu, double dt);
+  geometry_msgs::msg::Wrench convert_to_wrench(const Eigen::VectorXd &vec);
+
+  // Control variables
+  Eigen::VectorXd eta_e_prev_global;
+  Eigen::VectorXd integral_eta_e_global;
+  rclcpp::Time prev_time;
+
 };
 
 #endif
diff --git a/control/dp_controller/src/cpp_functions.cpp b/control/dp_controller/src/cpp_functions.cpp