feat: implementation of a pid dp controller

control/pid_controller_dp/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.8)
+project(pid_controller_dp)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(vortex_msgs REQUIRED)
+
+include_directories(include)
+
+add_executable(pid_controller_node
+  src/pid_controller_node.cpp
+  src/pid_controller_ros.cpp
+  src/pid_controller.cpp
+  src/pid_controller_utils.cpp
+  src/pid_controller_conversions.cpp
+)
+
+ament_target_dependencies(pid_controller_node
+  rclcpp
+  geometry_msgs
+  nav_msgs
+  Eigen3
+  tf2
+  vortex_msgs
+)
+
+install(TARGETS
+  pid_controller_node
+  DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

control/pid_controller_dp/config/pid_params.yaml

@@ -0,0 +1,8 @@
+pid_controller_node:
+  ros__parameters:
+    control_topic: /thrust/wrench_input
+    dp_reference_topic: /dp/reference
+    odom_topic: /orca/odom
+    Kp: [70.0, 70.0, 70.0, 12.0, 12.0, 12.0]
+    Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12]
+    Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0]

control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp

@@ -0,0 +1,47 @@
+#ifndef PID_CONTROLLER_HPP
+#define PID_CONTROLLER_HPP
+
+#include "pid_controller_dp/typedefs.hpp"
+
+class PIDController {
+   public:
+    explicit PIDController();
+
+    // @brief Calculate the control input tau
+    // @param eta: 7D vector containing the vehicle pose [x, y, z, w, x, y, z]
+    // @param eta_d: 7D vector containing the desired vehicle pose [x, y, z, w,
+    // x, y, z]
+    // @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r]
+    // @param eta_dot_d: 7D vector containing the desired vehicle velocity [u,
+    // v, w, p, q, r]
+    // @return 6D vector containing the control input tau [X, Y, Z, K, M, N]
+    types::Vector6d calculate_tau(const types::Eta& eta,
+                                  const types::Eta& eta_d,
+                                  const types::Nu& nu,
+                                  const types::Eta& eta_dot_d);
+
+    // @brief Set the proportional gain matrix
+    // @param Kp: 6x6 matrix containing the proportional gain matrix
+    void setKp(const types::Matrix6d& Kp);
+
+    // @brief Set the integral gain matrix
+    // @param Ki: 6x6 matrix containing the integral gain matrix
+    void setKi(const types::Matrix6d& Ki);
+
+    // @brief Set the derivative gain matrix
+    // @param Kd: 6x6 matrix containing the derivative gain matrix
+    void setKd(const types::Matrix6d& Kd);
+
+    // @brief Set the time step
+    // @param dt: Time step
+    void setTimeStep(double dt);
+
+   private:
+    types::Matrix6d Kp_;
+    types::Matrix6d Ki_;
+    types::Matrix6d Kd_;
+    types::Vector7d integral_;
+    double dt_;
+};
+
+#endif

control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp

@@ -0,0 +1,16 @@
+#ifndef PID_CONTROLLER_CONVERSIONS_HPP
+#define PID_CONTROLLER_CONVERSIONS_HPP
+
+#include <cmath>
+#include <eigen3/Eigen/Geometry>
+#include <nav_msgs/msg/odometry.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include "pid_controller_dp/typedefs.hpp"
+
+types::Eta eta_convert_from_ros_to_eigen(
+    const nav_msgs::msg::Odometry::SharedPtr msg);
+
+types::Nu nu_convert_from_ros_to_eigen(
+    const nav_msgs::msg::Odometry::SharedPtr msg);
+
+#endif

control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp

