feat/fix: PID quaternion-based DP controller

.gitignore

@@ -59,3 +59,8 @@ qtcreator-*
 .#*
 
 # End of https://www.gitignore.io/api/ros
+control/pid_controller_dp/test/test_main.cpp
+control/pid_controller_dp/test/test_pid_basic.cpp
+control/pid_controller_dp/test/test_pid_controller.cpp
+control/pid_controller_dp/test/test_type_casting.cpp
+scripts/ci_install_dependencies.sh

control/pid_controller_dp/CMakeLists.txt

@@ -16,28 +16,74 @@ find_package(geometry_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(vortex_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 find_package(vortex_utils REQUIRED)
+find_package(spdlog REQUIRED)
+find_package(fmt REQUIRED)
 find_package(vortex_utils_ros REQUIRED)
 
 include_directories(include)
+set(LIB_NAME ${PROJECT_NAME}_lib)
 
-add_executable(pid_controller_node
-  src/pid_controller_node.cpp
-  src/pid_controller_ros.cpp
+add_library(${LIB_NAME} SHARED
   src/pid_controller.cpp
   src/pid_controller_utils.cpp
   src/pid_controller_conversions.cpp
 )
 
+ament_target_dependencies(${LIB_NAME} PUBLIC
+  rclcpp
+  geometry_msgs
+  nav_msgs
+  Eigen3
+  tf2
+  vortex_msgs
+  rcl_interfaces
+  vortex_utils
+  vortex_utils_ros
+  spdlog
+  fmt
+)
+
+
+install(TARGETS
+  ${LIB_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+add_executable(pid_controller_node
+  src/pid_controller_node.cpp
+  src/pid_controller_ros.cpp
+)
+
 ament_target_dependencies(pid_controller_node
   rclcpp
   geometry_msgs
   nav_msgs
   Eigen3
   tf2
   vortex_msgs
+  rcl_interfaces
   vortex_utils
   vortex_utils_ros
+  spdlog
+  fmt
+)
+
+target_link_libraries(
+    pid_controller_node
+    ${LIB_NAME}
+    spdlog::spdlog
+    # vortex_utilis::vortex_utils
+)
+
+ament_export_targets(export_${LIB_NAME})
+
+install(TARGETS ${LIB_NAME}
+  EXPORT export_${LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
 install(TARGETS
@@ -50,4 +96,9 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
 ament_package()

control/pid_controller_dp/README.md

@@ -1,7 +1,94 @@
-## PID controller
+# PID controller
+
 The PID controller is defined
+
 ```math
 \tau = -J_{q}^{\dagger}(K_p \tilde{\eta} + K_d \dot{\tilde{\eta}} + K_i \int^t_0 \tilde{\eta}(\tau)d\tau)
 ```
 
-where $\tau$ is the control input, $\tilde{\eta} = \eta - \eta_d$ is the pose error, $J_q$ is the quaternion based Jacobian matrix and $K_p$, $K_d$ and $K_i$ are tuning matrices.
+where:
+
+- $\tilde{\eta} = \eta - \eta_d$ is the pose error (7D quaternion representation),
+- $J_q$ is the quaternion Jacobian (7×6), and $J_q^{\dagger}$ is its (pseudo-)inverse,
+- $K_p$, $K_d$, $K_i$ are 6×6 gain matrices.
+
+## PID controller (pid_controller_dp)
+
+This package implements a 6-DOF PID controller that operates on the
+6-dimensional control vector
+$$\tau = [X, Y, Z, K, M, N]^T$$
+and uses a quaternion-based 7D pose representation for attitude.
+
+## Build
+
+
+This package is built as part of the workspace. From the workspace root:
+
+```bash
+colcon build --packages-select pid_controller_dp 
+```
+
+To run tests for this package only:
+
+```bash
+colcon test --packages-select pid_controller_dp && colcon test-result --verbose
+```
+
+## Usage (ROS 2 node)
+
+The package provides a node `pid_controller_node` that subscribes to pose,
+twist and guidance topics and publishes wrench (tau) commands.
+
+
+- `topics.pose` (type: `geometry_msgs/PoseWithCovarianceStamped`) — vehicle pose input
+- `topics.twist` (type: `geometry_msgs/TwistWithCovarianceStamped`) — velocity input
+- `topics.guidance.dp` (type: `vortex_msgs/ReferenceFilter`) — desired states (pose/vecocity)
+- `topics.wrench_input` (type: `geometry_msgs/WrenchStamped`) — output wrench
+
+Parameters expose PID gains (Kp, Ki, Kd) as per-component values which are
+assembled into diagonal gain matrices inside the node. See the node source
+(`src/pid_controller_ros.cpp`) for parameter names.
+
+## Examples
+
+Start the node (after sourcing workspace):
+
+1. Run the simulation
+
+ ```bash
+ ros2 launch stonefish_sim simulation.launch.py scenario:=default
+ ```
+
+2. Run the thrust allocation node:
+
+ ```bash
+ ros2 launch thrust_allocator_auv thrust_allocator_auv.launch.py
+ ```
+
+3. To move the robot, run the joystick node
+
+ ```bash
+ ros2 launch stonefish_sim orca_sim.launch.py
+ ```
+
+4. Run the controller
+
+ ```bash
+ ros2 launch  pid_controller_dp pid_controller_dp.launch.py
+ ```
+
+Use the joy stick to move the robot. The key mappings are:
+
+- B - kill
+- Y - autonomous mode (reference model)
+- A - manual mode
+
+Note: When plotting, the axis plotted and actual command might not align since the plotting is based on the joy controller frame (`odom`), whereas the controller works on the robot frame (`body_frame`)
+
+## Tuning
+
+The `rqt_reconfigure` can be used to change the controller gains.
+
+```bash
+ros2 run rqt_reconfigure rqt_reconfigure 
+```

control/pid_controller_dp/config/pid_params.yaml

@@ -1,5 +1,20 @@
 /**:
   ros__parameters:
-    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]
+    Kp_x: 20.0
+    Kp_y: 26.0
+    Kp_z: 50.0
+    Kp_roll: 10.0
+    Kp_pitch: 41.0
+    Kp_yaw: 6.0
+    Ki_x: 0.092
+    Ki_y: 0.00059
+    Ki_z: 0.085
+    Ki_roll: 0.002
+    Ki_pitch: 0.01
+    Ki_yaw: 0.0003
+    Kd_x: 0.001
+    Kd_y: 0.0
+    Kd_z: 0.0
+    Kd_roll: 0.002
+    Kd_pitch: 0.0
+    Kd_yaw: 0.001

control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp

@@ -1,6 +1,7 @@
 #ifndef PID_CONTROLLER_DP__PID_CONTROLLER_HPP_
 #define PID_CONTROLLER_DP__PID_CONTROLLER_HPP_
 
+#include <spdlog/spdlog.h>
 #include "pid_controller_dp/typedefs.hpp"
 
 class PIDController {
@@ -36,6 +37,10 @@ class PIDController {
     // @param dt: Time step
     void set_time_step(double dt);
 
+    types::Matrix6d get_kp();
+    types::Matrix6d get_ki();
+    types::Matrix6d get_kd();
+
    private:
     types::Matrix6d Kp_;
     types::Matrix6d Ki_;

control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp

@@ -3,16 +3,16 @@
 
 #include <cmath>
 #include <eigen3/Eigen/Geometry>
-#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance.hpp>
+#include <geometry_msgs/msg/twist_with_covariance.hpp>
 #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 geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+    const geometry_msgs::msg::PoseWithCovariance& msg);
 
 types::Nu nu_convert_from_ros_to_eigen(
-    const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
+    const geometry_msgs::msg::TwistWithCovariance& msg);
 
 #endif  // PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_

control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp

@@ -7,6 +7,7 @@
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/float64_multi_array.hpp>
@@ -35,16 +36,6 @@ class PIDControllerNode : public rclcpp::Node {
     void operation_mode_callback(
         const vortex_msgs::msg::OperationMode::SharedPtr msg);
 
-    // @brief Callback function for the pose topic
-    // @param msg: PoseWithCovarianceStamped message containing the AUV pose
-    void pose_callback(
-        const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
-
-    // @brief Callback function for the twist topic
-    // @param msg: TwistWithCovarianceStamped message containing the AUV speed
-    void twist_callback(
-        const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
-
     // @brief Callback function for the tau publisher timer
     void publish_tau();
 
@@ -64,6 +55,15 @@ class PIDControllerNode : public rclcpp::Node {
     void guidance_callback(
         const vortex_msgs::msg::ReferenceFilter::SharedPtr msg);
 
+    // @brief Callback function for the odometry topic
+    // @param msg: Odometry message containing the AUV pose and speed
+    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+
+    // @brief Callback function for parameter updates
+    // @param parameters: vector of parameters to be set
+    rcl_interfaces::msg::SetParametersResult parametersCallback(
+        const std::vector<rclcpp::Parameter>& parameters);
+
     rclcpp::Client<vortex_msgs::srv::GetOperationMode>::SharedPtr
         get_operation_mode_client_;
 
@@ -74,11 +74,7 @@ class PIDControllerNode : public rclcpp::Node {
     rclcpp::Subscription<vortex_msgs::msg::OperationMode>::SharedPtr
         operation_mode_sub_;
 
-    rclcpp::Subscription<
-        geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
-
-    rclcpp::Subscription<
-        geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
 
     rclcpp::Subscription<vortex_msgs::msg::ReferenceFilter>::SharedPtr
         guidance_sub_;
@@ -107,6 +103,8 @@ class PIDControllerNode : public rclcpp::Node {
 
     vortex::utils::types::Mode operation_mode_{
         vortex::utils::types::Mode::manual};
+
+    OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 };
 
 #endif  // PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_

control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp

@@ -1,12 +1,15 @@
 #ifndef PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_
 #define PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_
 
+#include <spdlog/spdlog.h>
 #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 <vortex/utils/types.hpp>
 #include "pid_controller_dp/typedefs.hpp"
+#include "typedefs.hpp"
 
 // @brief Calculate the sine of an angle in degrees
 // @param angle: Angle in degrees