@@ -0,0 +1,93 @@
+#ifndef PID_CONTROLLER_ROS_HPP
+#define PID_CONTROLLER_ROS_HPP
+
+#include <chrono>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/wrench.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <variant>
+#include <vortex_msgs/msg/reference_filter.hpp>
+#include "pid_controller_dp/pid_controller.hpp"
+#include "pid_controller_dp/typedefs.hpp"
+
+// @brief Class for the PID controller node
+class PIDControllerNode : public rclcpp::Node {
+   public:
+    explicit PIDControllerNode();
+
+   private:
+    void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg);
+
+    void software_mode_callback(const std_msgs::msg::String::SharedPtr msg);
+
+    // @brief Callback function for the odometry topic
+    // @param msg: Odometry message containing the vehicle pose and velocity
+    void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+
+    // @brief Callback function for the proportional gain matrix
+    void publish_tau();
+
+    // @brief Set the PID controller parameters
+    void set_pid_params();
+
+    // @brief Callback function for the proportional gain matrix
+    // @param msg: Float64MultiArray message containing the proportional gain
+    // matrix
+    void kp_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
+
+    // @brief Callback function for the integral gain matrix
+    // @param msg: Float64MultiArray message containing the integral gain matrix
+    void ki_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
+
+    // @brief Callback function for the derivative gain matrix
+    // @param msg: Float64MultiArray message containing the derivative gain
+    // matrix
+    void kd_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
+
+    // @brief Callback function for the guidance topic
+    // @param msg: ReferenceFilter message containing the desired vehicle pose
+    // and velocity
+    void guidance_callback(
+        const vortex_msgs::msg::ReferenceFilter::SharedPtr msg);
+
+    PIDController pid_controller_;
+
+    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr killswitch_sub_;
+
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr software_mode_sub_;
+
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;
+
+    rclcpp::Subscription<vortex_msgs::msg::ReferenceFilter>::SharedPtr
+        guidance_sub_;
+
+    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr kp_sub_;
+
+    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr ki_sub_;
+
+    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr kd_sub_;
+
+    rclcpp::Publisher<geometry_msgs::msg::Wrench>::SharedPtr tau_pub_;
+
+    rclcpp::TimerBase::SharedPtr tau_pub_timer_;
+
+    std::chrono::milliseconds time_step_;
+
+    types::Eta eta_;
+
+    types::Eta eta_d_;
+
+    types::Nu nu_;
+
+    types::Eta eta_dot_d_;
+
+    bool killswitch_on_;
+
+    std::string software_mode_;
+};
+
+#endif

control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp

@@ -0,0 +1,87 @@
+#ifndef PID_UTILS_HPP
+#define PID_UTILS_HPP
+
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <cmath>
+#include <eigen3/Eigen/Geometry>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include "pid_controller_dp/typedefs.hpp"
+
+// @brief Calculate the sine of an angle in degrees
+// @param angle: Angle in degrees
+// @return Smallest sine angle of the input
+double ssa(double angle);
+
+// @brief Convert a Float64MultiArray message to a 6x6 diagonal matrix
+// @param msg: Float64MultiArray message containing 6 elements
+// @return 6x6 diagonal matrix with the elements of the message on the diagonal
+types::Matrix6d float64multiarray_to_diagonal_matrix6d(
+    const std_msgs::msg::Float64MultiArray& msg);
+
+// @brief Calculate the rotation matrix from a quaternion
+// @param q: Quaternion represented as a 4D vector [w, x, y, z]
+// @return 3x3 rotation matrix
+// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021
+// p.34 eq: 2.72
+types::Matrix3d calculate_R_quat(const types::Eta& eta);
+
+// @brief Calculate the transformation matrix from a quaternion
+// @param q: Quaternion represented as a 4D vector [w, x, y, z]
+// @return 4x3 transformation matrix
+// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021
+// p.35 eq: 2.78
+types::Matrix4x3d calculate_T_quat(const types::Eta& eta);
+
+// @brief Calculate the Jacobian matrix
+// @param eta: 7D vector containing the vehicle pose [x, y, z, w, x, y, z]
+// @return 7x6 Jacobian matrix
+// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021
+// p.36 eq: 2.83
+types::J_transformation calculate_J(const types::Eta& eta);
+
+// @brief Calculate the pseudo-inverse of the Jacobian matrix
+// @param eta: 7D vector containing the vehicle pose [x, y, z, w, x, y, z]
+// @return 6x7 pseudo-inverse Jacobian matrix
+// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021
+// p.34 eq: 2.72
+types::Matrix6x7d calculate_J_sudo_inv(const types::Eta& eta);
+
+// @brief Calculate the error between the desired and actual vehicle pose
+// @param eta: 7D vector containing the actual vehicle pose [x, y, z, w, x, y,
+// z]
+// @param eta_d: 7D vector containing the desired vehicle pose [x, y, z, w, x,
+// y, z]
+// @return 7D vector containing the error between the desired and actual vehicle
+// pose
+
+types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d);
+
+// @brief Clamp the values between a minimum and maximum value
+// @param values: Vector containing the values to be clamped
+// @param min_val: Minimum value
+// @param max_val: Maximum value
+// @return Vector containing the clamped values
+Eigen::VectorXd clamp_values(const Eigen::VectorXd& values,
+                             double min_val,
+                             double max_val);
+
+// @brief Calculate the anti-windup term
+// @param dt: Time step
+// @param error: 7D vector containing the error between the desired and
+// actual vehicle pose [x, y, z, w, x, y, z]
+// @param integral: 7D vector containing the integral term of the PID
+// controller [x, y, z, w, x, y, z]
+// @return 7D vector containing the anti-windup term
+types::Vector7d anti_windup(const double dt,
+                            const types::Eta& error,
+                            const types::Vector7d& integral);
+
+// @brief Limit the input to the PID controller
+// @param input: 6D vector containing the input to the PID controller [u, v, w,
+// p, q, r]
+// @return 6D vector containing the limited input to the PID controller [u, v,
+// w, p, q, r]
+types::Vector6d limit_input(const types::Vector6d& input);
+
+#endif

control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp

@@ -0,0 +1,59 @@
+/**
+ * @file eigen_typedefs.hpp
+ * @brief Contains the typedef for a 6x1 Eigen vector and a 6x6 Eigen matrix.
+ */
+
+#ifndef VORTEX_EIGEN_TYPEDEFS_H
+#define VORTEX_EIGEN_TYPEDEFS_H
+
+#include <eigen3/Eigen/Dense>
+
+namespace types {
+typedef Eigen::Matrix<double, 3, 1> Vector3d;
+typedef Eigen::Matrix<double, 6, 1> Vector6d;
+typedef Eigen::Matrix<double, 7, 1> Vector7d;
+typedef Eigen::Matrix<double, 4, 1> Vector4d;
+typedef Eigen::Matrix<double, 6, 6> Matrix6d;
+typedef Eigen::Matrix<double, 3, 3> Matrix3d;
+typedef Eigen::Matrix<double, 4, 3> Matrix4x3d;
+typedef Eigen::Matrix<double, 7, 6> Matrix7x6d;
+typedef Eigen::Matrix<double, 6, 7> Matrix6x7d;
+typedef Eigen::Matrix<double, 7, 7> Matrix7d;
+typedef Eigen::Quaterniond Quaterniond;
+
+struct Eta {
+    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
+    Eigen::Quaterniond ori = Eigen::Quaterniond::Identity();
+
+    types::Vector7d as_vector() const {
+        types::Vector7d vec;
+        vec << pos, ori.w(), ori.x(), ori.y(), ori.z();
+        return vec;
+    }
+};
+
+struct Nu {
+    Eigen::Vector3d linear_speed = types::Vector3d::Zero();
+    Eigen::Vector3d angular_speed = types::Vector3d::Zero();
+
+    types::Vector6d as_vector() const {
+        types::Vector6d vec;
+        vec << linear_speed, angular_speed;
+        return vec;
+    }
+};
+
+struct J_transformation {
+    Eigen::Matrix3d R = types::Matrix3d::Identity();
+    types::Matrix4x3d T = types::Matrix4x3d::Zero();
+
+    types::Matrix7x6d as_matrix() const {
+        types::Matrix7x6d mat = types::Matrix7x6d::Zero();
+        mat.block<3, 3>(0, 0) = R;
+        mat.block<4, 3>(3, 3) = T;
+        return mat;
+    }
+};
+}  // namespace types
+
+#endif

control/pid_controller_dp/launch/pid_controller_dp.launch.py

@@ -0,0 +1,22 @@
+from os import path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+pid_params = path.join(
+    get_package_share_directory("pid_controller_dp"), "config", "pid_params.yaml"
+)
+
+
+def generate_launch_description():
+    pid_controller_node = Node(
+        package="pid_controller_dp",
+        executable="pid_controller_node",
+        name="pid_controller_node",
+        parameters=[
+            pid_params,
+        ],
+        output="screen",
+    )
+    return LaunchDescription([pid_controller_node])