diff --git a/auv_setup/README.md b/auv_setup/README.md index ead2200b4..d77915149 100644 --- a/auv_setup/README.md +++ b/auv_setup/README.md @@ -10,8 +10,64 @@ The config folder contains physical parameters related to the AUV and the enviro * thrusters: Thruster configs for different thruster types ### Launch -This package contains a launchfile for each specific AUV. Additionally the topside.launch file is to -be used on the topside computer that the joystick is connected to, for ROV operations. + +This package contains launchfiles for each specific AUV. Additionally `topside.launch.py` is used on the topside computer that the joystick is connected to, for ROV operations. + +```bash +ros2 launch auv_setup topside.launch.py --orientation_mode:=your_argument +``` + +| Argument | Default | Description | +|---|---|---| +| `orientation_mode` |`euler`| Types to choose from `euler` or `quat` | + + + +**`adaptive`** — launches `dp_adapt_backs_controller` with `reference_filter_dp` (Euler/RPY reference filter). Controller params are loaded from `dp_adapt_backs_controller/config/adapt_params_.yaml`. + +```bash +ros2 launch auv_setup dp.launch.py controller_type:=adaptive drone:=nautilus +``` + +**`adaptive_quat`** — launches `dp_adapt_backs_controller` with `reference_filter_dp` (Quaternion reference filter). Controller params are loaded from `dp_adapt_backs_controller_quat/config/adapt_params_.yaml`. + +```bash +ros2 launch auv_setup dp_quat.launch.py controller_type:=adaptive drone:=nautilus +``` + +#### dp.launch.py or dp_quat.launch.py + +Both launch the DP (dynamic positioning) stack, consisting of a reference filter and a controller launched together in a composable node container to limit intra process communication and allow for shared memory. + +```bash +ros2 launch auv_setup dp.launch.py +``` + +| Argument | Default | Description | +|---|---|---| +| `drone` | `nautilus` | Drone model — loads `auv_setup/config/robots/.yaml` | +| `namespace` | `` | ROS namespace. Defaults to the drone name if left empty | +| `controller_type` | `adaptive` | Controller to use: `adaptive`, `adaptive_quat` or `pid` | + +**`adaptive`** — launches `dp_adapt_backs_controller` with `reference_filter_dp` (Euler/RPY reference filter). Controller params are loaded from `dp_adapt_backs_controller/config/adapt_params_.yaml`. + +```bash +ros2 launch auv_setup dp.launch.py controller_type:=adaptive drone:=nautilus +``` + +**`adaptive_quat`** — launches `dp_adapt_backs_controller` with `reference_filter_dp` (Quaternion reference filter). Controller params are loaded from `dp_adapt_backs_controller_quat/config/adapt_params_.yaml`. + +```bash +ros2 launch auv_setup dp_quat.launch.py controller_type:=adaptive drone:=nautilus +``` + +**`pid`** — launches `pid_controller_dp` with `reference_filter_dp_quat` (quaternion reference filter). Controller params are loaded from `pid_controller_dp/config/pid_params.yaml`. + +```bash +ros2 launch auv_setup dp.launch.py controller_type:=pid drone:=nautilus +``` + +> When using the joystick to send references, make sure `orientation_mode` in `joystick_interface_auv` matches the controller: use `euler` with `adaptive` and `quat` with `pid`. ### Description The description folder contains the URDF and xacro files for the AUVs. The main description launch file is drone_description.launch.py, which makes all static transforms available to the ros graph. diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 61fa6b061..41bbf4377 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -16,7 +16,7 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.01] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch + center_of_mass: [0.0, 0.0, 0.025] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: @@ -53,22 +53,22 @@ 0.00000, # Heave ] thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0) - 0.413892, - 0.140095, - -0.163904, - -0.413892, - -0.413892, - -0.163904, - 0.140095, - 0.413892, # x-positions of the thrusters + 0.45015, + 0.24060, + -0.22970, + -0.43861, + -0.43861, + -0.22970, + 0.240600, + 0.450150, # x-positions of the thrusters + 0.305680, 0.313022, 0.313022, - 0.313022, - 0.313022, - -0.313022, + 0.305680, + -0.305680, -0.313022, -0.313022, - -0.313022, # y-positions of the thrusters + -0.305680, # y-positions of the thrusters 0.021736, 0.021736, 0.021736, @@ -80,8 +80,8 @@ ] constraints: - max_force: 40.0 - min_force: -40.0 + max_force: 39.0 + min_force: -41.0 input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0] slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0] @@ -110,7 +110,7 @@ waypoint: "waypoint" guidance: los: "guidance/los" - dp: "guidance/dp" + dp_rpy: "guidance/dp_rpy" dp_quat: "guidance/dp_quat" dvl_twist: "dvl/twist" dvl_altitude: "dvl/altitude" diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 93425c34f..d23dd352c 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -134,7 +134,7 @@ waypoint: "waypoint" guidance: los: "guidance/los" - dp: "guidance/dp" + dp_rpy: "guidance/dp_rpy" dp_quat: "guidance/dp_quat" fsm: active_controller: "fsm/active_controller" diff --git a/auv_setup/launch/dp_quat.launch.py b/auv_setup/launch/dp_quat.launch.py new file mode 100644 index 000000000..b0a50c925 --- /dev/null +++ b/auv_setup/launch/dp_quat.launch.py @@ -0,0 +1,72 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + filter_config = os.path.join( + get_package_share_directory("reference_filter_dp_quat"), + "config", + "reference_filter_params.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + adapt_params = os.path.join( + get_package_share_directory("dp_adapt_backs_controller_quat"), + "config", + f"adapt_params_{drone}.yaml", + ) + + container = ComposableNodeContainer( + name="dp_quat_container", + namespace=namespace, + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="reference_filter_dp_quat", + plugin="ReferenceFilterNode", + name="reference_filter_node", + namespace=namespace, + parameters=[filter_config, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="dp_adapt_backs_controller_quat", + plugin="DPAdaptBacksControllerNode", + name="dp_adapt_backs_controller_node", + namespace=namespace, + parameters=[adapt_params, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + ) + + return [container] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + + [ + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp index 8e6c884ce..4c8e2e548 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp @@ -40,9 +40,9 @@ void DPAdaptBacksControllerNode::set_subscribers_and_publisher() { vortex::utils::qos_profiles::sensor_data_profile(1)}; const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)}; - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_rpy"); std::string dp_reference_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_rpy").as_string(); guidance_sub_ = this->create_subscription( dp_reference_topic, qos_sensor_data, @@ -234,10 +234,7 @@ void DPAdaptBacksControllerNode::publish_tau() { tau_msg.wrench.force.x = tau(0); tau_msg.wrench.force.y = tau(1); tau_msg.wrench.force.z = tau(2); - - // comment out if roll control is not needed tau_msg.wrench.torque.x = tau(3); - tau_msg.wrench.torque.y = tau(4); tau_msg.wrench.torque.z = tau(5); diff --git a/control/dp_adapt_backs_controller_quat/CMakeLists.txt b/control/dp_adapt_backs_controller_quat/CMakeLists.txt new file mode 100644 index 000000000..0556acc81 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/CMakeLists.txt @@ -0,0 +1,72 @@ +cmake_minimum_required(VERSION 3.8) +project(dp_adapt_backs_controller_quat) +set(CMAKE_CXX_STANDARD 20) + +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(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(fmt REQUIRED) +find_package(spdlog REQUIRED) + + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/dp_adapt_backs_controller.cpp + src/dp_adapt_backs_controller_ros.cpp + src/dp_adapt_backs_controller_utils.cpp) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + geometry_msgs + Eigen3 + fmt + spdlog + vortex_msgs + vortex_utils + vortex_utils_ros +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "DPAdaptBacksControllerNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/control/dp_adapt_backs_controller_quat/README.md b/control/dp_adapt_backs_controller_quat/README.md new file mode 100644 index 000000000..14c0dd3cb --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/README.md @@ -0,0 +1,334 @@ +## DP Adaptive Backstepping Controller — Quaternion Formulation + +This package implements a dynamic positioning (DP) adaptive backstepping controller for the Nautilus AUV. It uses a **unit-quaternion error-state formulation** for the attitude kinematics, which eliminates the gimbal-lock singularity that occurs at ±90° pitch in the Euler-angle version. The adaptive terms estimate the linear and quadratic damping online and compensate for unmodelled disturbances, in the same spirit as a model reference adaptive controller. Stability and convergence are proven via a composite Lyapunov function. + +### Overview +- Uses the backstepping control method for position and orientation control +- Replaces the Euler-angle Jacobian $J(\eta)$ with the **quaternion error-state Jacobian** $J_e(\eta)$ (called `L` in the code), which is square, smooth, and invertible for all attitude errors smaller than 180° +- Includes adaptive terms to estimate linear and nonlinear (quadratic) damping and external disturbances + +### Model for AUV + +**Kinematics (error-state form):** + +```math +\dot{z}_1 = J_e(\eta)\,\nu +``` + +where the error state $z_1 \in \mathbb{R}^6$ is defined below, and the error-state Jacobian is: + +```math +J_e(\eta) = +\begin{bmatrix} +R(q) & 0_{3\times 3} \\ +0_{3\times 3} & T_e(q_e) +\end{bmatrix}, \quad +T_e(q_e) = \eta_e I_3 + S(\varepsilon_e) +``` + +Here $R(q) \in SO(3)$ is the rotation matrix from NED to body, $q_e = q_d^* \otimes q$ is the error quaternion (scalar part $\eta_e$, vector part $\varepsilon_e$), and $S(\cdot)$ is the skew-symmetric (cross-product) matrix. + +**Dynamics (Newton–Euler, body frame):** + +```math +M\dot{\nu} + C(\nu)\,\nu - F(\nu,\Theta^\star) = \tau + d^\star +``` + +- $\nu \in \mathbb{R}^6$: body-fixed velocity (linear and angular) +- $M$: constant, symmetric, positive-definite mass-inertia matrix +- $C(\nu)$: Coriolis and centripetal matrix +- $F(\nu, \Theta^\star) = Y(\nu)\Theta^\star$: damping in regressor form (linear and quadratic per DOF) +- $\tau$: control wrench +- $d^\star$: lumped disturbances and unmodelled dynamics + +**Notation note:** The symbol $\eta_e$ denotes the scalar part of the error quaternion (not the pose vector $\eta$); this follows Fossen's standard overloading. + +### File overview + +1. **dp_adapt_backs_controller.cpp/hpp** + - Core controller implementation: computes `L`, `L_inv`, `L_dot`, the error state $z_1$, $z_2$, $\alpha$, $\dot\alpha$, and the full control wrench $\tau$. + - Integrates the adaptive parameters online. + +2. **dp_adapt_backs_controller_utils.cpp/hpp** + - Utility functions: `calculate_L_inv`, `calculate_R_dot`, `calculate_Q_dot`, `calculate_L_dot`, `calculate_coriolis`, `calculate_Y_v`. + +3. **dp_adapt_backs_controller_ros.cpp/hpp** + - ROS 2 node wrapper: subscribes to odometry, killswitch, and reference topics; publishes thrust commands. + +4. **adapt_params_nautilus.yaml / adapt_params_nautilus_sim.yaml** + - Tunable controller parameters (`K1`, `K2`, `adapt_gain`, `d_gain`, `r_b_bg`, `time_step`). + +5. **CMakeLists.txt** + - Build configuration, ROS 2 dependencies, executable generation, and installation setup. + +### Tuning Parameters +- **K1**: Outer loop gain matrix (position and orientation errors $z_1$). +- **K2**: Inner loop gain matrix (velocity error $z_2$). +- **adapt\_gain**: Diagonal adaptation rate for the 12 damping parameters ($\Gamma_\theta$). +- **d\_gain**: Diagonal adaptation rate for the 6 disturbance estimates ($\Gamma_d$). +- **r\_b\_bg**: Vector from body origin to centre of gravity (used in the Coriolis matrix). + +## Backstepping Controller + +### Error state and backstepping variables + +The orientation error quaternion is formed by left-multiplication with the desired quaternion conjugate: + +```math +q_e = q_d^* \otimes q = \begin{bmatrix} \eta_e \\ \varepsilon_e \end{bmatrix} +``` + +The tracking error in $\mathbb{R}^6$ is then: + +```math +z_1 = \begin{bmatrix} p - p_d \\ 2\varepsilon_e \end{bmatrix} +``` + +The factor of 2 on the vector part of the error quaternion ensures $\dot{z}_{1,\text{ori}} = T_e(q_e)\,\omega$ and makes the LFC derivative tractable. The velocity error is: + +```math +z_2 = \nu - \alpha +``` + +where $\alpha$ is the virtual control law defined below. + +### Adaptive parameters + +```math +\tilde{\Theta} = \hat{\Theta} - \Theta^\star, \qquad \tilde{d} = \hat{d} - d^\star +``` + +where: +- $\Theta^\star$ and $d^\star$ are the (unknown) true parameters +- $\hat{\Theta}$ and $\hat{d}$ are online estimates +- $\tilde{\Theta}$ and $\tilde{d}$ are the estimation errors + +### Proof of control law + +#### Step 1 — Outer loop (position and attitude) + +Define the LFC: + +```math +V_1 = \frac{1}{2} z_1^\top z_1 +``` + +which is positive definite, radially unbounded, and satisfies $V_1(0) = 0$. For a constant setpoint ($\dot{\eta}_d = 0$): + +```math +\dot{V}_1 = z_1^\top \dot{z}_1 = z_1^\top J_e(\eta)\,\nu +``` + +Treating $\nu$ as a virtual input (Khalil §14.3) and splitting $\nu = \alpha + z_2$: + +```math +\dot{V}_1 = z_1^\top J_e\,\alpha + z_1^\top J_e\,z_2 +``` + +Choose the virtual control law: + +```math +\boxed{ +\alpha = -J_e(\eta)^{-1} K_1\, z_1, \quad K_1 = K_1^\top > 0 +} +``` + +Then $z_1^\top J_e\,\alpha = -z_1^\top K_1 z_1 < 0$ and: + +```math +\dot{V}_1 = -z_1^\top K_1 z_1 + z_1^\top J_e\, z_2 +``` + +The cross term $z_1^\top J_e z_2$ will be cancelled in Step 2. + +#### Step 2 — Inner loop (velocity) + +Augment the LFC with the inertia-weighted velocity term (Fossen 2021, §12.1): + +```math +V_2 = \frac{1}{2} z_2^\top M\, z_2, \quad M = M^\top > 0,\; \dot{M} = 0 +``` + +Differentiating and substituting the dynamics: + +```math +\dot{V}_2 = z_2^\top M\,(\dot{\nu} - \dot{\alpha}) = z_2^\top\bigl(\tau - C(\nu)\nu + Y(\nu)\Theta^\star + d^\star - M\dot{\alpha}\bigr) +``` + +**Cross-term cancellation.** The scalar $z_1^\top J_e z_2 = z_2^\top J_e^\top z_1$. If the control law contains $-J_e^\top z_1$, then in $\dot{V}_1 + \dot{V}_2$: + +```math +z_1^\top J_e\, z_2 + z_2^\top(-J_e^\top z_1) = z_2^\top J_e^\top z_1 - z_2^\top J_e^\top z_1 = 0 +``` + +The cross terms cancel exactly, independent of the structure of $J_e$. + +#### Adaptive extension + +Since $\Theta^\star$ and $d^\star$ are unknown, form the composite LFC: + +```math +V = V_1 + V_2 + \frac{1}{2}\tilde{\Theta}^\top \Gamma^{-1}_{\theta}\,\tilde{\Theta} + \frac{1}{2}\tilde{d}^\top \Gamma^{-1}_{d}\,\tilde{d} +``` + +Assuming $\dot{\Theta}^\star = 0$ and $\dot{d}^\star = 0$ (static true parameters): + +```math +\dot{V} = \dot{V}_1 + z_2^\top M(\dot{\nu} - \dot{\alpha}) + \tilde{\Theta}^\top \Gamma^{-1}_{\theta}\,\dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d}\,\dot{\hat{d}} +``` + +Substituting the control law: + +```math +\tau = -J_e^\top z_1 - K_2\, z_2 + M\dot{\alpha} + C(\nu)\nu - Y(\nu)\hat{\Theta} - \hat{d} +``` + +and collecting terms (cross terms cancel, $C(\nu)\nu$ cancels, $M\dot\alpha$ cancels): + +```math +\dot{V} = -z_1^\top K_1 z_1 - z_2^\top K_2 z_2 ++ \tilde{\Theta}^\top\!\left(\Gamma_\theta^{-1}\dot{\hat{\Theta}} - Y(\nu)^\top z_2\right) ++ \tilde{d}^\top\!\left(\Gamma_d^{-1}\dot{\hat{d}} - z_2\right) +``` + +From this we can separate the adaptive terms: + +```math +\tilde{\Theta}^\top\!\left(\Gamma_\theta^{-1}\dot{\hat{\Theta}} - Y(\nu)^\top z_2\right) = \tilde{\Theta}^\top \Gamma_\theta^{-1}\!\left(\dot{\hat{\Theta}} - \Gamma_\theta Y(\nu)^\top z_2\right) +``` + +```math +\tilde{d}^\top\!\left(\Gamma_d^{-1}\dot{\hat{d}} - z_2\right) = \tilde{d}^\top \Gamma_d^{-1}\!\left(\dot{\hat{d}} - \Gamma_d z_2\right) +``` + +Choosing the update laws to zero these brackets: + +```math +\boxed{ +\dot{\hat{\Theta}} = \Gamma_{\theta}\, Y(\nu)^\top z_2 +} +``` + +```math +\boxed{ +\dot{\hat{d}} = \Gamma_{d}\, z_2 +} +``` + +This gives the final Lyapunov derivative: + +```math +\dot{V} = -z_1^\top K_1 z_1 - z_2^\top K_2 z_2 < 0, \quad \forall\,(z_1,z_2) \neq 0 +``` + +Global asymptotic stability of $z_1 = 0$, $z_2 = 0$ follows from LaSalle's invariance principle (Khalil §4.2), with parameter estimates remaining bounded by the adaptive law structure. + +### Full control law + +```math +\boxed{ +\tau = -J_e^\top z_1 - K_2\, z_2 + M\dot{\alpha} + C(\nu)\nu - Y(\nu)\hat{\Theta} - \hat{d} +} +``` + +### Controller gains + +**$K_1$** is the outer loop gain (position and orientation errors): + +```math +K_1 = +\begin{bmatrix} +k_{1,1} & & & & & \\ +& k_{1,2} & & & & \\ +& & k_{1,3} & & & \\ +& & & k_{1,4} & & \\ +& & & & k_{1,5} & \\ +& & & & & k_{1,6} +\end{bmatrix} +``` + +**$K_2$** is the inner loop gain (velocity errors): + +```math +K_2 = +\begin{bmatrix} +k_{2,1} & & & & & \\ +& k_{2,2} & & & & \\ +& & k_{2,3} & & & \\ +& & & k_{2,4} & & \\ +& & & & k_{2,5} & \\ +& & & & & k_{2,6} +\end{bmatrix} +``` + +### Adaptive parameters and functions + +The damping regressor $Y(\nu) \in \mathbb{R}^{6 \times 12}$ captures one linear and one quadratic term per DOF: + +```math +Y(\nu) = +\begin{bmatrix} +\nu_1 & \nu_1|\nu_1| & 0 & 0 & \cdots & 0 & 0 \\ +0 & 0 & \nu_2 & \nu_2|\nu_2| & \cdots & 0 & 0 \\ +\vdots & & & & \ddots & & \vdots \\ +0 & 0 & 0 & 0 & \cdots & \nu_6 & \nu_6|\nu_6| +\end{bmatrix} +``` + +The parameter vector $\hat{\Theta} \in \mathbb{R}^{12}$ is: + +```math +\hat{\Theta} = +\begin{bmatrix} +\alpha_1 & \beta_1 & \alpha_2 & \beta_2 & \alpha_3 & \beta_3 & \alpha_4 & \beta_4 & \alpha_5 & \beta_5 & \alpha_6 & \beta_6 +\end{bmatrix}^\top +``` + +where $\alpha_i$ and $\beta_i$ are the estimated linear and quadratic damping coefficients for DOF $i$. The adaptation gain $\Gamma_\theta$ is a $12 \times 12$ positive-definite diagonal matrix. + +The disturbance estimate $\hat{d} \in \mathbb{R}^6$ has one component per DOF, adapted with the $6 \times 6$ diagonal gain $\Gamma_d$. + +### Important implementation detail: computing $\dot{\alpha}$ + +The control law requires $\dot{\alpha}$, the time derivative of the virtual control. Since $\alpha = -J_e(\eta)^{-1}K_1 z_1$, applying the matrix identity $\tfrac{d}{dt}(A^{-1}) = -A^{-1}\dot{A}A^{-1}$: + +```math +\dot{\alpha} = J_e^{-1}\dot{J}_e J_e^{-1} K_1 z_1 - J_e^{-1} K_1 J_e\,\nu +``` + +The block structure of $J_e$ gives: + +```math +\dot{J}_e = +\begin{bmatrix} +\dot{R} & 0_{3\times 3} \\ +0_{3\times 3} & \dot{T}_e +\end{bmatrix} +``` + +**$\dot{R}$** uses the standard identity: + +```math +\dot{R} = R\,S(\omega) +``` + +**$\dot{T}_e$** follows from differentiating $T_e = \eta_e I + S(\varepsilon_e)$ using the quaternion kinematic equations $\dot{\eta}_e = -\tfrac{1}{2}\varepsilon_e^\top\omega$ and $\dot{\varepsilon}_e = \tfrac{1}{2}T_e\omega$: + +```math +\dot{T}_e = \tfrac{1}{2}\bigl(S(T_e\,\omega) - (\varepsilon_e^\top\omega)\,I_3\bigr) +``` + +This compact closed form — compared to the lengthy trigonometric expression required for $\dot{T}$ in the Euler-angle version — is a key practical advantage of the quaternion parameterisation. + +## Launch + +To run the controller, use the ROS 2 launch file: +```bash +ros2 launch dp_adapt_backs_controller_quat dp_adapt_backs_controller_quat.launch.py +``` + +Remember to `colcon build` and `source install/setup.bash` first. + +Two configuration files are provided: +- `adapt_params_nautilus.yaml` — tuned for the physical Nautilus AUV +- `adapt_params_nautilus_sim.yaml` — tuned for simulation diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml new file mode 100644 index 000000000..a26482c83 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + K1 : [4.0, 4.0, 12.5, 1.5, 3.0, 3.0] # Outer loop tuning parameters + K2 : [25.0, 25.0, 50.0, 25.0, 50.0, 50.0] # Inner loop tuning parameters + r_b_bg : [0.00, 0.0, 0.015] # Vector from body centre to centre of gravity + adapt_gain : [0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2] # Tuning parameters for linear and nonlinear damping + d_gain : [0.3, 0.3, 0.3, 0.3, 0.3, 0.3] # Tuning parameters for the unmodeled disturbances and uncertanties + time_step_ms: 10 #Controller timestep in milliseconds (ms) + singularity_tolerance: 1.0e-8 # Determinant threshold below which L is treated as singular + adapt_param_max: 20.0 # Symmetric clamp on adaptive damping parameter estimates + d_est_max: 20.0 # Symmetric clamp on disturbance estimates diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_physical.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_physical.yaml new file mode 100644 index 000000000..64a744bb2 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_physical.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + K1 : [-0.55, -0.7, 2.5, 1.4, 2.0, 0.9] # Outer loop tuning parameters + K2 : [125.0, 120.0, 100.0, 60.0, 60.0, 60.0] # Inner loop tuning parameters + r_b_bg : [0.00, 0.0, 0.010] # Vector from body centre to centre of gravity + adapt_gain : [0.32, 0.11, 0.32, 0.11, 0.32, 0.11, 0.32, 0.11, 0.32, 0.11, 0.32, 0.11] # Tuning parameters for linear and nonlinear damping + d_gain : [0.13, 0.13, 0.13, 0.13, 0.13, 0.13] # Tuning parameters for the unmodeled disturbances and uncertanties + time_step_ms: 10 #Controller timestep in milliseconds (ms) + singularity_tolerance: 1.0e-8 # Determinant threshold below which L is treated as singular + adapt_param_max: 15.0 # Symmetric clamp on adaptive damping parameter estimates + d_est_max: 15.0 # Symmetric clamp on disturbance estimates diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_sim.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_sim.yaml new file mode 100644 index 000000000..a26482c83 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_sim.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + K1 : [4.0, 4.0, 12.5, 1.5, 3.0, 3.0] # Outer loop tuning parameters + K2 : [25.0, 25.0, 50.0, 25.0, 50.0, 50.0] # Inner loop tuning parameters + r_b_bg : [0.00, 0.0, 0.015] # Vector from body centre to centre of gravity + adapt_gain : [0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2] # Tuning parameters for linear and nonlinear damping + d_gain : [0.3, 0.3, 0.3, 0.3, 0.3, 0.3] # Tuning parameters for the unmodeled disturbances and uncertanties + time_step_ms: 10 #Controller timestep in milliseconds (ms) + singularity_tolerance: 1.0e-8 # Determinant threshold below which L is treated as singular + adapt_param_max: 20.0 # Symmetric clamp on adaptive damping parameter estimates + d_est_max: 20.0 # Symmetric clamp on disturbance estimates diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp new file mode 100644 index 000000000..ec916d7c3 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp @@ -0,0 +1,86 @@ +#ifndef DP_ADAPT_BACKS_CONTROLLER_QUAT__DP_ADAPT_BACKS_CONTROLLER_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER_QUAT__DP_ADAPT_BACKS_CONTROLLER_HPP_ + +#include +#include +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +namespace vortex::control { + +/** + * @brief Parameters for the DP adaptive backstepping controller. + * + * Collects all tuning gains, physical properties, and numerical thresholds + * needed to construct a DPAdaptBacksController instance. + */ +struct DPAdaptParams { + Eigen::Vector12d adapt_param = Eigen::Vector12d::Zero(); + Eigen::Vector6d d_gain = Eigen::Vector6d::Zero(); + Eigen::Vector6d K1 = Eigen::Vector6d::Zero(); + Eigen::Vector6d K2 = Eigen::Vector6d::Zero(); + Eigen::Vector3d r_b_bg = Eigen::Vector3d::Zero(); + Eigen::Vector3d inertia_matrix_body = Eigen::Vector3d::Zero(); + Eigen::Matrix6d mass_intertia_matrix = Eigen::Matrix6d::Zero(); + Eigen::Vector6d tau_max = Eigen::Vector6d::Ones(); + double mass{}; + double time_step_s{}; + double singularity_tolerance; + double adapt_param_max; + double d_est_max; +}; + +class DPAdaptBacksController { + public: + explicit DPAdaptBacksController(const DPAdaptParams& dp_adapt_params); + + /** + * @brief Calculates the control input tau found in the backstepping proof. + * + * Utilizes error state to avoid 7x6 non-invertible J matrix. The + * approximation of quaternion error -> euler angle error is used, and + * therefore we explicitly assume small perturbations. + * + * @param pose 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, + * qz] + * @param pose_d 7D vector containing the desired vehicle pose [x, y, z, qw, + * qx, qy, qz] + * @param twist 6D vector containing the vehicle velocity [u, v, w, p, q, r] + * @return 6D vector containing the control input tau [X, Y, Z, K, M, N] + */ + Eigen::Vector6d calculate_tau(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Pose& pose_d, + const vortex::utils::types::Twist& twist); + + /** @brief Reset the adaptive parameters to zero. */ + void reset_adap_param(); + + /** @brief Reset the disturbance estimate to zero. */ + void reset_d_est(); + + /** + * @brief Set the integration time step. + * @param time_step_s Time step in seconds. + */ + void set_time_step(const double time_step_s); + + private: + Eigen::Matrix6d K1_; + Eigen::Matrix6d K2_; + Eigen::Vector3d r_b_bg_; + Eigen::Matrix12d adapt_gain_; + Eigen::Matrix6d d_gain_; + Eigen::Vector12d adapt_param_; + Eigen::Vector6d d_est_; + Eigen::Matrix3d inertia_matrix_body_; + Eigen::Matrix6d mass_intertia_matrix_; + Eigen::Vector6d tau_max_; + double m_{}; + double time_step_s_{}; + double singularity_tolerance_{}; + double adapt_param_max_{}; + double d_est_max_{}; +}; + +} // namespace vortex::control + +#endif // DP_ADAPT_BACKS_CONTROLLER_QUAT__DP_ADAPT_BACKS_CONTROLLER_HPP_ diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp new file mode 100644 index 000000000..488a34113 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp @@ -0,0 +1,124 @@ +#ifndef DP_ADAPT_BACKS_CONTROLLER_QUAT__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER_QUAT__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" +#include "typedefs.hpp" + +namespace vortex::control { + +/** @brief ROS 2 node wrapper for the DP Adaptive Backstepping controller. */ +class DPAdaptBacksControllerNode : public rclcpp::Node { + public: + explicit DPAdaptBacksControllerNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + /** @brief Client for the GetOperationMode service. */ + rclcpp::Client::SharedPtr + get_operation_mode_client_; + + /** + * @brief Callback for the killswitch topic. + * @param msg Bool message containing the killswitch status. + */ + void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg); + + /** + * @brief Callback for the operation mode topic. + * @param msg OperationMode message containing the current software mode. + */ + void operation_mode_callback( + const vortex_msgs::msg::OperationMode::SharedPtr msg); + + /** + * @brief Callback for the pose topic. + * @param msg PoseWithCovarianceStamped message containing the AUV pose. + */ + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + /** + * @brief Callback for the twist topic. + * @param msg TwistWithCovarianceStamped message containing the AUV + * velocity. + */ + void twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + /** @brief Timer callback that computes and publishes the control wrench. */ + void publish_tau(); + + /** @brief Declare and load all adaptive controller parameters from ROS + * params. */ + void set_adap_params(); + + /** @brief Create all subscribers, publishers, and the timer. */ + void set_subscribers_and_publisher(); + + /** @brief Query the GetOperationMode service to initialise the operation + * mode. */ + void initialize_operation_mode(); + + /** + * @brief Callback for the guidance topic. + * @param msg ReferenceFilterQuat message containing the desired pose and + * velocity. + */ + void guidance_callback( + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg); + + rclcpp::Subscription::SharedPtr killswitch_sub_{}; + + rclcpp::Subscription::SharedPtr + operation_mode_sub_{}; + + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_{}; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_{}; + + rclcpp::Subscription::SharedPtr + guidance_sub_{}; + + rclcpp::Publisher::SharedPtr tau_pub_{}; + + rclcpp::TimerBase::SharedPtr tau_pub_timer_{}; + + std::chrono::milliseconds time_step_ms{}; + + vortex::utils::types::Pose pose_; + + vortex::utils::types::Pose pose_d_; + + vortex::utils::types::Twist twist_; + + std::unique_ptr dp_adapt_backs_controller_{}; + + bool killswitch_on_{true}; + + vortex::utils::types::Mode operation_mode_{ + vortex::utils::types::Mode::manual}; +}; + +} // namespace vortex::control + +#endif // DP_ADAPT_BACKS_CONTROLLER_QUAT__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp new file mode 100644 index 000000000..24fc531c7 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp @@ -0,0 +1,107 @@ +#ifndef DP_ADAPT_BACKS_CONTROLLER_QUAT__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER_QUAT__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ + +#include +#include "dp_adapt_backs_controller_quat/typedefs.hpp" +#include "typedefs.hpp" + +namespace vortex::control { + +/** + * @brief Calculate the time derivative of the rotation matrix R. + * @param pose Vehicle pose containing the current orientation quaternion. + * @param twist Vehicle velocity containing the angular rate omega. + * @return 3x3 matrix R_dot = R * S(omega). + */ +Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Twist& twist); + +/** + * @brief Build the orientation part of the error-state Jacobian. + * + * Computes Q_e = qw_e * I_3 + S(eps_e), the 3x3 block that maps angular + * velocity to the time derivative of 2*eps_e in the error-state kinematics + * z_dot_1_ori = Q_e * omega. + * + * @param eps_e Vector part of the error quaternion q_e = q_d^* otimes q. + * @param qw_e Scalar part of the error quaternion. + * @return 3x3 matrix Q_e. + */ +Eigen::Matrix3d calculate_Q_e(const Eigen::Vector3d& eps_e, double qw_e); + +/** + * @brief Assemble the 6x6 error-state Jacobian L = diag(R, Q_e). + * + * @param R 3x3 rotation matrix from NED to body (from the current pose). + * @param Q_e 3x3 orientation Jacobian block (from calculate_Q_e). + * @return 6x6 block-diagonal matrix L. + */ +Eigen::Matrix6d calculate_L(const Eigen::Matrix3d& R, + const Eigen::Matrix3d& Q_e); + +/** + * @brief Compute the inverse of the error-state Jacobian L with a singularity + * guard. + * + * Falls back to the Moore-Penrose pseudo-inverse when |det(L)| < tolerance. + * + * @param L Pre-built 6x6 error-state Jacobian. + * @param tolerance Determinant threshold below which the pseudo-inverse is + * used. + * @return 6x6 inverse (or pseudo-inverse) of L. + */ +Eigen::Matrix6d calculate_L_inv(const Eigen::Matrix6d& L, + double tolerance = 1e-8); + +/** + * @brief Compute the time derivative of Q_e via error-quaternion kinematics. + * + * Derived from q_e_dot using qw_e_dot = -0.5 * eps_e^T * omega and + * eps_e_dot = 0.5 * Q_e * omega: + * Q_e_dot = S(0.5 * Q_e * omega) - 0.5 * (eps_e^T * omega) * I_3 + * + * @param eps_e Vector part of the error quaternion. + * @param Q_e Current Q_e matrix (from calculate_Q_e). + * @param omega Body-frame angular velocity. + * @return 3x3 matrix Q_e_dot. + */ +Eigen::Matrix3d calculate_Q_e_dot(const Eigen::Vector3d& eps_e, + const Eigen::Matrix3d& Q_e, + const Eigen::Vector3d& omega); + +/** + * @brief Assemble the time derivative of the error-state Jacobian + * L_dot = diag(R_dot, Q_e_dot). + * + * @param R_dot 3x3 time derivative of the rotation matrix. + * @param Q_e_dot 3x3 time derivative of Q_e (from calculate_Q_e_dot). + * @return 6x6 block-diagonal matrix L_dot. + */ +Eigen::Matrix6d calculate_L_dot(const Eigen::Matrix3d& R_dot, + const Eigen::Matrix3d& Q_e_dot); + +/** + * @brief Calculate the Coriolis and centripetal matrix. + * @param mass Vehicle mass. + * @param r_b_bg Vector from the body-frame origin to the centre of gravity. + * @param twist Body-frame velocity (linear and angular). + * @param inertia_matrix_body 3x3 body-frame inertia matrix. + * @return 6x6 Coriolis matrix C(nu). + */ +Eigen::Matrix6d calculate_coriolis(const double mass, + const Eigen::Vector3d& r_b_bg, + const vortex::utils::types::Twist& twist, + const Eigen::Matrix3d& inertia_matrix_body); + +/** + * @brief Build the damping regressor matrix Y(nu). + * @param twist Vehicle velocity (one linear + one quadratic damping term per + * DOF). + * @return 6x12 regressor matrix Y_v such that Y_v * Theta gives the damping + * wrench. + */ +Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist); + +} // namespace vortex::control + +#endif // DP_ADAPT_BACKS_CONTROLLER_QUAT__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/typedefs.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/typedefs.hpp new file mode 100644 index 000000000..31d0ee296 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/typedefs.hpp @@ -0,0 +1,22 @@ +/** + * @file typedefs.hpp + * @brief Contains the Eigen typedefs for the controller. + */ + +#ifndef DP_ADAPT_BACKS_CONTROLLER_QUAT__TYPEDEFS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER_QUAT__TYPEDEFS_HPP_ + +#include + +namespace Eigen { + +typedef Eigen::Matrix Vector6d; +typedef Eigen::Matrix Vector12d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix6x12d; +typedef Eigen::Matrix Matrix12x6d; +typedef Eigen::Matrix Matrix12d; + +} // namespace Eigen + +#endif // DP_ADAPT_BACKS_CONTROLLER_QUAT__TYPEDEFS_HPP_ diff --git a/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py b/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py new file mode 100644 index 000000000..8350bd019 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py @@ -0,0 +1,45 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + adapt_params = os.path.join( + get_package_share_directory("dp_adapt_backs_controller_quat"), + "config", + f"adapt_params_{drone}.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + return [ + Node( + package="dp_adapt_backs_controller_quat", + executable="dp_adapt_backs_controller_quat_node", + name="dp_adapt_backs_controller_node", + namespace=namespace, + parameters=[adapt_params, drone_params], + output="screen", + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) diff --git a/control/dp_adapt_backs_controller_quat/package.xml b/control/dp_adapt_backs_controller_quat/package.xml new file mode 100644 index 000000000..155e9228d --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/package.xml @@ -0,0 +1,23 @@ + + + + dp_adapt_backs_controller_quat + 1.0.0 + Adaptive backstepping controller for DP + Cyprian Osinski + MIT + + ament_cmake + + rclcpp + geometry_msgs + eigen + vortex_msgs + vortex_utils + vortex_utils_ros + yaml-cpp + + + ament_cmake + + diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp new file mode 100644 index 000000000..4186c37fc --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp @@ -0,0 +1,96 @@ +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" +#include +#include +#include +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +namespace vortex::control { + +using vortex::utils::types::Pose; +using vortex::utils::types::Twist; + +DPAdaptBacksController::DPAdaptBacksController( + const DPAdaptParams& dp_adapt_params) + : K1_(dp_adapt_params.K1.asDiagonal().toDenseMatrix()), + K2_(dp_adapt_params.K2.asDiagonal().toDenseMatrix()), + r_b_bg_(dp_adapt_params.r_b_bg), + adapt_gain_(dp_adapt_params.adapt_param.asDiagonal().toDenseMatrix()), + d_gain_(dp_adapt_params.d_gain.asDiagonal().toDenseMatrix()), + adapt_param_(Eigen::Vector12d::Zero()), + d_est_(Eigen::Vector6d::Zero()), + inertia_matrix_body_( + dp_adapt_params.inertia_matrix_body.asDiagonal().toDenseMatrix()), + mass_intertia_matrix_(dp_adapt_params.mass_intertia_matrix), + tau_max_(dp_adapt_params.tau_max), + m_(dp_adapt_params.mass), + time_step_s_(dp_adapt_params.time_step_s), + singularity_tolerance_(dp_adapt_params.singularity_tolerance), + adapt_param_max_(dp_adapt_params.adapt_param_max), + d_est_max_(dp_adapt_params.d_est_max) {} + +Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Pose& pose, + const Pose& pose_d, + const Twist& twist) { + Eigen::Vector3d pos_error = pose.pos_vector() - pose_d.pos_vector(); + + // Error quaternion q_e = q_d^{-1} * q (rotation from desired to current). + // z_1_ori = 2*eps_e, so d/dt(z_1_ori) = (qw_e*I + S(eps_e)) * omega. + // This requires building L with the error quaternion, not q_current, + // otherwise the Lyapunov cross-terms don't cancel and orientation diverges. + const Eigen::Quaterniond q_e = vortex::utils::math::error_quaternion( + pose_d.ori_quaternion(), pose.ori_quaternion()); + const Eigen::Vector3d eps_e = q_e.vec(); + const double qw_e = q_e.w(); + const Eigen::Vector3d quat_error = 2.0 * eps_e; + + Eigen::Vector6d z_1; + z_1 << pos_error, quat_error; + + const Eigen::Matrix3d R = pose.as_rotation_matrix(); + const Eigen::Matrix3d Q_e = calculate_Q_e(eps_e, qw_e); + const Eigen::Matrix6d L = calculate_L(R, Q_e); + const Eigen::Matrix6d L_inv = calculate_L_inv(L, singularity_tolerance_); + + const Eigen::Vector3d omega = twist.to_vector().tail<3>(); + const Eigen::Matrix3d Q_e_dot = calculate_Q_e_dot(eps_e, Q_e, omega); + const Eigen::Matrix6d L_dot = + calculate_L_dot(calculate_R_dot(pose, twist), Q_e_dot); + + Eigen::Matrix6d C = + calculate_coriolis(m_, r_b_bg_, twist, inertia_matrix_body_); + Eigen::Vector6d alpha = -L_inv * K1_ * z_1; + Eigen::Vector6d z_2 = twist.to_vector() - alpha; + Eigen::Vector6d alpha_dot = ((L_inv * L_dot * L_inv) * K1_ * z_1) - + (L_inv * K1_ * L * twist.to_vector()); + Eigen::Matrix6x12d Y_v = calculate_Y_v(twist); + Eigen::Vector12d adapt_param_dot = adapt_gain_ * Y_v.transpose() * z_2; + Eigen::Vector6d d_est_dot = d_gain_ * z_2; + Eigen::Vector6d F_est = Y_v * adapt_param_; + Eigen::Vector6d tau = (mass_intertia_matrix_ * alpha_dot) + + (C * twist.to_vector()) - (L.transpose() * z_1) - + (K2_ * z_2) - F_est - d_est_; + + tau = tau.cwiseMax(-tau_max_).cwiseMin(tau_max_); + adapt_param_ += adapt_param_dot * time_step_s_; + d_est_ += d_est_dot * time_step_s_; + adapt_param_ = + adapt_param_.cwiseMax(-adapt_param_max_).cwiseMin(adapt_param_max_); + d_est_ = d_est_.cwiseMax(-d_est_max_).cwiseMin(d_est_max_); + + return tau; +} + +void DPAdaptBacksController::reset_adap_param() { + adapt_param_.setZero(); +} + +void DPAdaptBacksController::reset_d_est() { + d_est_.setZero(); +} + +void DPAdaptBacksController::set_time_step(const double time_step_s) { + time_step_s_ = time_step_s; +} + +} // namespace vortex::control diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp new file mode 100644 index 000000000..212df015c --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp @@ -0,0 +1,334 @@ +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp" +#include +#include +#include +#include +#include +#include +#include +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +constexpr std::string_view start_message = R"( + ██████╗ ██╗ ██╗ █████╗ ████████╗███████╗██████╗ ███╗ ██╗██╗ ██████╗ ███╗ ██╗ ██████╗ ██████╗ +██╔═══██╗██║ ██║██╔══██╗╚══██╔══╝██╔════╝██╔══██╗████╗ ██║██║██╔═══██╗████╗ ██║ ██╔══██╗██╔══██╗ +██║ ██║██║ ██║███████║ ██║ █████╗ ██████╔╝██╔██╗ ██║██║██║ ██║██╔██╗ ██║ ██║ ██║██████╔╝ +██║▄▄ ██║██║ ██║██╔══██║ ██║ ██╔══╝ ██╔══██╗██║╚██╗██║██║██║ ██║██║╚██╗██║ ██║ ██║██╔═══╝ +╚██████╔╝╚██████╔╝██║ ██║ ██║ ███████╗██║ ██║██║ ╚████║██║╚██████╔╝██║ ╚████║ ██████╔╝██║ + ╚══▀▀═╝ ╚═════╝ ╚═╝ ╚═╝ ╚═╝ ╚══════╝╚═╝ ╚═╝╚═╝ ╚═══╝╚═╝ ╚═════╝ ╚═╝ ╚═══╝ ╚═════╝ ╚═╝ +)"; + +namespace vortex::control { + +DPAdaptBacksControllerNode::DPAdaptBacksControllerNode( + const rclcpp::NodeOptions& options) + : Node("dp_adapt_backs_controller_node", options) { + this->declare_parameter("time_step_ms"); + time_step_ms = std::chrono::milliseconds( + static_cast(this->get_parameter("time_step_ms").as_int())); + + set_subscribers_and_publisher(); + initialize_operation_mode(); + + tau_pub_timer_ = + this->create_wall_timer(time_step_ms, [this]() { publish_tau(); }); + set_adap_params(); + + spdlog::info(start_message); +} + +void DPAdaptBacksControllerNode::set_subscribers_and_publisher() { + const auto qos_sensor_data{ + vortex::utils::qos_profiles::sensor_data_profile(1)}; + const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)}; + + this->declare_parameter("topics.guidance.dp_quat"); + std::string dp_reference_topic = + this->get_parameter("topics.guidance.dp_quat").as_string(); + guidance_sub_ = + this->create_subscription( + dp_reference_topic, qos_sensor_data, + [this](const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) { + guidance_callback(msg); + }); + + this->declare_parameter("topics.pose"); + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + msg) { pose_callback(msg); }); + + this->declare_parameter("topics.twist"); + std::string twist_topic = this->get_parameter("topics.twist").as_string(); + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + twist_topic, qos_sensor_data, + [this](const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr + msg) { twist_callback(msg); }); + + this->declare_parameter("topics.killswitch"); + std::string software_kill_switch_topic = + this->get_parameter("topics.killswitch").as_string(); + killswitch_sub_ = this->create_subscription( + software_kill_switch_topic, qos_reliable, + [this](const std_msgs::msg::Bool::SharedPtr msg) { + killswitch_callback(msg); + }); + + this->declare_parameter("topics.operation_mode"); + std::string software_operation_mode_topic = + this->get_parameter("topics.operation_mode").as_string(); + operation_mode_sub_ = + this->create_subscription( + software_operation_mode_topic, qos_reliable, + [this](const vortex_msgs::msg::OperationMode::SharedPtr msg) { + operation_mode_callback(msg); + }); + + this->declare_parameter("topics.wrench_input"); + std::string control_topic = + this->get_parameter("topics.wrench_input").as_string(); + tau_pub_ = this->create_publisher( + control_topic, qos_sensor_data); +} + +void DPAdaptBacksControllerNode::initialize_operation_mode() { + this->declare_parameter("services.get_operation_mode"); + std::string get_operation_mode_service = + this->get_parameter("services.get_operation_mode").as_string(); + + get_operation_mode_client_ = + this->create_client( + get_operation_mode_service); + + while (!get_operation_mode_client_->wait_for_service( + std::chrono::seconds(1))) { + spdlog::warn("Waiting for GetOperationMode service to be available..."); + } + + auto request = + std::make_shared(); + get_operation_mode_client_->async_send_request( + request, + [this](rclcpp::Client::SharedFuture + future) { + try { + auto response = future.get(); + operation_mode_ = + vortex::utils::ros_conversions::convert_from_ros( + response->current_operation_mode); + killswitch_on_ = response->killswitch_status; + spdlog::info( + "Initial operation mode: {} | Killswitch status: {}", + vortex::utils::types::mode_to_string(operation_mode_), + killswitch_on_ ? "on" : "off"); + } catch (const std::exception& e) { + spdlog::error("Failed to get initial operation mode: {}", + e.what()); + killswitch_on_ = true; + } + }); +} + +void DPAdaptBacksControllerNode::killswitch_callback( + const std_msgs::msg::Bool::SharedPtr msg) { + killswitch_on_ = msg->data; + dp_adapt_backs_controller_->reset_adap_param(); + dp_adapt_backs_controller_->reset_d_est(); + spdlog::info("Killswitch: {}", killswitch_on_ ? "on" : "off"); +} + +void DPAdaptBacksControllerNode::operation_mode_callback( + const vortex_msgs::msg::OperationMode::SharedPtr msg) { + operation_mode_ = vortex::utils::ros_conversions::convert_from_ros(*msg); + spdlog::info("Operation mode: {}", + vortex::utils::types::mode_to_string(operation_mode_)); + + if (operation_mode_ == vortex::utils::types::Mode::autonomous || + operation_mode_ == vortex::utils::types::Mode::reference) { + pose_d_ = pose_; + } +} + +void DPAdaptBacksControllerNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + pose_ = vortex::utils::ros_conversions::ros_pose_to_pose(msg->pose.pose); +} + +void DPAdaptBacksControllerNode::twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + twist_ = + vortex::utils::ros_conversions::ros_twist_to_twist(msg->twist.twist); +} + +void DPAdaptBacksControllerNode::set_adap_params() { + this->declare_parameter>("adapt_gain"); + this->declare_parameter>("d_gain"); + this->declare_parameter>("K1"); + this->declare_parameter>("K2"); + this->declare_parameter>("r_b_bg"); + this->declare_parameter>("physical.mass_matrix"); + this->declare_parameter>("physical.center_of_mass"); + this->declare_parameter>( + "propulsion.thrusters.thruster_force_direction"); + this->declare_parameter>( + "propulsion.thrusters.thruster_position"); + this->declare_parameter("propulsion.thrusters.num"); + this->declare_parameter("propulsion.dimensions.num"); + this->declare_parameter( + "propulsion.thrusters.constraints.min_force"); + this->declare_parameter( + "propulsion.thrusters.constraints.max_force"); + this->declare_parameter("singularity_tolerance"); + this->declare_parameter("adapt_param_max"); + this->declare_parameter("d_est_max"); + + std::vector adapt_param_vec = + this->get_parameter("adapt_gain").as_double_array(); + std::vector d_gain_vec = + this->get_parameter("d_gain").as_double_array(); + std::vector K1_vec = this->get_parameter("K1").as_double_array(); + std::vector K2_vec = this->get_parameter("K2").as_double_array(); + std::vector r_b_bg_vec = + this->get_parameter("r_b_bg").as_double_array(); + std::vector mass_intertia_matrix_vec = + this->get_parameter("physical.mass_matrix").as_double_array(); + + Eigen::Vector12d adapt_param_eigen = + Eigen::Map(adapt_param_vec.data()); + Eigen::Vector6d d_gain_eigen = + Eigen::Map(d_gain_vec.data()); + Eigen::Vector6d K1_eigen = Eigen::Map(K1_vec.data()); + Eigen::Vector6d K2_eigen = Eigen::Map(K2_vec.data()); + Eigen::Vector3d r_b_bg_eigen = + Eigen::Map(r_b_bg_vec.data()); + Eigen::Matrix6d mass_intertia_matrix = + Eigen::Map(mass_intertia_matrix_vec.data()); + + double mass = mass_intertia_matrix(0, 0); + Eigen::Vector3d inertia_matrix_body_eigen(mass_intertia_matrix(3, 3), + mass_intertia_matrix(4, 4), + mass_intertia_matrix(5, 5)); + + // Compute per-DOF max wrench from the thruster configuration + int num_thrusters = + this->get_parameter("propulsion.thrusters.num").as_int(); + int num_dims = this->get_parameter("propulsion.dimensions.num").as_int(); + double min_force = + this->get_parameter("propulsion.thrusters.constraints.min_force") + .as_double(); + double max_force = + this->get_parameter("propulsion.thrusters.constraints.max_force") + .as_double(); + + Eigen::Vector3d center_of_mass = Eigen::Map( + this->get_parameter("physical.center_of_mass") + .as_double_array() + .data()); + + auto dir_vec = + this->get_parameter("propulsion.thrusters.thruster_force_direction") + .as_double_array(); + auto pos_vec = this->get_parameter("propulsion.thrusters.thruster_position") + .as_double_array(); + + Eigen::MatrixXd thruster_dir = + Eigen::Map>( + dir_vec.data(), num_dims, num_thrusters); + Eigen::MatrixXd thruster_pos = + Eigen::Map>( + pos_vec.data(), num_dims, num_thrusters); + + const Eigen::MatrixXd T = + vortex::utils::math::build_thrust_configuration_matrix( + thruster_dir, thruster_pos, center_of_mass); + const Eigen::VectorXd u_min = + Eigen::VectorXd::Constant(num_thrusters, min_force); + const Eigen::VectorXd u_max = + Eigen::VectorXd::Constant(num_thrusters, max_force); + const Eigen::Vector6d tau_max = + vortex::utils::math::calculate_valid_thrust_region_polyhedron(T, u_min, + u_max); + + DPAdaptParams dp_adapt_params = DPAdaptParams{ + .adapt_param = adapt_param_eigen, + .d_gain = d_gain_eigen, + .K1 = K1_eigen, + .K2 = K2_eigen, + .r_b_bg = r_b_bg_eigen, + .inertia_matrix_body = inertia_matrix_body_eigen, + .mass_intertia_matrix = mass_intertia_matrix, + .tau_max = tau_max, + .mass = mass, + .time_step_s = static_cast(time_step_ms.count()) / 1000.0, + .singularity_tolerance = + this->get_parameter("singularity_tolerance").as_double(), + .adapt_param_max = this->get_parameter("adapt_param_max").as_double(), + .d_est_max = this->get_parameter("d_est_max").as_double(), + }; + + dp_adapt_backs_controller_ = + std::make_unique(dp_adapt_params); +} + +void DPAdaptBacksControllerNode::publish_tau() { + if (killswitch_on_ || + operation_mode_ == vortex::utils::types::Mode::manual) { + return; + } + + Eigen::Vector6d tau = + dp_adapt_backs_controller_->calculate_tau(pose_, pose_d_, twist_); + + geometry_msgs::msg::WrenchStamped tau_msg; + tau_msg.header.stamp = this->now(); + tau_msg.header.frame_id = "base_link"; + tau_msg.wrench.force.x = tau(0); + tau_msg.wrench.force.y = tau(1); + tau_msg.wrench.force.z = tau(2); + tau_msg.wrench.torque.x = tau(3); + tau_msg.wrench.torque.y = tau(4); + tau_msg.wrench.torque.z = tau(5); + + tau_pub_->publish(tau_msg); +} + +void DPAdaptBacksControllerNode::guidance_callback( + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) { + pose_d_ = + vortex::utils::ros_conversions::reference_filter_quat_to_pose(*msg); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(DPAdaptBacksControllerNode) + +} // namespace vortex::control + +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⣀⣠⡤⣦⢦⠖⡶⠲⢦⣤⢤⣤⣤⣀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⡴⢶⠛⡍⢎⠱⣈⠒⢌⡘⠰⠉⢆⠰⢉⠔⠢⡉⢝⢫⠳⢦⣄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⡶⢏⠳⡘⠢⢉⠔⡈⠔⠠⠈⠄⠠⠁⠌⡀⠂⠌⠠⠁⠌⡐⢂⠉⢆⠩⡝⢳⣦⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣴⡞⢫⠔⡉⠆⡁⠢⢁⣂⡐⡈⠄⢁⠈⠄⣁⡤⠄⣁⣠⠁⡈⠄⠐⠠⠉⠠⠁⡌⢡⠊⡝⢷⣄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⣠⡾⢣⠎⡡⢊⠴⣅⣶⣽⣷⣾⣿⣶⠌⠀⠄⢺⢾⣿⣿⣿⣷⣾⣿⣤⣧⡰⠈⠠⢁⠐⢂⠑⠌⡸⠸⣷⣤⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⣸⢯⣑⠣⣌⠱⢨⣿⣿⣿⣿⡿⠟⠋⠁⡀⠌⠐⠠⢁⠚⡹⢛⡿⢿⣿⣿⣿⠯⢀⠁⠂⠌⠠⢈⠂⣁⠣⡘⠽⣦⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⢴⡟⡥⢊⠵⣠⠣⢍⠺⢟⠩⣁⠒⠨⢀⠂⠀⠄⠡⡁⢂⠠⠀⠄⢠⠉⠄⡊⢄⠂⡄⠊⠄⢃⠤⢁⠂⢄⠂⠥⡙⣚⢷⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢀⡿⡜⠴⣉⠖⣡⢚⣬⢳⡉⠖⣀⠃⣁⠢⠐⡈⠠⢁⠒⡄⢂⠡⠊⠄⡘⠤⠑⡌⢒⠤⢃⠜⣀⠂⢆⠘⡠⠘⡀⢆⢡⢻⣧⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢸⣛⣌⠳⡌⢎⡵⣫⣎⠧⡘⠡⠀⠂⢀⠂⠱⢠⢁⠊⡴⣘⠢⠁⠌⠀⠐⠠⢁⠘⡄⠫⡜⠲⣄⠩⢄⠢⠁⠆⠡⠌⡂⢎⡽⣆⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢸⡷⢌⠳⡌⢧⣿⢿⡜⣢⣱⣤⡧⣐⣢⡀⢃⢢⠁⢎⡶⣡⢆⣵⠴⠧⢦⣁⣂⠰⣈⠱⣘⠳⣬⡓⢌⠢⡉⢌⠡⡘⢄⢣⢚⣿⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢹⡞⣌⠳⡌⢧⣿⣿⣽⢷⣫⣔⡠⢀⢀⠈⠉⢲⣭⣰⣟⣯⣹⣔⣢⢡⠀⡀⠌⠙⠞⣳⠿⢷⣷⣝⠦⡑⠨⠄⡃⠔⡈⠦⣙⡾⡆⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢸⡿⢤⠓⣌⢣⢻⣹⣿⣻⣿⣿⣿⣶⠢⣌⢡⠂⡴⢻⡿⣷⣿⣿⣿⣷⣳⠴⡈⡜⡰⣄⢻⣜⣿⢳⡡⢊⡑⢌⠰⠡⡘⡰⣡⢿⡇⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠸⣟⢦⢋⡔⢣⡙⣷⡻⣿⣿⣿⣿⣿⣷⣎⢦⣙⢶⢏⠻⣿⣿⣿⣿⣿⡿⢧⡹⣴⢳⣽⣳⡿⢡⠗⡠⢃⠔⡈⢆⠱⣐⠱⡬⢿⡇⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⢻⣎⠖⡬⢑⡜⢲⢻⣮⠻⣿⣿⣻⣷⣾⡻⢞⠧⢎⠛⣮⣛⠿⣽⣻⣽⣯⣿⣾⣿⠿⢋⡴⢋⡒⢡⠊⡔⣁⠊⡔⠤⣛⠼⣿⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠘⢯⣞⠰⡡⠎⢥⢋⡞⡿⢶⡞⣭⡝⣲⡙⢎⡱⢊⡱⠄⣍⠛⢦⢋⠭⣉⡍⣡⠔⣎⠣⡜⡡⠘⡄⢣⠐⡄⢣⠘⢦⢭⣿⡟⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⣠⣤⡸⣿⢢⡑⠎⣆⢣⠚⣭⢣⡛⡴⡙⢆⠹⢠⠑⠢⢄⠃⠄⣉⠂⠍⠒⡡⠜⠤⢋⠔⡃⢆⠡⢃⡘⢄⠣⡘⢤⢋⡼⣺⣿⠉⣀⣤⣄⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⣤⡗⠀⣉⣻⣧⣙⠲⡐⢎⡱⢂⠇⣎⠱⢡⠊Life is pain,⡈⢂⠡⠌⡐⡈⠤⢁⠰⡈⢆⠱⣊⠼⣼⣿⣿⣛⣉⠐⠾⣄⠀⠀⠀⠀ +// ⠀⠀⠀⡼⠃⠀⠀⠙⣿⡘⢿⣵⠿⡼⣴⣩⣚Existence is meaningless⢢⣑⣎⡱⢬⣻⣿⠇⣸⡿⠁⠀⠀⠙⣆⠀⠀⠀ +// ⠀⠀⣰⡗⠀⠀⠀⠀⠘⣧⡀⠙⠻⣷⣾⣭⣿⣹⢏⡻⢩⢛⠛⡛⢛⠻⠛⠟⡛⠟⢛⠛⡛⠹⢛⠛⣙⡋⣏⣙⣩⣭⣭⣴⣼⣿⠟⠁⠈⣿⠁⠀⠀⠀⠀⢾⡄⠀⠀ +// ⠀⢠⠻⠀⢀⣠⣄⠀⠀⣻⠀⠀⠀⠀⠛⠿⣿⣿⣿⣷⣷⣮⣵⣌⣦⡱⣌⣲⡰⣌⣦⣱⣬⣷⣾⣿⣿⣿⡿⣿⢿⣿⣿⣿⠛⠁⠀⢀⢸⡇⠀⠀⣀⣀⠀⠘⡿⡀⠀ +// ⠀⣧⠇⠀⣴⠟⠈⠀⠀⣿⡄⠀⠀⠀⠀⠀⠈⠙⠻⢿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⢿⣻⢯⣷⣯⣿⣽⡿⠟⠉⠀⠀⠀⠀⠀⣘⣿⠀⢸⠉⠿⣄⠀⠰⣡⠀ +// ⠸⡌⠀⢹⡇⠀⠀⣏⠀⣽⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠉⠙⠛⠛⠿⠿⠿⢽⢿⡾⠷⠿⠿⠿⠟⠛⠋⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⣇⠀⠨⠀⠀⢹⠇⠀⢏⠂ +// ⠀⡇⠀⣻⡆⠀⠀⠉⠒⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠓⠒⠃⠀⠀⢸⣟⠀⣸⠀ +// ⠀⠣⠴⠞⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⠷⠄⠆⠀ diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp new file mode 100644 index 000000000..60894c53c --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp @@ -0,0 +1,101 @@ +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include +#include +#include +#include +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +namespace vortex::control { + +Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Twist& twist) { + return pose.as_rotation_matrix() * + vortex::utils::math::get_skew_symmetric_matrix( + twist.to_vector().tail(3)); +} + +Eigen::Matrix3d calculate_Q_e(const Eigen::Vector3d& eps_e, const double qw_e) { + return qw_e * Eigen::Matrix3d::Identity() + + vortex::utils::math::get_skew_symmetric_matrix(eps_e); +} + +Eigen::Matrix6d calculate_L(const Eigen::Matrix3d& R, + const Eigen::Matrix3d& Q_e) { + Eigen::Matrix6d L = Eigen::Matrix6d::Zero(); + L.topLeftCorner<3, 3>() = R; + L.bottomRightCorner<3, 3>() = Q_e; + return L; +} + +Eigen::Matrix6d calculate_L_inv(const Eigen::Matrix6d& L, + const double tolerance) { + if (std::abs(L.determinant()) < tolerance) { + spdlog::error("L is singular"); + return L.completeOrthogonalDecomposition().pseudoInverse(); + } + return L.inverse(); +} + +Eigen::Matrix3d calculate_Q_e_dot(const Eigen::Vector3d& eps_e, + const Eigen::Matrix3d& Q_e, + const Eigen::Vector3d& omega) { + return (-0.5 * eps_e.dot(omega)) * Eigen::Matrix3d::Identity() + + vortex::utils::math::get_skew_symmetric_matrix(0.5 * Q_e * omega); +} + +Eigen::Matrix6d calculate_L_dot(const Eigen::Matrix3d& R_dot, + const Eigen::Matrix3d& Q_e_dot) { + Eigen::Matrix6d L_dot = Eigen::Matrix6d::Zero(); + L_dot.topLeftCorner<3, 3>() = R_dot; + L_dot.bottomRightCorner<3, 3>() = Q_e_dot; + return L_dot; +} + +Eigen::Matrix6d calculate_coriolis(const double mass, + const Eigen::Vector3d& r_b_bg, + const vortex::utils::types::Twist& twist, + const Eigen::Matrix3d& inertia_matrix_body) { + using vortex::utils::math::get_skew_symmetric_matrix; + const Eigen::Vector3d linear_speed = twist.to_vector().head(3); + const Eigen::Vector3d angular_speed = twist.to_vector().tail(3); + Eigen::Matrix6d C; + C.topLeftCorner<3, 3>() = + mass * vortex::utils::math::get_skew_symmetric_matrix(linear_speed); + C.topRightCorner<3, 3>() = -mass * + get_skew_symmetric_matrix(angular_speed) * + get_skew_symmetric_matrix(r_b_bg); + C.bottomLeftCorner<3, 3>() = mass * + get_skew_symmetric_matrix(angular_speed) * + get_skew_symmetric_matrix(r_b_bg); + C.bottomRightCorner<3, 3>() = + get_skew_symmetric_matrix(inertia_matrix_body * angular_speed); + + return C; +} + +Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist) { + Eigen::Matrix6x12d Y_v; + Y_v.setZero(); + + Y_v(0, 0) = twist.u; + Y_v(0, 1) = twist.u * std::abs(twist.u); + + Y_v(1, 2) = twist.v; + Y_v(1, 3) = twist.v * std::abs(twist.v); + + Y_v(2, 4) = twist.w; + Y_v(2, 5) = twist.w * std::abs(twist.w); + + Y_v(3, 6) = twist.p; + Y_v(3, 7) = twist.p * std::abs(twist.p); + + Y_v(4, 8) = twist.q; + Y_v(4, 9) = twist.q * std::abs(twist.q); + + Y_v(5, 10) = twist.r; + Y_v(5, 11) = twist.r * std::abs(twist.r); + + return Y_v; +} + +} // namespace vortex::control diff --git a/control/dp_adapt_backs_controller_quat/test/CMakeLists.txt b/control/dp_adapt_backs_controller_quat/test/CMakeLists.txt new file mode 100644 index 000000000..fbd7acb2f --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/test/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(Git) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + dp_adapt_backs_controller_tests.cpp +) + +execute_process( + COMMAND ${GIT_EXECUTABLE} rev-parse --show-toplevel + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + OUTPUT_VARIABLE GIT_ROOT + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +target_compile_definitions(${TEST_BINARY_NAME} PRIVATE + DRONE_YAML_PATH="${GIT_ROOT}/auv_setup/config/robots/nautilus.yaml" + CONTROLLER_YAML_PATH="${CMAKE_SOURCE_DIR}/config/adapt_params_nautilus.yaml" +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${LIB_NAME} + yaml-cpp + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp b/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp new file mode 100644 index 000000000..94e7db7de --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp @@ -0,0 +1,477 @@ +#include +#include + +#include + +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +namespace vortex::control { + +using vortex::utils::types::Pose; +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Twist; + +DPAdaptParams load_dp_adapt_params(const std::string& drone_yaml_path, + const std::string& controller_yaml_path) { + YAML::Node drone_params = + YAML::LoadFile(drone_yaml_path)["/**"]["ros__parameters"]; + YAML::Node controller_params = + YAML::LoadFile(controller_yaml_path)["/**"]["ros__parameters"]; + + auto K1_vec = controller_params["K1"].as>(); + auto K2_vec = controller_params["K2"].as>(); + auto adapt_gain_vec = + controller_params["adapt_gain"].as>(); + auto d_gain_vec = controller_params["d_gain"].as>(); + auto r_b_bg_vec = controller_params["r_b_bg"].as>(); + + auto mass_matrix_vec = + drone_params["physical"]["mass_matrix"].as>(); + + Eigen::Matrix6d mass_matrix = + Eigen::Map(mass_matrix_vec.data()); + + DPAdaptParams params; + params.K1 = Eigen::Map(K1_vec.data()); + params.K2 = Eigen::Map(K2_vec.data()); + params.adapt_param = Eigen::Map(adapt_gain_vec.data()); + params.d_gain = Eigen::Map(d_gain_vec.data()); + params.r_b_bg = Eigen::Map(r_b_bg_vec.data()); + params.mass_intertia_matrix = mass_matrix; + params.mass = mass_matrix(0, 0); + params.inertia_matrix_body = Eigen::Vector3d( + mass_matrix(3, 3), mass_matrix(4, 4), mass_matrix(5, 5)); + return params; +} + +class DPAdaptBacksControllerTests : public ::testing::Test { + protected: + DPAdaptBacksControllerTests() + : dp_adapt_backs_controller_{ + load_dp_adapt_params(DRONE_YAML_PATH, CONTROLLER_YAML_PATH)} {} + + Pose generate_current_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { + return PoseEuler{north_pos, east_pos, down_pos, + roll_angle, pitch_angle, yaw_angle} + .as_pose(); + } + + Pose generate_reference_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { + return PoseEuler{north_pos, east_pos, down_pos, + roll_angle, pitch_angle, yaw_angle} + .as_pose(); + } + + Twist generate_current_velocity(const double surge_vel, + const double sway_vel, + const double heave_vel, + const double roll_rate, + const double pitch_rate, + const double yaw_rate) { + return {surge_vel, sway_vel, heave_vel, + roll_rate, pitch_rate, yaw_rate}; + } + + DPAdaptBacksController dp_adapt_backs_controller_; +}; + +/* +Test that negative north error only (in body) gives positive surge command only. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T01_neg_north_error_with_zero_heading_gives_surge_only_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with positive heading gives a positive surge +command and negative sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + Pose pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with negative heading gives a positive surge +command and positive sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + Pose pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and pitch gives a positive heave +command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and negative pitch gives a positive +heave and positive surge command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T05_neg_down_error_with_zero_roll_and_neg_pitch_gives_positive_heave_and_positive_surge_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and positive pitch gives a positive +heave and negative surge command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T06_neg_down_error_with_zero_roll_and_pos_pitch_gives_positive_heave_and_negative_surge_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with zero heading gives a positive sway command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T07_neg_east_error_with_zero_heading_gives_positive_sway_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive east error with zero heading gives a negative sway command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T08_pos_east_error_with_zero_heading_gives_pos_sway_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with positive heading gives a positive surge and +sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + Pose pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with negative heading gives a negative surge and +positive sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + Pose pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative roll error gives positive roll command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T11_neg_roll_error_gives_positive_roll_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_GT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive roll error gives negative roll command. +*/ + +TEST_F(DPAdaptBacksControllerTests, T12_pos_roll_error_gives_neg_roll_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_LT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative pitch error gives positive pitch command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T13_neg_pitch_error_gives_pos_pitch_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_GT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive pitch error gives negative pitch command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T14_pos_pitch_error_gives_neg_pitch_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_LT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative yaw error gives positive yaw command. +*/ + +TEST_F(DPAdaptBacksControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_GT(tau[5], 0.0); +} + +/* +Test that positive yaw error gives negative yaw command. +*/ + +TEST_F(DPAdaptBacksControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_LT(tau[5], 0.0); +} + +/* +Test that positive surge velocity only results in negative surge command +(breaking effect). +*/ + +TEST_F(DPAdaptBacksControllerTests, + T17_pos_surge_vel_gives_negative_surge_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix +} + +/* +Test that positive sway velocity only results in negative sway command (breaking +effect). +*/ + +TEST_F(DPAdaptBacksControllerTests, + T18_pos_sway_vel_gives_negative_sway_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix +} + +/* +Test that positive heave velocity only results in negative heave command +(breaking effect). +*/ + +TEST_F(DPAdaptBacksControllerTests, + T19_pos_heave_vel_gives_negative_heave_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_LT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix +} + +} // namespace vortex::control + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/control/pid_controller_dp/CMakeLists.txt b/control/pid_controller_dp/CMakeLists.txt index 17db75976..f5bafc206 100644 --- a/control/pid_controller_dp/CMakeLists.txt +++ b/control/pid_controller_dp/CMakeLists.txt @@ -11,38 +11,94 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) 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(pid_controller_node +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + geometry_msgs + nav_msgs + Eigen3 + vortex_msgs + rcl_interfaces + vortex_utils + vortex_utils_ros + spdlog + fmt +) + + +add_library(pid_controller_component SHARED + src/pid_controller_ros.cpp +) + +ament_target_dependencies(pid_controller_component PUBLIC rclcpp + rclcpp_components geometry_msgs nav_msgs Eigen3 - tf2 vortex_msgs + rcl_interfaces vortex_utils vortex_utils_ros + spdlog + fmt ) -install(TARGETS - pid_controller_node - DESTINATION lib/${PROJECT_NAME}) +target_link_libraries(pid_controller_component PUBLIC + ${LIB_NAME} + spdlog::spdlog +) + +rclcpp_components_register_node(pid_controller_component + PLUGIN "PIDControllerNode" + EXECUTABLE pid_controller_node_component +) + +add_executable(pid_controller_node + src/pid_controller_node.cpp +) + +target_link_libraries(pid_controller_node PRIVATE + pid_controller_component + spdlog::spdlog +) + +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 pid_controller_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS pid_controller_node + DESTINATION lib/${PROJECT_NAME} +) install(DIRECTORY config @@ -50,4 +106,9 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + ament_package() diff --git a/control/pid_controller_dp/README.md b/control/pid_controller_dp/README.md index 902fc9fc3..6d81aff27 100644 --- a/control/pid_controller_dp/README.md +++ b/control/pid_controller_dp/README.md @@ -1,7 +1,117 @@ -## PID controller -The PID controller is defined +# PID controller + +The controller operates on a **6D quaternion error state** — the scalar part $\tilde{q}_w$ is dropped and only the vector part is retained, keeping all matrices square (6×6). + +The control law is + +```math +\tau = -\left(K_p\, e_b + K_d\, \dot{e}_b + K_i \int^t_0 e_b\, d\tau\right) +``` + +where the body-frame error and velocity error are + +```math +e_b = J^{-1}\,\tilde{\varepsilon}, \qquad +\dot{e}_b = \nu - J^{-1}\,\dot{\eta}_d +``` + +**Error state** $\tilde{\varepsilon} \in \mathbb{R}^6$: + ```math -\tau = -J_{q}^{\dagger}(K_p \tilde{\eta} + K_d \dot{\tilde{\eta}} + K_i \int^t_0 \tilde{\eta}(\tau)d\tau) +\tilde{\varepsilon} = \begin{bmatrix} \Delta p \\ \varepsilon_q \end{bmatrix}, \quad +\Delta p = p - p_d \in \mathbb{R}^3, \quad +\varepsilon_q = \begin{bmatrix} \tilde{q}_x \\ \tilde{q}_y \\ \tilde{q}_z \end{bmatrix} +``` + +where $\tilde{q} = q \ominus q_d$ is the quaternion error. The sign convention $\tilde{q}_w \geq 0$ is enforced to guarantee the shortest-path rotation. + +**6×6 Jacobian** $J = \begin{bmatrix} R & 0 \\ 0 & T_{33} \end{bmatrix}$: + +- $R \in SO(3)$ — rotation matrix from body to world frame. +- $T_{33} \in \mathbb{R}^{3 \times 3}$ — lower three rows of the quaternion kinematic matrix mapping body angular rates to $\dot{\varepsilon}_q$. + +**Inverse Jacobian** $J^{-1} = \begin{bmatrix} R^\top & 0 \\ 0 & I_3 \end{bmatrix}$: + +The exact inverse of $T_{33}$ is approximated by $I_3$. When the Jacobian is near-singular the right Moore–Penrose pseudoinverse is used instead. + + +## 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 ``` -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. +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 +``` diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index 2a77ffd59..276faf697 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -1,5 +1,21 @@ + /**: 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: 50.0 + Kp_y: 50.0 + Kp_z: 25.0 + Kp_roll: 50.0 + Kp_pitch: 80.0 + Kp_yaw: 50.0 + Ki_x: 0.00 + Ki_y: 0.00 + Ki_z: 0.01 + Ki_roll: 0.07 + Ki_pitch: 0.10 + Ki_yaw: 0.0 + Kd_x: 120.0 + Kd_y: 120.0 + Kd_z: 250.0 + Kd_roll: 90.0 + Kd_pitch: 120.0 + Kd_yaw: 90.0 diff --git a/control/pid_controller_dp/config/pid_params_physical.yaml b/control/pid_controller_dp/config/pid_params_physical.yaml new file mode 100644 index 000000000..226d6b219 --- /dev/null +++ b/control/pid_controller_dp/config/pid_params_physical.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + Kp_x: -80.0 + Kp_y: -90.0 + Kp_z: 120.0 + Kp_roll: 35.0 + Kp_pitch: 35.0 + Kp_yaw: 60.0 + Ki_x: 0.00000006 + Ki_y: 0.00000006 + Ki_z: 0.2 + Ki_roll: 0.35 + Ki_pitch: 0.35 + Ki_yaw: 0.005 + Kd_x: 235.0 + Kd_y: 240.0 + Kd_z: 135.0 + Kd_roll: 135.0 + Kd_pitch: 135.0 + Kd_yaw: 125.0 diff --git a/control/pid_controller_dp/config/pid_params_sim.yaml b/control/pid_controller_dp/config/pid_params_sim.yaml new file mode 100644 index 000000000..487640b84 --- /dev/null +++ b/control/pid_controller_dp/config/pid_params_sim.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + Kp_x: 50.0 + Kp_y: 50.0 + Kp_z: 25.0 + Kp_roll: 50.0 + Kp_pitch: 80.0 + Kp_yaw: 50.0 + Ki_x: 0.00 + Ki_y: 0.00 + Ki_z: 0.01 + Ki_roll: 0.07 + Ki_pitch: 0.10 + Ki_yaw: 0.0 + Kd_x: 120.0 + Kd_y: 120.0 + Kd_z: 250.0 + Kd_roll: 90.0 + Kd_pitch: 120.0 + Kd_yaw: 90.0 diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp index 1017b5bd6..16b0013a1 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp @@ -1,46 +1,61 @@ #ifndef PID_CONTROLLER_DP__PID_CONTROLLER_HPP_ #define PID_CONTROLLER_DP__PID_CONTROLLER_HPP_ +#include #include "pid_controller_dp/typedefs.hpp" class PIDController { public: PIDController(); - // @brief Calculate the control input tau - // @param eta: struct containing the vehicle pose [position, orientation] - // @param eta_d: struct containing the desired vehicle pose [position, - // orientation] - // @param nu: struct containing the vehicle velocity [linear, angular] - // @param eta_dot_d: struct containing the derivative of the desired vehicle - // pose [position, orientation] - // @return 6D vector containing the control input tau [X, Y, Z, K, M, N] + /** + * @brief Calculate the control input tau. + * @param eta Struct containing the vehicle pose [position, orientation] + * @param eta_d Struct containing the desired vehicle pose [position, + * orientation] + * @param nu Struct containing the vehicle velocity [linear, angular] + * @param eta_dot_d Struct containing the derivative of the desired vehicle + * pose [position, orientation] + * @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 + /** + * @brief Set the proportional gain matrix. + * @param Kp 6×6 proportional gain matrix + */ void set_kp(const types::Matrix6d& Kp); - // @brief Set the integral gain matrix - // @param Ki: 6x6 matrix containing the integral gain matrix + /** + * @brief Set the integral gain matrix. + * @param Ki 6×6 integral gain matrix + */ void set_ki(const types::Matrix6d& Ki); - // @brief Set the derivative gain matrix - // @param Kd: 6x6 matrix containing the derivative gain matrix + /** + * @brief Set the derivative gain matrix. + * @param Kd 6×6 derivative gain matrix + */ void set_kd(const types::Matrix6d& Kd); - // @brief Set the time step - // @param dt: Time step + /** + * @brief Set the controller time step. + * @param dt Time step in seconds + */ 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_; types::Matrix6d Kd_; - types::Vector7d integral_; + types::Vector6d integral_; double dt_; }; diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp deleted file mode 100644 index a28097e9f..000000000 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ -#define PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ - -#include -#include -#include -#include -#include -#include -#include "pid_controller_dp/typedefs.hpp" - -types::Eta eta_convert_from_ros_to_eigen( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - -types::Nu nu_convert_from_ros_to_eigen( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - -#endif // PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp index 9b302d1af..e3346f764 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp @@ -2,67 +2,72 @@ #define PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_ #include -#include -#include -#include #include #include +#include #include #include -#include -#include #include -#include +#include #include #include -#include +#include #include #include "pid_controller_dp/pid_controller.hpp" #include "pid_controller_dp/typedefs.hpp" -// @brief Class for the PID controller node +/** @brief ROS 2 node wrapping the PID dynamic positioning controller. */ class PIDControllerNode : public rclcpp::Node { public: - PIDControllerNode(); + explicit PIDControllerNode(const rclcpp::NodeOptions& options); private: - // @brief Callback function for the killswitch topic - // @param msg: Bool message containing the killswitch status + /** + * @brief Callback for the killswitch topic. + * @param msg Bool message containing the killswitch status + */ void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg); - // @brief Callback function for the software mode topic - // @param msg: String message containing the software mode + /** + * @brief Callback for the operation mode topic. + * @param msg OperationMode message containing the current software mode + */ 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 + /** @brief Timer callback that computes and publishes the wrench tau. */ void publish_tau(); - // @brief Set the PID controller parameters + /** @brief Read PID gain parameters from the ROS parameter server. */ void set_pid_params(); - // @brief Set the subscriber and publisher for the node + /** @brief Create all subscribers and the wrench publisher. */ void set_subscribers_and_publisher(); - // @brief Initialize the operation mode by calling the GetOperationMode - // service + /** @brief Query the GetOperationMode service to initialise the operation + * mode. */ void initialize_operation_mode(); - // @brief Callback function for the guidance topic - // @param msg: ReferenceFilter message containing the desired vehicle pose - // and velocity + /** + * @brief Callback for the guidance topic. + * @param msg ReferenceFilterQuat message with desired pose and velocity + */ void guidance_callback( - const vortex_msgs::msg::ReferenceFilter::SharedPtr msg); + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg); + + /** + * @brief Callback for the odometry topic. + * @param msg Odometry message containing the AUV pose and body velocity + */ + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + + /** + * @brief Callback invoked when ROS parameters are updated at runtime. + * @param parameters Vector of parameters that were set + * @return Result indicating success or failure of the parameter update + */ + rcl_interfaces::msg::SetParametersResult parametersCallback( + const std::vector& parameters); rclcpp::Client::SharedPtr get_operation_mode_client_; @@ -74,21 +79,11 @@ class PIDControllerNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr operation_mode_sub_; - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; - - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr guidance_sub_; - rclcpp::Subscription::SharedPtr kp_sub_; - - rclcpp::Subscription::SharedPtr ki_sub_; - - rclcpp::Subscription::SharedPtr kd_sub_; - rclcpp::Publisher::SharedPtr tau_pub_; rclcpp::TimerBase::SharedPtr tau_pub_timer_; @@ -107,6 +102,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_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp index 1bdce214b..c5d311192 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp @@ -1,74 +1,90 @@ #ifndef PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ #define PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ -#include -#include +#include #include #include -#include +#include #include "pid_controller_dp/typedefs.hpp" +#include "typedefs.hpp" -// @brief Calculate the sine of an angle in degrees -// @param angle: Angle in degrees -// @return Smallest sine angle of the input +/** + * @brief Smallest signed angle (SSA) — wraps an angle to [-π, π]. + * @param angle Angle in radians + * @return Equivalent angle in [-π, π] + */ double ssa(double angle); -// @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 +/** + * @brief Compute the rotation matrix from body to world frame. + * + * REF: Fossen, "Handbook of Marine Craft Hydrodynamics and Motion Control", + * 2021, p.34, eq. 2.72. + * + * @param eta Vehicle pose containing the unit quaternion [qw, qx, qy, qz] + * @return 3×3 rotation matrix R ∈ SO(3) + */ 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 Compute the 3×3 quaternion kinematic sub-matrix T₃₃. + * + * Returns the bottom three rows of the full 4×3 quaternion transformation + * matrix T, giving the mapping from body angular velocity to + * d/dt [qx, qy, qz]. + * + * REF: Fossen, "Handbook of Marine Craft Hydrodynamics and Motion Control", + * 2021, p.35, eq. 2.78. + * + * @param eta Vehicle pose containing the unit quaternion + * @return 3×3 matrix T₃₃ + */ +types::Matrix3d 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 +/** + * @brief Compute the inverse of the 6×6 Jacobian: J⁻¹ = blockdiag(Rᵀ, I₃). + * + * The exact inverse of T₃₃ is approximated by I₃. When the Jacobian is + * near-singular the right Moore–Penrose pseudoinverse is used instead. + * + * @param eta Vehicle pose [x, y, z, qw, qx, qy, qz] + * @return 6×6 inverse Jacobian matrix + */ +types::Matrix6d calculate_J_sudo_inv(const types::Eta& eta); +/** + * @brief Compute the 6D quaternion error state ε̃ = [Δp; ε_q]. + * + * The scalar part q̃_w is not returned. The sign of the quaternion error is + * flipped when q̃_w < 0 to enforce the shortest-path convention. + * + * @param eta Actual vehicle pose [x, y, z, qw, qx, qy, qz] + * @param eta_d Desired vehicle pose [x, y, z, qw, qx, qy, qz] + * @return Eta struct whose [qx, qy, qz] fields carry ε_q (qw is discarded) + */ 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 +/** + * @brief Element-wise clamp of a vector to [min_val, max_val]. + * @param values Input vector + * @param min_val Lower bound + * @param max_val Upper bound + * @return Clamped vector of the same size + */ 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 Integrate the body-frame error with anti-windup clamping. + * @param dt Time step in seconds + * @param error_body 6D error in body frame [surge, sway, heave, roll, pitch, + * yaw] + * @param integral Current 6D integral accumulator + * @return Updated 6D integral after clamping + */ +types::Vector6d anti_windup(const double dt, + const types::Vector6d& error_body, + const types::Vector6d& integral); #endif // PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp index beeea3174..5b8a65373 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp @@ -7,50 +7,33 @@ #define PID_CONTROLLER_DP__TYPEDEFS_HPP_ #include +#include namespace types { -typedef Eigen::Matrix Vector3d; -typedef Eigen::Matrix Vector6d; -typedef Eigen::Matrix Vector7d; -typedef Eigen::Matrix Vector4d; -typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Matrix3d; -typedef Eigen::Matrix Matrix4x3d; -typedef Eigen::Matrix Matrix7x6d; -typedef Eigen::Matrix Matrix6x7d; -typedef Eigen::Matrix 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; - } -}; +using Vector3d = Eigen::Matrix; +using Vector4d = Eigen::Matrix; +using Vector6d = Eigen::Matrix; +using Vector7d = Eigen::Matrix; +using Matrix3d = Eigen::Matrix; +using Matrix4x3d = Eigen::Matrix; +using Matrix6d = Eigen::Matrix; +using Matrix7x6d = Eigen::Matrix; +using Matrix6x7d = Eigen::Matrix; +using Matrix7d = Eigen::Matrix; +using Quaterniond = Eigen::Quaterniond; + +// Alias canonical types from vortex utils +using Eta = ::vortex::utils::types::Pose; +using Nu = ::vortex::utils::types::Twist; struct J_transformation { - Eigen::Matrix3d R = types::Matrix3d::Identity(); - types::Matrix4x3d T = types::Matrix4x3d::Zero(); + Matrix3d R = Matrix3d::Identity(); + Matrix3d T = Matrix3d::Zero(); - types::Matrix7x6d as_matrix() const { - types::Matrix7x6d mat = types::Matrix7x6d::Zero(); + Matrix6d as_matrix() const { + Matrix6d mat = Matrix6d::Zero(); mat.block<3, 3>(0, 0) = R; - mat.block<4, 3>(3, 3) = T; + mat.block<3, 3>(3, 3) = T; return mat; } }; diff --git a/control/pid_controller_dp/package.xml b/control/pid_controller_dp/package.xml index dd13d899e..0ec8c9b80 100644 --- a/control/pid_controller_dp/package.xml +++ b/control/pid_controller_dp/package.xml @@ -10,11 +10,13 @@ ament_cmake rclcpp + rclcpp_components geometry_msgs nav_msgs eigen tf2 vortex_msgs + rcl_interfaces vortex_utils vortex_utils_ros diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index 427c7dcdf..2b523248e 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -1,34 +1,109 @@ #include "pid_controller_dp/pid_controller.hpp" #include "pid_controller_dp/pid_controller_utils.hpp" +void print_eta(const types::Eta& eta) { + spdlog::info("Eta values:"); + auto pos = eta.pos_vector(); + auto ori = eta.ori_quaternion(); + spdlog::info("Position - North: {}, East: {}, Down: {}", pos[0], pos[1], + pos[2]); + spdlog::info("Orientation - w: {}, x: {}, y: {}, z: {}", ori.w(), ori.x(), + ori.y(), ori.z()); +} + +void print_nu(const types::Nu& nu) { + spdlog::info("Nu values:"); + auto v = nu.to_vector(); + spdlog::info("Linear Speed - u: {}, v: {}, w: {}", v(0), v(1), v(2)); + spdlog::info("Angular Speed - p: {}, q: {}, r: {}", v(3), v(4), v(5)); +} + +void print_vect_6d(const types::Vector6d& vec) { + spdlog::info("Vector6d values:"); + for (int i = 0; i < 6; ++i) { + spdlog::info("Element[{}]: {}", i, vec[i]); + } +} + +void print_J_transformation(const types::J_transformation& J) { + spdlog::info("J_transformation:"); + + spdlog::info("R (3x3) elements:"); + for (int i = 0; i < J.R.rows(); ++i) { + for (int j = 0; j < J.R.cols(); ++j) { + spdlog::info("R[{},{}] = {}", i, j, J.R(i, j)); + } + } + + spdlog::info("T (4x3) elements:"); + for (int i = 0; i < J.T.rows(); ++i) { + for (int j = 0; j < J.T.cols(); ++j) { + spdlog::info("T[{},{}] = {}", i, j, J.T(i, j)); + } + } + + spdlog::info("Combined Matrix (7x6) elements:"); + auto M = J.as_matrix(); + for (int i = 0; i < M.rows(); ++i) { + for (int j = 0; j < M.cols(); ++j) { + spdlog::info("M[{},{}] = {}", i, j, M(i, j)); + } + } +} + +void print_Jinv_transformation(const types::Matrix6x7d& J_inv) { + spdlog::info("J_pseudo_inverse (6x7):"); + for (int i = 0; i < J_inv.rows(); ++i) { + std::string row; + row.reserve(128); + row += "["; + for (int j = 0; j < J_inv.cols(); ++j) { + row += std::to_string(J_inv(i, j)); + if (j < J_inv.cols() - 1) + row += ", "; + } + row += "]"; + spdlog::info("{}", row); + } +} + PIDController::PIDController() : Kp_(types::Matrix6d::Identity()), Ki_(types::Matrix6d::Zero()), Kd_(types::Matrix6d::Zero()), - integral_(types::Vector7d::Zero()), + integral_(types::Vector6d::Zero()), dt_(0.01) {} types::Vector6d PIDController::calculate_tau(const types::Eta& eta, const types::Eta& eta_d, const types::Nu& nu, const types::Eta& eta_dot_d) { - types::Eta error = error_eta(eta, eta_d); + types::Eta error = error_eta(eta, eta_d); // calculate eta error - types::Matrix6x7d J_inv = calculate_J_sudo_inv(error); + // Only use the vector part of the quaternion for orientation error. + types::Vector6d error_6; + error_6 << error.x, error.y, error.z, error.qx, error.qy, error.qz; - types::Vector6d nu_d = J_inv * eta_dot_d.as_vector(); + // Desired velocity: also drop qw, keeping [x,y,z,qx,qy,qz] derivatives. + types::Vector6d eta_dot_d_6; + eta_dot_d_6 << eta_dot_d.x, eta_dot_d.y, eta_dot_d.z, eta_dot_d.qx, + eta_dot_d.qy, eta_dot_d.qz; - types::Vector6d error_nu = nu.as_vector() - nu_d; + // 6x6 J inverse: blockdiag(R, T_33)^{-1} + types::Matrix6d J_inv = calculate_J_sudo_inv(eta); - types::Vector6d P = Kp_ * J_inv * error.as_vector(); + types::Vector6d error_body = J_inv * error_6; // error in body frame - types::Vector6d I = Ki_ * J_inv * integral_; + types::Vector6d nu_d = J_inv * eta_dot_d_6; // desired body velocity + types::Vector6d error_nu = nu.to_vector() - nu_d; // velocity error - types::Vector6d D = Kd_ * error_nu; + types::Vector6d P = Kp_ * error_body; // P term + types::Vector6d I = Ki_ * integral_; // I term + types::Vector6d D = Kd_ * error_nu; // D term - types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); + types::Vector6d tau = -clamp_values((P + I + D), -90.0, 90.0); - integral_ = anti_windup(dt_, error, integral_); + integral_ = anti_windup(dt_, error_body, integral_); return tau; } @@ -48,3 +123,13 @@ void PIDController::set_kd(const types::Matrix6d& Kd) { void PIDController::set_time_step(double dt) { this->dt_ = dt; } + +types::Matrix6d PIDController::get_kp() { + return this->Kp_; +} +types::Matrix6d PIDController::get_ki() { + return this->Ki_; +} +types::Matrix6d PIDController::get_kd() { + return this->Kd_; +} diff --git a/control/pid_controller_dp/src/pid_controller_conversions.cpp b/control/pid_controller_dp/src/pid_controller_conversions.cpp deleted file mode 100644 index 8f6ff1970..000000000 --- a/control/pid_controller_dp/src/pid_controller_conversions.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "pid_controller_dp/pid_controller_conversions.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "pid_controller_dp/pid_controller.hpp" -#include "pid_controller_dp/pid_controller_utils.hpp" -#include "pid_controller_dp/typedefs.hpp" - -types::Eta eta_convert_from_ros_to_eigen( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - types::Eta eta; - eta.pos << msg->pose.pose.position.x, msg->pose.pose.position.y, - msg->pose.pose.position.z; - eta.ori.w() = msg->pose.pose.orientation.w; - eta.ori.x() = msg->pose.pose.orientation.x; - eta.ori.y() = msg->pose.pose.orientation.y; - eta.ori.z() = msg->pose.pose.orientation.z; - - return eta; -} - -types::Nu nu_convert_from_ros_to_eigen( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - types::Nu nu; - nu.linear_speed << msg->twist.twist.linear.x, msg->twist.twist.linear.y, - msg->twist.twist.linear.z; - nu.angular_speed << msg->twist.twist.angular.x, msg->twist.twist.angular.y, - msg->twist.twist.angular.z; - return nu; -} diff --git a/control/pid_controller_dp/src/pid_controller_node.cpp b/control/pid_controller_dp/src/pid_controller_node.cpp index f9bf44663..433ddf4bf 100644 --- a/control/pid_controller_dp/src/pid_controller_node.cpp +++ b/control/pid_controller_dp/src/pid_controller_node.cpp @@ -2,8 +2,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started PID Controller Node"); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); rclcpp::shutdown(); return 0; } diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 44a63cc63..db4105855 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -1,21 +1,37 @@ #include -#include +#include +#include #include #include -#include -#include "pid_controller_dp/pid_controller_conversions.hpp" #include "pid_controller_dp/pid_controller_utils.hpp" #include "pid_controller_dp/typedefs.hpp" -PIDControllerNode::PIDControllerNode() : Node("pid_controller_node") { +constexpr std::string_view start_message = R"( +██████╗ ██╗██████╗ ██████╗ ██████╗ ██████╗ ██╗ ██╗ █████╗ ████████╗ +██╔══██╗██║██╔══██╗ ██╔══██╗██╔══██╗ ██╔═══██╗██║ ██║██╔══██╗╚══██╔══╝ +██████╔╝██║██║ ██║ ██║ ██║██████╔╝ ██║ ██║██║ ██║███████║ ██║ +██╔═══╝ ██║██║ ██║ ██║ ██║██╔═══╝ ██║▄▄ ██║██║ ██║██╔══██║ ██║ +██║ ██║██████╔╝ ██████╔╝██║ ╚██████╔╝╚██████╔╝██║ ██║ ██║ +╚═╝ ╚═╝╚═════╝ ╚═════╝ ╚═╝ ╚══▀▀═╝ ╚═════╝ ╚═╝ ╚═╝ ╚═╝ +)"; + +PIDControllerNode::PIDControllerNode(const rclcpp::NodeOptions& options) + : Node("pid_controller_node", options) { time_step_ = std::chrono::milliseconds(10); set_subscribers_and_publisher(); initialize_operation_mode(); - tau_pub_timer_ = this->create_wall_timer( - time_step_, std::bind(&PIDControllerNode::publish_tau, this)); + tau_pub_timer_ = + this->create_wall_timer(time_step_, [this]() { publish_tau(); }); set_pid_params(); + + callback_handle_ = this->add_on_set_parameters_callback( + [this](const std::vector& params) { + return parametersCallback(params); + }); + + spdlog::info(start_message); } void PIDControllerNode::set_subscribers_and_publisher() { @@ -24,15 +40,12 @@ void PIDControllerNode::set_subscribers_and_publisher() { rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)}; - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_quat"); std::string dp_reference_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_quat").as_string(); - this->declare_parameter("topics.pose"); - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - - this->declare_parameter("topics.twist"); - std::string twist_topic = this->get_parameter("topics.twist").as_string(); + this->declare_parameter("topics.odom"); + std::string odom_topic = this->get_parameter("topics.odom").as_string(); this->declare_parameter("topics.killswitch"); std::string software_kill_switch_topic = @@ -48,31 +61,28 @@ void PIDControllerNode::set_subscribers_and_publisher() { killswitch_sub_ = this->create_subscription( software_kill_switch_topic, qos_reliable, - std::bind(&PIDControllerNode::killswitch_callback, this, - std::placeholders::_1)); + [this](const std_msgs::msg::Bool::SharedPtr msg) { + killswitch_callback(msg); + }); operation_mode_sub_ = this->create_subscription( software_operation_mode_topic, qos_reliable, - std::bind(&PIDControllerNode::operation_mode_callback, this, - std::placeholders::_1)); - - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&PIDControllerNode::pose_callback, this, - std::placeholders::_1)); - - twist_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( - twist_topic, qos_sensor_data, - std::bind(&PIDControllerNode::twist_callback, this, - std::placeholders::_1)); + [this](const vortex_msgs::msg::OperationMode::SharedPtr msg) { + operation_mode_callback(msg); + }); + + odom_sub_ = this->create_subscription( + odom_topic, qos_sensor_data, + [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + odom_callback(msg); + }); guidance_sub_ = - this->create_subscription( + this->create_subscription( dp_reference_topic, qos_sensor_data, - std::bind(&PIDControllerNode::guidance_callback, this, - std::placeholders::_1)); + [this](const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) { + guidance_callback(msg); + }); tau_pub_ = this->create_publisher( control_topic, qos_sensor_data); @@ -121,14 +131,16 @@ void PIDControllerNode::operation_mode_callback( operation_mode_ = vortex::utils::ros_conversions::convert_from_ros(*msg); } -void PIDControllerNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - eta_ = eta_convert_from_ros_to_eigen(msg); -} - -void PIDControllerNode::twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - nu_ = nu_convert_from_ros_to_eigen(msg); +void PIDControllerNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + eta_ = vortex::utils::ros_conversions::ros_pose_to_pose(msg->pose.pose); + if (eta_.qw < 0.0) { + eta_.qw = -eta_.qw; + eta_.qx = -eta_.qx; + eta_.qy = -eta_.qy; + eta_.qz = -eta_.qz; + } + nu_ = vortex::utils::ros_conversions::ros_twist_to_twist(msg->twist.twist); } void PIDControllerNode::publish_tau() { @@ -154,20 +166,57 @@ void PIDControllerNode::publish_tau() { } void PIDControllerNode::set_pid_params() { - this->declare_parameter>( - "Kp", {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}); - this->declare_parameter>( - "Ki", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - this->declare_parameter>( - "Kd", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - - std::vector Kp_vec = this->get_parameter("Kp").as_double_array(); - std::vector Ki_vec = this->get_parameter("Ki").as_double_array(); - std::vector Kd_vec = this->get_parameter("Kd").as_double_array(); - - types::Matrix6d Kp_eigen = Eigen::Map(Kp_vec.data()); - types::Matrix6d Ki_eigen = Eigen::Map(Ki_vec.data()); - types::Matrix6d Kd_eigen = Eigen::Map(Kd_vec.data()); + this->declare_parameter("Kp_x", 1.0); + this->declare_parameter("Kp_y", 1.0); + this->declare_parameter("Kp_z", 1.0); + this->declare_parameter("Kp_roll", 1.0); + this->declare_parameter("Kp_pitch", 1.0); + this->declare_parameter("Kp_yaw", 1.0); + this->declare_parameter("Ki_x", 0.1); + this->declare_parameter("Ki_y", 0.1); + this->declare_parameter("Ki_z", 0.1); + this->declare_parameter("Ki_roll", 0.1); + this->declare_parameter("Ki_pitch", 0.1); + this->declare_parameter("Ki_yaw", 0.1); + this->declare_parameter("Kd_x", 0.1); + this->declare_parameter("Kd_y", 0.1); + this->declare_parameter("Kd_z", 0.1); + this->declare_parameter("Kd_roll", 0.1); + this->declare_parameter("Kd_pitch", 0.1); + this->declare_parameter("Kd_yaw", 0.1); + + std::vector Kp_vec = { + this->get_parameter("Kp_x").as_double(), + this->get_parameter("Kp_y").as_double(), + this->get_parameter("Kp_z").as_double(), + this->get_parameter("Kp_roll").as_double(), + this->get_parameter("Kp_pitch").as_double(), + this->get_parameter("Kp_yaw").as_double(), + }; + std::vector Ki_vec = { + this->get_parameter("Ki_x").as_double(), + this->get_parameter("Ki_y").as_double(), + this->get_parameter("Ki_z").as_double(), + this->get_parameter("Ki_roll").as_double(), + this->get_parameter("Ki_pitch").as_double(), + this->get_parameter("Ki_yaw").as_double(), + }; + std::vector Kd_vec = { + this->get_parameter("Kd_x").as_double(), + this->get_parameter("Kd_y").as_double(), + this->get_parameter("Kd_z").as_double(), + this->get_parameter("Kd_roll").as_double(), + this->get_parameter("Kd_pitch").as_double(), + this->get_parameter("Kd_yaw").as_double(), + }; + + types::Vector6d Kp_vec_eigen(Kp_vec.data()); + types::Vector6d Ki_vec_eigen(Ki_vec.data()); + types::Vector6d Kd_vec_eigen(Kd_vec.data()); + + types::Matrix6d Kp_eigen = Kp_vec_eigen.asDiagonal().toDenseMatrix(); + types::Matrix6d Ki_eigen = Ki_vec_eigen.asDiagonal().toDenseMatrix(); + types::Matrix6d Kd_eigen = Kd_vec_eigen.asDiagonal().toDenseMatrix(); pid_controller_.set_kp(Kp_eigen); pid_controller_.set_ki(Ki_eigen); @@ -175,14 +224,147 @@ void PIDControllerNode::set_pid_params() { } void PIDControllerNode::guidance_callback( - const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { - eta_d_.pos << msg->x, msg->y, msg->z; + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) { + eta_d_.x = msg->x; + eta_d_.y = msg->y; + eta_d_.z = msg->z; + + // Enforce positive hemisphere so eta_d_ and eta_ stay in the same half of + // the double cover, reducing sign-flip errors in the PID error computation. + const double sign = msg->qw >= 0.0 ? 1.0 : -1.0; + eta_d_.qw = sign * msg->qw; + eta_d_.qx = sign * msg->qx; + eta_d_.qy = sign * msg->qy; + eta_d_.qz = sign * msg->qz; + + // Desired velocity feedforward (x/y/z in world frame; roll/pitch/yaw are + // the body-frame angular-velocity components from the reference filter). + eta_dot_d_.x = msg->x_dot; + eta_dot_d_.y = msg->y_dot; + eta_dot_d_.z = msg->z_dot; + eta_dot_d_.qx = msg->roll_dot; + eta_dot_d_.qy = msg->pitch_dot; + eta_dot_d_.qz = msg->yaw_dot; +} - double roll = msg->roll; - double pitch = msg->pitch; - double yaw = msg->yaw; +rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( + const std::vector& parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + bool kp_x_updated = false; + bool kp_y_updated = false; + bool kp_z_updated = false; + bool kp_roll_updated = false; + bool kp_pitch_updated = false; + bool kp_yaw_updated = false; + + bool ki_x_updated = false; + bool ki_y_updated = false; + bool ki_z_updated = false; + bool ki_roll_updated = false; + bool ki_pitch_updated = false; + bool ki_yaw_updated = false; + + bool kd_x_updated = false; + bool kd_y_updated = false; + bool kd_z_updated = false; + bool kd_roll_updated = false; + bool kd_pitch_updated = false; + bool kd_yaw_updated = false; + + types::Vector6d Kp_vec_eigen = pid_controller_.get_kp().diagonal(); + types::Vector6d Ki_vec_eigen = pid_controller_.get_ki().diagonal(); + types::Vector6d Kd_vec_eigen = pid_controller_.get_kd().diagonal(); + + for (const auto& param : parameters) { + if (param.get_name() == "Kp_x") { + Kp_vec_eigen(0) = param.as_double(); + kp_x_updated = true; + } else if (param.get_name() == "Kp_y") { + Kp_vec_eigen(1) = param.as_double(); + kp_y_updated = true; + } else if (param.get_name() == "Kp_z") { + Kp_vec_eigen(2) = param.as_double(); + kp_z_updated = true; + } else if (param.get_name() == "Kp_roll") { + Kp_vec_eigen(3) = param.as_double(); + kp_roll_updated = true; + } else if (param.get_name() == "Kp_pitch") { + Kp_vec_eigen(4) = param.as_double(); + kp_pitch_updated = true; + } else if (param.get_name() == "Kp_yaw") { + Kp_vec_eigen(5) = param.as_double(); + kp_yaw_updated = true; + } else if (param.get_name() == "Ki_x") { + Ki_vec_eigen(0) = param.as_double(); + ki_x_updated = true; + } else if (param.get_name() == "Ki_y") { + Ki_vec_eigen(1) = param.as_double(); + ki_y_updated = true; + } else if (param.get_name() == "Ki_z") { + Ki_vec_eigen(2) = param.as_double(); + ki_z_updated = true; + } else if (param.get_name() == "Ki_roll") { + Ki_vec_eigen(3) = param.as_double(); + ki_roll_updated = true; + } else if (param.get_name() == "Ki_pitch") { + Ki_vec_eigen(4) = param.as_double(); + ki_pitch_updated = true; + } else if (param.get_name() == "Ki_yaw") { + Ki_vec_eigen(5) = param.as_double(); + ki_yaw_updated = true; + } else if (param.get_name() == "Kd_x") { + Kd_vec_eigen(0) = param.as_double(); + kd_x_updated = true; + } else if (param.get_name() == "Kd_y") { + Kd_vec_eigen(1) = param.as_double(); + kd_y_updated = true; + } else if (param.get_name() == "Kd_z") { + Kd_vec_eigen(2) = param.as_double(); + kd_z_updated = true; + } else if (param.get_name() == "Kd_roll") { + Kd_vec_eigen(3) = param.as_double(); + kd_roll_updated = true; + } else if (param.get_name() == "Kd_pitch") { + Kd_vec_eigen(4) = param.as_double(); + kd_pitch_updated = true; + } else if (param.get_name() == "Kd_yaw") { + Kd_vec_eigen(5) = param.as_double(); + kd_yaw_updated = true; + } + } - eta_d_.ori = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + // Only set the gains if the parameter update was successful + if (result.successful) { + if (kp_x_updated || kp_y_updated || kp_z_updated || kp_roll_updated || + kp_pitch_updated || kp_yaw_updated) { + types::Matrix6d Kp_eigen = + Kp_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_kp(Kp_eigen); + } + if (ki_x_updated || ki_y_updated || ki_z_updated || ki_roll_updated || + ki_pitch_updated || ki_yaw_updated) { + types::Matrix6d Ki_eigen = + Ki_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_ki(Ki_eigen); + } + if (kd_x_updated || kd_y_updated || kd_z_updated || kd_roll_updated || + kd_pitch_updated || kd_yaw_updated) { + types::Matrix6d Kd_eigen = + Kd_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_kd(Kd_eigen); + } + } + + // print + for (const auto& param : parameters) { + RCLCPP_INFO(this->get_logger(), "%s", param.get_name().c_str()); + RCLCPP_INFO(this->get_logger(), "%s", param.get_type_name().c_str()); + RCLCPP_INFO(this->get_logger(), "%s", param.value_to_string().c_str()); + } + return result; } + +RCLCPP_COMPONENTS_REGISTER_NODE(PIDControllerNode) diff --git a/control/pid_controller_dp/src/pid_controller_utils.cpp b/control/pid_controller_dp/src/pid_controller_utils.cpp index 246e15500..411b6808e 100644 --- a/control/pid_controller_dp/src/pid_controller_utils.cpp +++ b/control/pid_controller_dp/src/pid_controller_utils.cpp @@ -1,75 +1,48 @@ #include "pid_controller_dp/pid_controller_utils.hpp" #include -#include "pid_controller_dp/pid_controller_conversions.hpp" +#include +#include #include "pid_controller_dp/typedefs.hpp" types::Matrix3d calculate_R_quat(const types::Eta& eta) { - return eta.ori.normalized().toRotationMatrix(); + return eta.as_rotation_matrix(); } -types::Matrix4x3d calculate_T_quat(const types::Eta& eta) { - types::Quaterniond quaternion_norm = eta.ori.normalized(); - - double w = quaternion_norm.w(); - double x = quaternion_norm.x(); - double y = quaternion_norm.y(); - double z = quaternion_norm.z(); - - types::Matrix4x3d transformation_matrix; - - transformation_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w; - - return transformation_matrix * 0.5; +types::Matrix3d calculate_T_quat(const types::Eta& eta) { + // Full T is 4x3; rows 1-3 map body angular velocity to d/dt[qx, qy, qz]. + return eta.as_transformation_matrix().bottomRows<3>(); } -types::Matrix6x7d calculate_J_sudo_inv(const types::Eta& eta) { - types::Eta eta_norm; - - eta_norm.pos = eta.pos; - eta_norm.ori = eta.ori; - - types::Matrix3d R = calculate_R_quat(eta_norm); - types::Matrix4x3d T = calculate_T_quat(eta_norm); - - types::J_transformation J; - J.R = R; - J.T = T; - - types::Matrix6x7d J_transpose = J.as_matrix().transpose(); - types::Matrix6x7d J_pseudo_inv = - (J_transpose * J.as_matrix()).inverse() * J_transpose; - - return J_pseudo_inv; +types::Matrix6d calculate_J_sudo_inv(const types::Eta& eta) { + types::Matrix3d R = calculate_R_quat(eta); + types::Matrix6d J_inv = types::Matrix6d::Zero(); + J_inv.topLeftCorner<3, 3>() = R.transpose(); + J_inv.bottomRightCorner<3, 3>() = types::Matrix3d::Identity(); + return J_inv; } types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d) { - types::Eta eta_error; - - eta_error.pos = eta.pos - eta_d.pos; - eta_error.ori = eta_d.ori.conjugate() * eta.ori; - - eta_error.ori = eta_error.ori.normalized(); - - return eta_error; + types::Eta error = eta - eta_d; + // Enforce shortest path: q and -q represent the same rotation, but only + // qw >= 0 gives the correct sign for the vector part used as error signal. + if (error.qw < 0.0) { + error.qw = -error.qw; + error.qx = -error.qx; + error.qy = -error.qy; + error.qz = -error.qz; + } + return error; } Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, double min_val, double max_val) { - return values.cwiseMax(min_val).cwiseMin(max_val); + return vortex::utils::math::clamp_values(values, min_val, max_val); } -types::Vector7d anti_windup(const double dt, - const types::Eta& error, - const types::Vector7d& integral) { - types::Eta error_norm; - - error_norm.pos = error.pos; - error_norm.ori = error.ori; - - types::Vector7d integral_anti_windup = - integral + (error_norm.as_vector() * dt); - - integral_anti_windup = clamp_values(integral_anti_windup, -80.0, 80.0); - return integral_anti_windup; +types::Vector6d anti_windup(const double dt, + const types::Vector6d& error_body, + const types::Vector6d& integral) { + return vortex::utils::math::anti_windup(dt, error_body, integral, -50.0, + 50.0); } diff --git a/control/pid_controller_dp/test/CMakeLists.txt b/control/pid_controller_dp/test/CMakeLists.txt new file mode 100644 index 000000000..7b8da00db --- /dev/null +++ b/control/pid_controller_dp/test/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + pid_controller_tests.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${LIB_NAME} + GTest::GTest + spdlog::spdlog +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3 vortex_utils) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/control/pid_controller_dp/test/pid_controller_tests.cpp b/control/pid_controller_dp/test/pid_controller_tests.cpp new file mode 100644 index 000000000..18b2a7e6f --- /dev/null +++ b/control/pid_controller_dp/test/pid_controller_tests.cpp @@ -0,0 +1,522 @@ +#include +#include + +#include +#include +#include "pid_controller_dp/pid_controller.hpp" +#include "pid_controller_dp/pid_controller_utils.hpp" +#include "pid_controller_dp/typedefs.hpp" + +void print_tau(const types::Vector6d& tau) { + spdlog::info("Tau values:"); + spdlog::info("Surge: {}", tau[0]); + spdlog::info("Sway: {}", tau[1]); + spdlog::info("Heave: {}", tau[2]); + spdlog::info("Roll: {}", tau[3]); + spdlog::info("Pitch: {}", tau[4]); + spdlog::info("Yaw: {}", tau[5]); +} + +class PIDControllerTests : public ::testing::Test { + protected: + PIDControllerTests() : pid_controller_() { + // Set PID gains for testing + types::Matrix6d Kp = types::Matrix6d::Identity() * 10.0; + types::Matrix6d Ki = types::Matrix6d::Identity() * 0.5; + types::Matrix6d Kd = types::Matrix6d::Identity() * 2.0; + + pid_controller_.set_kp(Kp); + pid_controller_.set_ki(Ki); + pid_controller_.set_kd(Kd); + } + + types::Eta generate_current_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { + types::Eta current_pose; + current_pose.x = north_pos; + current_pose.y = east_pos; + current_pose.z = down_pos; + Eigen::Quaterniond q = vortex::utils::math::euler_to_quat( + roll_angle, pitch_angle, yaw_angle); + current_pose.qw = q.w(); + current_pose.qx = q.x(); + current_pose.qy = q.y(); + current_pose.qz = q.z(); + return current_pose; + } + + types::Eta generate_reference_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { + types::Eta reference_pose; + reference_pose.x = north_pos; + reference_pose.y = east_pos; + reference_pose.z = down_pos; + Eigen::Quaterniond q = vortex::utils::math::euler_to_quat( + roll_angle, pitch_angle, yaw_angle); + reference_pose.qw = q.w(); + reference_pose.qx = q.x(); + reference_pose.qy = q.y(); + reference_pose.qz = q.z(); + return reference_pose; + } + + types::Nu generate_current_velocity(const double surge_vel, + const double sway_vel, + const double heave_vel, + const double roll_rate, + const double pitch_rate, + const double yaw_rate) { + types::Nu current_velocity; + current_velocity.u = surge_vel; + current_velocity.v = sway_vel; + current_velocity.w = heave_vel; + current_velocity.p = roll_rate; + current_velocity.q = pitch_rate; + current_velocity.r = yaw_rate; + return current_velocity; + } + + PIDController pid_controller_; +}; + +/* +Test that negative north error only (in body) gives positive surge command only. +*/ + +TEST_F(PIDControllerTests, + T01_neg_north_error_with_zero_heading_gives_surge_only_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with positive heading gives a positive surge +command and negative sway command. +*/ + +TEST_F( + PIDControllerTests, + T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + EXPECT_GT(tau[0], 0.0); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with negative heading gives a positive surge +command and positive sway command. +*/ + +TEST_F( + PIDControllerTests, + T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and pitch gives a positive heave +command. +*/ + +TEST_F( + PIDControllerTests, + T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and negative pitch gives a positive +heave and positive surge command. +*/ + +TEST_F( + PIDControllerTests, + T05_neg_down_error_with_zero_roll_and_neg_pitch_gives_positive_heave_and_positive_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and positive pitch gives a positive +heave and negative surge command. +*/ + +TEST_F( + PIDControllerTests, + T06_neg_down_error_with_zero_roll_and_pos_pitch_gives_positive_heave_and_negative_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with zero heading gives a positive sway command. +*/ + +TEST_F(PIDControllerTests, + T07_neg_east_error_with_zero_heading_gives_positive_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive east error with zero heading gives a negative sway command. +*/ + +TEST_F(PIDControllerTests, + T08_pos_east_error_with_zero_heading_gives_pos_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with positive heading gives a positive surge and +sway command. +*/ + +TEST_F( + PIDControllerTests, + T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with negative heading gives a negative surge and +positive sway command. +*/ + +TEST_F( + PIDControllerTests, + T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_LT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative roll error gives positive roll command. +*/ + +TEST_F(PIDControllerTests, T11_neg_roll_error_gives_positive_roll_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_GT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive roll error gives negative roll command. +*/ + +TEST_F(PIDControllerTests, T12_pos_roll_error_gives_neg_roll_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_LT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative pitch error gives positive pitch command. +*/ + +TEST_F(PIDControllerTests, T13_neg_pitch_error_gives_pos_pitch_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_GT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive pitch error gives negative pitch command. +*/ + +TEST_F(PIDControllerTests, T14_pos_pitch_error_gives_neg_pitch_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_LT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative yaw error gives positive yaw command. +*/ + +TEST_F(PIDControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_GT(tau[5], 0.0); +} + +/* +Test that positive yaw error gives negative yaw command. +*/ + +TEST_F(PIDControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_LT(tau[5], 0.0); +} + +/* +Test that positive surge velocity only results in negative surge command +(breaking effect). +*/ + +TEST_F(PIDControllerTests, T17_pos_surge_vel_gives_negative_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive sway velocity only results in negative sway command (breaking +effect). +*/ + +TEST_F(PIDControllerTests, T18_pos_sway_vel_gives_negative_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive heave velocity only results in negative heave command +(breaking effect). +*/ + +TEST_F(PIDControllerTests, T19_pos_heave_vel_gives_negative_heave_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_LT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/control/pid_controller_dp_euler/CMakeLists.txt b/control/pid_controller_dp_euler/CMakeLists.txt index 9f01ca638..99682fe05 100644 --- a/control/pid_controller_dp_euler/CMakeLists.txt +++ b/control/pid_controller_dp_euler/CMakeLists.txt @@ -12,10 +12,8 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) -find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) -find_package(tf2 REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils REQUIRED) find_package(vortex_utils_ros REQUIRED) @@ -33,10 +31,9 @@ add_executable(pid_controller_node ament_target_dependencies(pid_controller_node rclcpp + std_msgs geometry_msgs - nav_msgs Eigen3 - tf2 vortex_msgs vortex_utils vortex_utils_ros diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp index 2d760bbd7..a2a696380 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp @@ -1,16 +1,12 @@ #ifndef PID_CONTROLLER_DP_EULER__PID_CONTROLLER_ROS_HPP_ #define PID_CONTROLLER_DP_EULER__PID_CONTROLLER_ROS_HPP_ -#include #include #include #include -#include #include #include #include -#include -#include #include #include #include diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp index f1db15a2d..d62868112 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp @@ -1,12 +1,9 @@ #ifndef PID_CONTROLLER_DP_EULER__PID_CONTROLLER_UTILS_HPP_ #define PID_CONTROLLER_DP_EULER__PID_CONTROLLER_UTILS_HPP_ -#include -#include #include #include #include -#include double ssa(double angle); diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp index 5d659fa93..9b39e66df 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp @@ -7,99 +7,13 @@ #define PID_CONTROLLER_DP_EULER__TYPEDEFS_HPP_ #include -#include +#include typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Matrix3d; typedef Eigen::Matrix Vector6d; -struct Eta { - double x = 0.0; - double y = 0.0; - double z = 0.0; - double roll = 0.0; - double pitch = 0.0; - double yaw = 0.0; - - Eta operator-(const Eta& other) const { - Eta eta; - eta.x = x - other.x; - eta.y = y - other.y; - eta.z = z - other.z; - eta.roll = roll - other.roll; - eta.pitch = pitch - other.pitch; - eta.yaw = yaw - other.yaw; - return eta; - } - - Vector6d to_vector() const { - Vector6d eta; - eta << x, y, z, roll, pitch, yaw; - return eta; - } - - // @brief Make the rotation matrix according to eq. 2.31 in Fossen, 2021 - Matrix3d as_rotation_matrix() const { - Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX()); - Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY()); - Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ()); - - Eigen::Quaterniond q = yaw_angle * pitch_angle * roll_angle; - - return q.toRotationMatrix(); - } - - // @brief Make the transformation matrix according to eq. 2.41 in Fossen, - // 2021 - Matrix3d as_transformation_matrix() const { - double cphi = cos(roll); - double sphi = sin(roll); - double ctheta = cos(pitch); - double stheta = sin(pitch); - - if (ctheta == 0) { - throw std::runtime_error( - "Division by zero in transformation matrix."); - } - Matrix3d T; - T(0, 0) = 1; - T(0, 1) = sphi * stheta / ctheta; - T(0, 2) = cphi * stheta / ctheta; - T(1, 0) = 0; - T(1, 1) = cphi; - T(1, 2) = -sphi; - T(2, 0) = 0; - T(2, 1) = sphi / ctheta; - T(2, 2) = cphi / ctheta; - - return T; - } -}; - -struct Nu { - double u = 0.0; - double v = 0.0; - double w = 0.0; - double p = 0.0; - double q = 0.0; - double r = 0.0; - - Nu operator-(const Nu& other) const { - Nu nu; - nu.u = u - other.u; - nu.v = v - other.v; - nu.w = w - other.w; - nu.p = p - other.p; - nu.q = q - other.q; - nu.r = r - other.r; - return nu; - } - - Vector6d to_vector() const { - Vector6d nu; - nu << u, v, w, p, q, r; - return nu; - } -}; +using Eta = ::vortex::utils::types::PoseEuler; +using Nu = ::vortex::utils::types::Twist; #endif // PID_CONTROLLER_DP_EULER__TYPEDEFS_HPP_ diff --git a/control/pid_controller_dp_euler/src/pid_controller_ros.cpp b/control/pid_controller_dp_euler/src/pid_controller_ros.cpp index 2b456647f..64ec793a1 100644 --- a/control/pid_controller_dp_euler/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp_euler/src/pid_controller_ros.cpp @@ -1,12 +1,8 @@ #include -#include -#include #include #include -#include #include #include -#include PIDControllerNode::PIDControllerNode() : Node("pid_controller_euler_node") { time_step_ = std::chrono::milliseconds(10); @@ -14,8 +10,8 @@ PIDControllerNode::PIDControllerNode() : Node("pid_controller_euler_node") { set_subscribers_and_publisher(); initialize_operation_mode(); - tau_pub_timer_ = this->create_wall_timer( - time_step_, std::bind(&PIDControllerNode::publish_tau, this)); + tau_pub_timer_ = + this->create_wall_timer(time_step_, [this]() { publish_tau(); }); set_pid_params(); } @@ -48,28 +44,31 @@ void PIDControllerNode::set_subscribers_and_publisher() { const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)}; killswitch_sub_ = this->create_subscription( software_kill_switch_topic, qos_reliable, - std::bind(&PIDControllerNode::killswitch_callback, this, - std::placeholders::_1)); + [this](const std_msgs::msg::Bool::SharedPtr msg) { + killswitch_callback(msg); + }); operation_mode_sub_ = this->create_subscription( software_operation_mode_topic, qos_reliable, - std::bind(&PIDControllerNode::operation_mode_callback, this, - std::placeholders::_1)); + [this](const vortex_msgs::msg::OperationMode::SharedPtr msg) { + operation_mode_callback(msg); + }); pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( pose_topic, qos_sensor_data, - std::bind(&PIDControllerNode::pose_callback, this, - std::placeholders::_1)); + [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + msg) { pose_callback(msg); }); twist_sub_ = this->create_subscription< geometry_msgs::msg::TwistWithCovarianceStamped>( twist_topic, qos_sensor_data, - std::bind(&PIDControllerNode::twist_callback, this, - std::placeholders::_1)); + [this](const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr + msg) { twist_callback(msg); }); guidance_sub_ = this->create_subscription( dp_reference_topic, qos_sensor_data, - std::bind(&PIDControllerNode::guidance_callback, this, - std::placeholders::_1)); + [this](const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { + guidance_callback(msg); + }); tau_pub_ = this->create_publisher( control_topic, qos_sensor_data); } @@ -127,33 +126,13 @@ void PIDControllerNode::operation_mode_callback( void PIDControllerNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - eta_.x = msg->pose.pose.position.x; - eta_.y = msg->pose.pose.position.y; - eta_.z = msg->pose.pose.position.z; - - tf2::Quaternion quat; - quat.setX(msg->pose.pose.orientation.x); - quat.setY(msg->pose.pose.orientation.y); - quat.setZ(msg->pose.pose.orientation.z); - quat.setW(msg->pose.pose.orientation.w); - - tf2::Matrix3x3 m(quat); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - - eta_.roll = roll; - eta_.pitch = pitch; - eta_.yaw = yaw; + eta_ = + vortex::utils::ros_conversions::ros_pose_to_pose_euler(msg->pose.pose); } void PIDControllerNode::twist_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - nu_.u = msg->twist.twist.linear.x; - nu_.v = msg->twist.twist.linear.y; - nu_.w = msg->twist.twist.linear.z; - nu_.p = msg->twist.twist.angular.x; - nu_.q = msg->twist.twist.angular.y; - nu_.r = msg->twist.twist.angular.z; + nu_ = vortex::utils::ros_conversions::ros_twist_to_twist(msg->twist.twist); } void PIDControllerNode::publish_tau() { @@ -200,10 +179,6 @@ void PIDControllerNode::set_pid_params() { void PIDControllerNode::guidance_callback( const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { - eta_d_.x = msg->x; - eta_d_.y = msg->y; - eta_d_.z = msg->z; - eta_d_.roll = msg->roll; - eta_d_.pitch = msg->pitch; - eta_d_.yaw = msg->yaw; + eta_d_ = + vortex::utils::ros_conversions::reference_filter_to_pose_euler(*msg); } diff --git a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp index 152a59aab..a4faa5131 100644 --- a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp @@ -42,13 +42,13 @@ ReferenceFilterNode::~ReferenceFilterNode() { void ReferenceFilterNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.twist"); - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_rpy"); this->declare_parameter("topics.reference_pose"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string twist_topic = this->get_parameter("topics.twist").as_string(); std::string guidance_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_rpy").as_string(); std::string reference_pose_topic = this->get_parameter("topics.reference_pose").as_string(); diff --git a/guidance/reference_filter_dp_quat/README.md b/guidance/reference_filter_dp_quat/README.md index 1573eb2e3..ada98da02 100644 --- a/guidance/reference_filter_dp_quat/README.md +++ b/guidance/reference_filter_dp_quat/README.md @@ -50,7 +50,7 @@ Each time step performs three operations: 1. **Compute reference error** — The 6D reference $r$ is the error between the waypoint goal and the nominal pose: - $r_{0:3} = p_{goal} - p_{nominal}$ - - $r_{3:6} = q_{\mathrm{err}}(q_{nominal}, q_{goal})$ + - $r_{3:6} = \text{quaternion\_error}(q_{nominal},\ q_{goal})$ The `quaternion_error` function returns $2 \cdot \text{vec}(q_{nominal}^{-1} \otimes q_{goal})$, which for small angles approximates the rotation vector from nominal to goal. diff --git a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml index 1aef0ec13..71aad134a 100644 --- a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml +++ b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml @@ -3,3 +3,4 @@ zeta: [1., 1., 1., 1., 1., 1.] omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] time_step_ms: 10 + publish_rpy_debug: false diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp index 87df8e7e6..8617098ca 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include "reference_filter_dp_quat/lib/waypoint_follower.hpp" @@ -68,6 +69,11 @@ class ReferenceFilterNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr reference_pub_; + rclcpp::Publisher::SharedPtr + rpy_debug_pub_; + + bool publish_rpy_debug_{false}; + rclcpp::Subscription::SharedPtr reference_sub_; diff --git a/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py index a610a8ef3..0db8046bb 100644 --- a/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py +++ b/guidance/reference_filter_dp_quat/launch/reference_filter_dp_quat.launch.py @@ -33,7 +33,7 @@ def launch_setup(context, *args, **kwargs): if rpy_publish: extra_params = { "publish_rpy_debug": True, - "topics.guidance.dp_rpy": "guidance/dp", + "topics.guidance.dp_rpy": "guidance/dp_rpy", "topics.guidance.dp": "guidance/dp_quat", } diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp index ce58bb3d3..a03164f94 100644 --- a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp @@ -57,7 +57,15 @@ void WaypointFollower::inject_and_reset() { if (angle >= 1e-10) { Eigen::Quaterniond delta_quat( Eigen::AngleAxisd(angle, delta_orientation.normalized())); - nominal_pose_.set_ori(nominal_pose_.ori_quaternion() * delta_quat); + Eigen::Quaterniond q_new = nominal_pose_.ori_quaternion() * delta_quat; + /** Enforce positive hemisphere to prevent sign flips in the published + * reference quaternion that would cause the downstream controller to + * see large spurious orientation errors. + */ + if (q_new.w() < 0.0) { + q_new.coeffs() = -q_new.coeffs(); + } + nominal_pose_.set_ori(q_new); state_.segment<3>(3).setZero(); } } diff --git a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp index a129d2aef..8b9bfd49f 100644 --- a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp @@ -8,12 +8,12 @@ #include "reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp" const auto start_message = R"( - ____ __ _____ _ _ _ - | _ \ ___ / _| ___ _ __ ___ _ __ ___ ___ | ___(_) | |_ ___ _ __ - | |_) / _ \ |_ / _ \ '__/ _ \ '_ \ / __/ _ \ | |_ | | | __/ _ \ '__| - | _ < __/ _| __/ | | __/ | | | (_| __/ | _| | | | || __/ | - |_| \_\___|_| \___|_| \___|_| |_|\___\___| |_| |_|_|\__\___|_| - +██████╗ ███████╗███████╗███████╗██████╗ ███████╗███╗ ██╗ ██████╗███████╗ ███████╗██╗██╗ ████████╗███████╗██████╗ ██████╗ ██╗ ██╗ █████╗ ████████╗ +██╔══██╗██╔════╝██╔════╝██╔════╝██╔══██╗██╔════╝████╗ ██║██╔════╝██╔════╝ ██╔════╝██║██║ ╚══██╔══╝██╔════╝██╔══██╗ ██╔═══██╗██║ ██║██╔══██╗╚══██╔══╝ +██████╔╝█████╗ █████╗ █████╗ ██████╔╝█████╗ ██╔██╗ ██║██║ █████╗ █████╗ ██║██║ ██║ █████╗ ██████╔╝ ██║ ██║██║ ██║███████║ ██║ +██╔══██╗██╔══╝ ██╔══╝ ██╔══╝ ██╔══██╗██╔══╝ ██║╚██╗██║██║ ██╔══╝ ██╔══╝ ██║██║ ██║ ██╔══╝ ██╔══██╗ ██║▄▄ ██║██║ ██║██╔══██║ ██║ +██║ ██║███████╗██║ ███████╗██║ ██║███████╗██║ ╚████║╚██████╗███████╗ ██║ ██║███████╗██║ ███████╗██║ ██║ ╚██████╔╝╚██████╔╝██║ ██║ ██║ +╚═╝ ╚═╝╚══════╝╚═╝ ╚══════╝╚═╝ ╚═╝╚══════╝╚═╝ ╚═══╝ ╚═════╝╚══════╝ ╚═╝ ╚═╝╚══════╝╚═╝ ╚══════╝╚═╝ ╚═╝ ╚══▀▀═╝ ╚═════╝ ╚═╝ ╚═╝ ╚═╝ )"; namespace vortex::guidance { @@ -57,6 +57,16 @@ void ReferenceFilterNode::set_subscribers_and_publisher() { this->create_publisher( guidance_topic, qos_sensor_data); + publish_rpy_debug_ = this->declare_parameter("publish_rpy_debug"); + if (publish_rpy_debug_) { + std::string rpy_topic = this->declare_parameter( + "topics.guidance.dp_rpy", guidance_topic + "_rpy"); + rpy_debug_pub_ = + this->create_publisher( + rpy_topic, qos_sensor_data); + spdlog::info("RPY debug publisher enabled on topic: {}", rpy_topic); + } + reference_sub_ = this->create_subscription( reference_pose_topic, qos_sensor_data, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { @@ -207,6 +217,10 @@ void ReferenceFilterNode::execute( fill_reference_msg(follower_->pose(), follower_->velocity()); reference_pub_->publish(final_reference_msg); + if (rpy_debug_pub_) { + rpy_debug_pub_->publish(fill_reference_rpy_msg( + follower_->pose(), follower_->velocity())); + } result->success = true; goal_handle->succeed(result); @@ -217,6 +231,10 @@ void ReferenceFilterNode::execute( auto reference_msg = fill_reference_msg(follower_->pose(), follower_->velocity()); reference_pub_->publish(reference_msg); + if (rpy_debug_pub_) { + rpy_debug_pub_->publish(fill_reference_rpy_msg( + follower_->pose(), follower_->velocity())); + } loop_rate.sleep(); } if (!rclcpp::ok() && goal_handle->is_active()) { diff --git a/mission/joystick_interface_auv/README.md b/mission/joystick_interface_auv/README.md index 36ae93c7b..eb7993f28 100644 --- a/mission/joystick_interface_auv/README.md +++ b/mission/joystick_interface_auv/README.md @@ -1,2 +1,44 @@ ## Joystick interface -A joystick interface for manual control of AUV. A ROS2 node that subscribes on inputs from the XBOX controller and publishes the according wrench message to be used in Thruster Allocation. + +A joystick interface for manual control and reference sending of the AUV. Subscribes to Xbox controller inputs and publishes wrench commands (manual mode) or pose references (reference/guidance mode) depending on the active operation mode. + +### Launching + +```bash +ros2 launch joystick_interface_auv joystick_interface_auv.launch.py +``` + +#### Launch arguments + +| Argument | Default | Description | +|---|---|---| +| `drone` | `nautilus` | Drone model — loads the matching config from `auv_setup/config/robots/.yaml` | +| `namespace` | `` | ROS namespace. Defaults to the drone name if left empty | +| `orientation_mode` | `euler` | Reference orientation representation: `euler` (publishes `ReferenceFilter` with RPY angles) or `quat` (publishes `ReferenceFilterQuat` with quaternion) | + +The `orientation_mode` must match the reference filter used by the active DP controller. Use `euler` with the adaptive backstepping controller (`reference_filter_dp`) and `quat` with the PID controller (`reference_filter_dp_quat`). + +Example — launch with quaternion mode for use with the PID controller: + +```bash +ros2 launch joystick_interface_auv joystick_interface_auv.launch.py orientation_mode:=quat +``` + +### Controller button mapping + +| Button | Action | +|---|---| +| **A** | Manual mode (direct wrench from joystick axes) | +| **B** | Toggle software killswitch | +| **X** | Autonomous mode | +| **Y** | Reference mode (joystick incrementally updates the pose reference sent to the DP controller) | + +In **reference mode**, the left stick controls surge/sway, triggers control heave, the right stick controls pitch/yaw, and shoulder buttons control roll. Movement is expressed in the body frame and rotated into the world frame before being added to the desired pose. + +### Config + +Gains for both manual wrench output and reference increments are set in `config/param_joystick_interface_auv.yaml`: + +- `joystick_*_gain` — scales raw axis/button input to force/torque in manual mode +- `guidance_*_gain` — scales input to position/orientation increments in reference mode +- `debounce_duration` — minimum seconds between button state changes (prevents double-triggers) diff --git a/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml index 5876ce830..e4720276e 100644 --- a/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml +++ b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml @@ -2,7 +2,7 @@ ros__parameters: joystick_surge_gain: 60.0 joystick_sway_gain: 60.0 - joystick_heave_gain: 17.5 + joystick_heave_gain: 60.0 joystick_roll_gain: 30.0 joystick_pitch_gain: 20.0 joystick_yaw_gain: 25.0 diff --git a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py index d50f83355..605679adc 100755 --- a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py +++ b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py @@ -1,11 +1,13 @@ #!/usr/bin/env python3 +import numpy as np import rclpy from geometry_msgs.msg import PoseWithCovarianceStamped, WrenchStamped from rclpy.node import Node, Parameter +from scipy.spatial.transform import Rotation from sensor_msgs.msg import JointState, Joy from std_msgs.msg import Bool -from vortex_msgs.msg import OperationMode, ReferenceFilter +from vortex_msgs.msg import OperationMode, ReferenceFilter, ReferenceFilterQuat from vortex_msgs.srv import GetOperationMode, SetOperationMode, ToggleKillswitch from vortex_utils.python_utils import PoseData from vortex_utils_ros.qos_profiles import ( @@ -41,6 +43,8 @@ def __init__(self): self._current_state = PoseData() self._desired_state = PoseData() + self._current_quat = np.array([0.0, 0.0, 0.0, 1.0]) # [x, y, z, w] + self._desired_quat = np.array([0.0, 0.0, 0.0, 1.0]) # [x, y, z, w] self._mode = OperationMode.MANUAL self._killswitch = True @@ -61,7 +65,7 @@ def __init__(self): def get_parameters(self): """Method to get the parameters from the config file.""" - self.declare_parameter('drone', 'orca') + self.declare_parameter('drone') self._drone = self.get_parameter('drone').value gain_params = [ @@ -116,8 +120,21 @@ def get_parameters(self): self.get_parameter(f'services.{param}').value, ) - self.declare_parameter('topics.guidance.dp', Parameter.Type.STRING) - self.guidance_topic = self.get_parameter('topics.guidance.dp').value + self.declare_parameter('orientation_mode', 'euler') + self.declare_parameter('topics.guidance.dp_rpy', Parameter.Type.STRING) + self.declare_parameter('topics.guidance.dp_quat', Parameter.Type.STRING) + + self._orientation_mode = self.get_parameter('orientation_mode').value + if self._orientation_mode not in ('euler', 'quat'): + self.get_logger().warn( + f"Unknown orientation_mode '{self._orientation_mode}', defaulting to 'euler'" + ) + self._orientation_mode = 'euler' + + if self._orientation_mode == 'quat': + self.guidance_topic = self.get_parameter('topics.guidance.dp_quat').value + else: + self.guidance_topic = self.get_parameter('topics.guidance.dp_rpy').value def init_movement(self): self.surge = 0.0 @@ -155,9 +172,14 @@ def set_publishers_and_subscribers(self): self._wrench_publisher = self.create_publisher( WrenchStamped, self.wrench_input_topic, qos_profile=best_effort_qos ) - self._ref_publisher = self.create_publisher( - ReferenceFilter, self.guidance_topic, qos_profile=best_effort_qos - ) + if self._orientation_mode == 'quat': + self._ref_publisher = self.create_publisher( + ReferenceFilterQuat, self.guidance_topic, qos_profile=best_effort_qos + ) + else: + self._ref_publisher = self.create_publisher( + ReferenceFilter, self.guidance_topic, qos_profile=best_effort_qos + ) self._gripper_publisher = self.create_publisher( JointState, self.gripper_servos_topic, qos_profile=best_effort_qos @@ -208,6 +230,8 @@ def handle_initial_operation_mode_response(self, future): def pose_cb(self, msg: PoseWithCovarianceStamped): """Callback function for the pose subscriber. Updates the current state of the AUV.""" self._current_state = pose_from_ros(msg.pose.pose) + q = msg.pose.pose.orientation + self._current_quat = np.array([q.x, q.y, q.z, q.w]) def operation_mode_cb(self, msg: OperationMode): self._mode = msg.operation_mode @@ -220,6 +244,7 @@ def create_reference_message(self) -> ReferenceFilter: reference_msg = ReferenceFilter() reference_msg.header.stamp = self.get_clock().now().to_msg() reference_msg.header.frame_id = "odom" + # reference_msg.header.frame_id = "base_link" reference_msg.x = self._desired_state.x reference_msg.y = self._desired_state.y reference_msg.z = self._desired_state.z @@ -228,6 +253,21 @@ def create_reference_message(self) -> ReferenceFilter: reference_msg.yaw = self._desired_state.yaw return reference_msg + def create_reference_quat_message(self) -> ReferenceFilterQuat: + """Creates a reference message with quaternion orientation from the desired state.""" + reference_msg = ReferenceFilterQuat() + reference_msg.header.stamp = self.get_clock().now().to_msg() + reference_msg.header.frame_id = "odom" + # reference_msg.header.frame_id = "base_link" + reference_msg.x = self._desired_state.x + reference_msg.y = self._desired_state.y + reference_msg.z = self._desired_state.z + reference_msg.qx = float(self._desired_quat[0]) + reference_msg.qy = float(self._desired_quat[1]) + reference_msg.qz = float(self._desired_quat[2]) + reference_msg.qw = float(self._desired_quat[3]) + return reference_msg + def create_wrench_message(self) -> WrenchStamped: """Creates a 3D wrench message with the given x, y, heave, roll, pitch, and yaw values. @@ -254,6 +294,12 @@ def transition_to_xbox_mode(self): future.add_done_callback(self.operation_mode_response_callback) self.get_logger().info("XBOX mode") + def _create_reference_msg(self): + """Returns the appropriate reference message based on orientation_mode.""" + if self._orientation_mode == 'quat': + return self.create_reference_quat_message() + return self.create_reference_message() + def transition_to_reference_mode(self): """Publishes a pose message and signals that the operational mode has switched to Reference mode.""" self._desired_state = PoseData( @@ -264,7 +310,8 @@ def transition_to_reference_mode(self): pitch=self._current_state.pitch, yaw=self._current_state.yaw, ) - reference_msg = self.create_reference_message() + self._desired_quat = self._current_quat.copy() + reference_msg = self._create_reference_msg() # Still autonomous mode, but now the reference is being controlled by the joystick request = SetOperationMode.Request() @@ -390,13 +437,42 @@ def update_reference(self): The position and orientation (roll, pitch, yaw) are updated using the current joystick inputs scaled by their respective parameters. + The linear velocities (surge, sway, heave) are transformed from the + body frame to the world frame using the current orientation. """ - self._desired_state.x += self.surge * self._guidance_surge_gain - self._desired_state.y += self.sway * self._guidance_sway_gain - self._desired_state.z -= self.heave * self._guidance_heave_gain - self._desired_state.roll += self.roll * self._guidance_roll_gain - self._desired_state.pitch += self.pitch * self._guidance_pitch_gain - self._desired_state.yaw += self.yaw * self._guidance_yaw_gain + surge_vector = self.surge * self._guidance_surge_gain + sway_vector = self.sway * self._guidance_sway_gain + heave_vector = -self.heave * self._guidance_heave_gain + + body_frame_vector = np.array([surge_vector, sway_vector, heave_vector]) + + if self._orientation_mode == 'quat': + rotation_matrix = Rotation.from_quat(self._desired_quat).as_matrix() + else: + rotation_matrix = self._desired_state.as_rotation_matrix() + + world_frame_vector = rotation_matrix @ body_frame_vector + + self._desired_state.x += world_frame_vector[0] + self._desired_state.y += world_frame_vector[1] + self._desired_state.z += world_frame_vector[2] + + if self._orientation_mode == 'quat': + delta = Rotation.from_euler( + 'xyz', + [ + self.roll * self._guidance_roll_gain, + self.pitch * self._guidance_pitch_gain, + self.yaw * self._guidance_yaw_gain, + ], + ) + self._desired_quat = ( + Rotation.from_quat(self._desired_quat) * delta + ).as_quat() + else: + self._desired_state.roll += self.roll * self._guidance_roll_gain + self._desired_state.pitch += self.pitch * self._guidance_pitch_gain + self._desired_state.yaw += self.yaw * self._guidance_yaw_gain def joystick_cb(self, msg: Joy): """Callback function that processes joy messages and converts them into wrench messages. @@ -458,7 +534,7 @@ def joystick_cb(self, msg: Joy): self._wrench_publisher.publish(wrench_msg) else: self.update_reference() - ref_msg = self.create_reference_message() + ref_msg = self._create_reference_msg() self._ref_publisher.publish(ref_msg) close = float(buttons.get("stick_button_left", 0)) diff --git a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py index 1359a43fd..e5acfdf9c 100755 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -2,7 +2,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from auv_setup.launch_arg_common import ( @@ -14,6 +15,7 @@ def launch_setup(context, *args, **kwargs): """Set up the joystick_interface_auv node with drone-specific config.""" drone, namespace = resolve_drone_and_namespace(context) + orientation_mode = LaunchConfiguration("orientation_mode").perform(context) joystick_params = os.path.join( get_package_share_directory("joystick_interface_auv"), @@ -35,7 +37,11 @@ def launch_setup(context, *args, **kwargs): name="joystick_interface_auv", namespace=namespace, output="screen", - parameters=[joystick_params, drone_params, {"drone": drone}], + parameters=[ + joystick_params, + drone_params, + {"drone": drone, "orientation_mode": orientation_mode}, + ], ) ] @@ -53,5 +59,13 @@ def generate_launch_description() -> LaunchDescription: """ return LaunchDescription( - declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + declare_drone_and_namespace_args() + + [ + DeclareLaunchArgument( + "orientation_mode", + default_value="euler", + description="Reference orientation representation: 'euler' (ReferenceFilter) or 'quat' (ReferenceFilterQuat)", + ), + OpaqueFunction(function=launch_setup), + ] ) diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp index 6ec57bc9d..6eec83751 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp @@ -32,32 +32,6 @@ inline bool is_invalid_matrix(const Eigen::MatrixBase& M) { return has_nan || has_inf; } -inline Eigen::MatrixXd calculate_thrust_configuration_matrix( - const Eigen::MatrixXd& thruster_force_direction, - const Eigen::MatrixXd& thruster_position, - const Eigen::Vector3d& center_of_mass) { - // Initialize thrust allocation matrix - Eigen::MatrixXd thrust_configuration_matrix = Eigen::MatrixXd::Zero(6, 8); - - // Calculate thrust and moment contributions from each thruster - for (int i = 0; i < thruster_position.cols(); i++) { - Eigen::Vector3d pos = - thruster_position.col(i); // Thrust vector in body frame - Eigen::Vector3d F = - thruster_force_direction.col(i); // Position vector in body frame - - // Calculate position vector relative to the center of mass - pos -= center_of_mass; - - // Fill in the thrust allocation matrix - thrust_configuration_matrix.block<3, 1>(0, i) = F; - thrust_configuration_matrix.block<3, 1>(3, i) = pos.cross(F); - } - - thrust_configuration_matrix = thrust_configuration_matrix.array(); - return thrust_configuration_matrix; -} - /** * @brief Saturates the values of a given Eigen vector between a minimum and * maximum value. @@ -110,35 +84,6 @@ inline Eigen::Vector3d double_array_to_eigen_vector3d( return Eigen::Map(vector.data()); } -/** - * @brief Computes the maximum wrench that can be constructed in all DOFs - * - * @param &T reference to the thrust configuration matrix - * @param u_min the minimum allowed value of thrust - * @param u_max the maximum allowed value of thrust - * @return The vector containing maximum value of thrust with the given thrust - * configuration and limits - */ -inline Eigen::VectorXd compute_max_wrench(const Eigen::MatrixXd& T, - const Eigen::VectorXd& u_min, - const Eigen::VectorXd& u_max) { - Eigen::VectorXd tau_max(T.rows()); - - for (int i = 0; i < T.rows(); i++) { - double w = 0.0; - for (int j = 0; j < T.cols(); j++) { - // For each DOF, greedily pick thruster contribution - if (T(i, j) > 0) - w += T(i, j) * u_max(j); - else - w += T(i, j) * u_min(j); - } - tau_max(i) = w; - } - - return tau_max; -} - /** * @brief Clamps the wrench vector in a way that preserves scale between * elements, will spdlog if intervention was needed if in debug diff --git a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp index 146a75e60..597e7d29d 100644 --- a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp +++ b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include "thrust_allocator_auv/pseudoinverse_allocator.hpp" #include "thrust_allocator_auv/thrust_allocator_utils.hpp" @@ -102,8 +103,9 @@ void ThrustAllocator::set_allocator() { .as_double_array(), num_dimensions_, num_thrusters_); - thrust_configuration_ = calculate_thrust_configuration_matrix( - thruster_force_direction_, thruster_position_, center_of_mass_); + thrust_configuration_ = + vortex::utils::math::build_thrust_configuration_matrix( + thruster_force_direction_, thruster_position_, center_of_mass_); Eigen::VectorXd input_weights = Eigen::Map( this->get_parameter( @@ -138,8 +140,8 @@ void ThrustAllocator::set_allocator() { Eigen::VectorXd max_force_vec = Eigen::VectorXd::Constant(num_thrusters_, max_thrust_); - tau_max_ = - compute_max_wrench(thrust_configuration_, min_force_vec, max_force_vec); + tau_max_ = vortex::utils::math::calculate_valid_thrust_region_polyhedron( + thrust_configuration_, min_force_vec, max_force_vec); allocator_ = Factory::make_allocator( solver_type, AllocatorConfig{ diff --git a/motion/thrust_allocator_auv/tests/thrust_allocator_tests.cpp b/motion/thrust_allocator_auv/tests/thrust_allocator_tests.cpp index 595011d78..ee7401ffa 100644 --- a/motion/thrust_allocator_auv/tests/thrust_allocator_tests.cpp +++ b/motion/thrust_allocator_auv/tests/thrust_allocator_tests.cpp @@ -101,8 +101,9 @@ static Eigen::VectorXd require_thrust(std::unique_ptr& allocator, static Eigen::VectorXd normalize_tau(const Eigen::VectorXd& tau, const AllocatorConfig& config) { - Eigen::VectorXd tau_max = compute_max_wrench( - config.extended_thrust_matrix, config.min_force, config.max_force); + Eigen::VectorXd tau_max = + vortex::utils::math::calculate_valid_thrust_region_polyhedron( + config.extended_thrust_matrix, config.min_force, config.max_force); return normalize_wrench_vector(tau, tau_max); } @@ -128,7 +129,7 @@ AllocatorConfig load_allocator_config(const std::string& yaml_path) { physical["center_of_mass"].as>()); Eigen::MatrixXd thrust_configuration_matrix = - calculate_thrust_configuration_matrix( + vortex::utils::math::build_thrust_configuration_matrix( thruster_force_directions, thruster_positions, center_of_mass); const double min_thruster_force = constraints["min_force"].as(); @@ -191,9 +192,9 @@ class PseudoinverseAllocatorTests : public ::testing::Test { void SetUp() override { allocator_config = load_allocator_config(YAML_PATH); allocator = Factory::make_allocator("pseudoinverse", allocator_config); - tau_max = compute_max_wrench(allocator_config.extended_thrust_matrix, - allocator_config.min_force, - allocator_config.max_force); + tau_max = vortex::utils::math::calculate_valid_thrust_region_polyhedron( + allocator_config.extended_thrust_matrix, allocator_config.min_force, + allocator_config.max_force); } }; @@ -206,9 +207,9 @@ class QPAllocatorTests : public ::testing::Test { void SetUp() override { allocator_config = load_allocator_config(YAML_PATH); allocator = Factory::make_allocator("qp", allocator_config); - tau_max = compute_max_wrench(allocator_config.extended_thrust_matrix, - allocator_config.min_force, - allocator_config.max_force); + tau_max = vortex::utils::math::calculate_valid_thrust_region_polyhedron( + allocator_config.extended_thrust_matrix, allocator_config.min_force, + allocator_config.max_force); } }; diff --git a/tests/ros_node_tests/reference_filter_node_test.sh b/tests/ros_node_tests/reference_filter_node_test.sh index dba21ac33..cb01309ee 100755 --- a/tests/ros_node_tests/reference_filter_node_test.sh +++ b/tests/ros_node_tests/reference_filter_node_test.sh @@ -38,7 +38,7 @@ sleep 2 # Check if controller correctly publishes guidance echo "Waiting for guidance data..." -timeout 10s ros2 topic echo /$DRONE_ARG/guidance/dp --once +timeout 10s ros2 topic echo /$DRONE_ARG/guidance/dp_rpy --once echo "Got guidance data" # Terminate processes diff --git a/tests/simulator_tests/waypoint_navigation_quat/quat_to_euler_bridge.py b/tests/simulator_tests/waypoint_navigation_quat/quat_to_euler_bridge.py index 6e101d7ae..4a2f125a5 100755 --- a/tests/simulator_tests/waypoint_navigation_quat/quat_to_euler_bridge.py +++ b/tests/simulator_tests/waypoint_navigation_quat/quat_to_euler_bridge.py @@ -26,12 +26,12 @@ def __init__(self): self.pub_ = self.create_publisher( ReferenceFilter, - '/nautilus/guidance/dp', + '/nautilus/guidance/dp_rpy', qos_profile, ) self.get_logger().info( - 'Bridge: /nautilus/guidance/dp_quat -> /nautilus/guidance/dp' + 'Bridge: /nautilus/guidance/dp_quat -> /nautilus/guidance/dp_rpy' ) def callback(self, msg: ReferenceFilterQuat): diff --git a/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh b/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh index 36c0fafab..b3e5c8daa 100755 --- a/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh +++ b/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh @@ -13,7 +13,7 @@ SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" # Function to terminate processes safely on error cleanup() { echo "Error detected. Cleaning up..." - kill -TERM -"$SIM_PID" -"$NAUTILUS_PID" -"$CONTROLLER_PID" -"$FILTER_PID" -"$BRIDGE_PID" -"$OP_MODE_PID" || true + kill -TERM -"$SIM_PID" -"$NAUTILUS_PID" -"$CONTROLLER_PID" -"$FILTER_PID" -"$BRIDGE_PID" || true exit 1 } trap cleanup ERR @@ -113,6 +113,6 @@ else fi # Terminate processes -kill -TERM -"$SIM_PID" -"$NAUTILUS_PID" -"$CONTROLLER_PID" -"$FILTER_PID" -"$BRIDGE_PID" -"$BAG_PID" -"$OP_MODE_PID" +kill -TERM -"$SIM_PID" -"$NAUTILUS_PID" -"$CONTROLLER_PID" -"$FILTER_PID" -"$BRIDGE_PID" -"$BAG_PID" echo "Test completed successfully." diff --git a/utility_scripts/build_ws_drone.sh b/utility_scripts/build_ws_drone.sh new file mode 100755 index 000000000..3201f17ba --- /dev/null +++ b/utility_scripts/build_ws_drone.sh @@ -0,0 +1,164 @@ +#!/bin/bash +set -e + +WS_DIR="$HOME/vscopium/ros2_ws" +SRC_DIR="$WS_DIR/src" +GITHUB_ORG="https://github.com/vortexntnu" + +# Repos to clone +REPOS=( + "vortex-auv" + "vortex-msgs" + "vortex-utils" +) + +# ---- Install dependencies script ---- + +# python3 -m pip install --upgrade pip +# install numpy, pynput, joy, wheel +# install ros-humble-xacro, ros-humble-joy +# run casadi install script + +# ------------ Clone missing repos ------------ +mkdir -p "$SRC_DIR" + +for repo in "${REPOS[@]}"; do + if [ -d "$SRC_DIR/$repo" ]; then + echo "[SKIP] $SRC_DIR/$repo already exists" + else + echo "[CLONE] $GITHUB_ORG/$repo.git -> $SRC_DIR/$repo" + git clone "$GITHUB_ORG/$repo.git" "$SRC_DIR/$repo" + fi +done + +# ---------- Build ---------- +cd "$WS_DIR" + +# Default packages to build +CONTROL_PKGS=( + vortex_msgs + vortex_utils + vortex_utils_ros + thrust_allocator_auv + thruster_interface_auv + dp_adapt_backs_controller + pid_controller_dp + reference_filter_dp + auv_setup + operation_mode_manager + los_guidance +) + +#TODO: add perception pkgs and a separate launch arg. +PERCEPTION_PKGS=() + +# Use arguments if provided, otherwise use defaults +if [ $# -gt 0 ]; then + PKGS=("$@") +else + PKGS=("${CONTROL_PKGS[@]}") +fi + +BUILD_FAILED=0 +echo "[BUILD] Building packages: ${PKGS[*]}" +colcon build \ + --packages-up-to "${PKGS[@]}" \ + --symlink-install \ + --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || + BUILD_FAILED=1 + +# ---------- Handle result ---------- +if [ $BUILD_FAILED -eq 1 ]; then + echo "" + echo " ╔══════════════════════════════════════════════════╗ " + echo " ║ BUILD FAILED ║ " + echo " ╚══════════════════════════════════════════════════╝ " + echo "" + echo "⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⡖⢀⠀⠒⠒⠒⠒⠒⠒⠒⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠿⠛⠙⠉⠉⠉⠉⠉⠉⠉⠙⠃⠀⠀⠠⠄⠂⠐⠀⠀⠀⠀⠀⠀⠀⠉⠁⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡿⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⠀⠀⡀⣀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⢀⣶⢿⣵⣮⣞⣶⣆⡶⣴⣀⠀⠀⠀⠀⠀⠀⠡⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠉⠉⠁⠂⠐⠂⡐⠈⠻⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠓⢀⡖⠀⢠⣴⣿⣿⣿⣿⣿⣿⣿⣿⣿⣶⣭⢚⠤⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠅" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠉⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⣾⠃⣴⣿⣿⢻⣿⣿⣿⣿⣿⣿⣿⣿⣿⡎⣧⢣⡅⢣⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠐⠨⣿⣿⣿⣿⣿⣿⣿⠿⠋⠀⠉⢠⢿⣟⣯⣿⣾⣿⣿⣿⣿⣿⣿⣿⣿⡿⣜⠮⡜⣌⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠚⠿⠿⠿⠿⠿⠏⠀⠂⣴⣲⢠⣛⣾⣿⣿⣿⣿⣿⢿⣿⣿⣿⣿⣿⣿⣯⡟⡴⢂⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠔" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠈⠷⡙⢰⣉⡴⣀⡉⠉⠋⠙⢉⣿⡟⠛⠛⠋⠛⠙⡉⠘⠁⠀⢀⣞⠀⠀⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠡⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⡀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠃⠀⠉⠘⠁⠀⢀⣠⣾⣧⡀⠀⠀⠐⠃⠂⠁⠀⠀⠠⠈⠄⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠂" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⠠⠀⠀⠀⠀⠀⠀⠀⢀⣋⢶⣠⣤⣤⡶⢮⣿⣿⣿⣷⠄⢦⣄⣀⡀⢄⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠁⠒⣀⠀⠀⠀⠀⠀⠀⠜⣿⡿⣿⡟⣡⡿⢿⣿⣿⢿⡒⢌⠻⣯⠗⡎⠄⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠀⠒⠤⣄⣀⢀⡀⠀⠀⢙⢣⡽⡅⠀⠀⠀⠀⠀⠀⢈⡖⠁⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⡶⡲⡒⡒⣴⡶⣒⣖⢶⣶" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠈⠄⡁⠢⠐⠠⢌⠁⠁⠀⠱⣈⠎⢙⡳⣍⠾⠶⡲⠦⣐⠎⠲⠀⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⠓⠔⠥⠇⠛⠆⠋⠭⠚⠛" + echo "⠀⠀⠐⠀⠀⠀⠀⢀⠀⠀⠂⢌⡐⢠⠒⣤⣁⠎⠅⣈⡀⠀⠀⠀⠑⢪⠀⡈⣈⣁⡀⠁⣠⡁⠀⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⢀⣄⣐⣠⡈⠤⡁⠆⡑⣀⠉⠆⠉⢂⡙⢀⣂⣤⠚⣵⢯⡄⠀⠀⠀⠀⠡⠀⠀⠉⠘⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠈⣨⣧⣶⣟⠷⢛⣡⢤⡲⡜⢮⠌⣍⢿⣦⠐⣈⢷⡌⠞⡜⢆⠀⠀⠀⠀⠡⣀⢄⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣾⣿⠟⣴⣞⣯⠻⣜⣣⢟⣹⢂⢯⡘⢎⡞⣦⠀⢂⠙⢦⠉⢄⠊⢀⠀⠀⠀⠀⠊⠐⠁⠈⠀⠀⠀⠀⠀⠀⠀⠀⡀⢀⠠⠀⠀⡣⢤⡉⢄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⡧⣏⢳⡬⢷⡙⢆⡳⢎⡳⢜⡢⢝⡲⢸⢥⠣⢌⠢⣤⣉⠢⠄⡄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⢆⡱⠌⢀⠐⢢⢍⠢⢈⠄⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⡟⣠⠻⣄⠻⣜⢻⡘⢣⡛⠼⣀⠻⡘⡤⢛⡄⢿⡀⢧⣛⢧⢇⡄⠀⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠀⢠⡘⠄⡤⢀⠄⡀⠘⡄⠇⡘⠠⣀⠀⠀⠀⠀⠀⠀⠀" + echo "⠐⣡⠛⣌⠳⣌⠳⣌⠣⡜⡱⢌⢣⠱⢌⠣⡜⢢⡑⢎⡝⣊⠞⡼⣉⠖⡠⢐⠀⠀⠀⠀⠀⠀⠀⠀⠀⢐⠀⡘⠤⣌⠳⠀⡌⠎⡄⠀⠘⡀⣁⠒⢠⠂⠀⠀⠀⠀⠀⠀" + echo "⠠⢡⠉⡄⠣⢌⠳⡄⡓⢬⡐⠎⣄⠣⢊⠱⡘⡄⢙⢢⠹⣤⢋⡴⠡⢎⠱⡈⠆⠀⢀⠀⠀⠀⠀⢀⠀⡜⠠⣘⠲⢌⡱⠠⡑⢌⠰⡡⠀⠐⢠⠘⠠⢀⠃⠀⠀⠀⠀⠀" + echo "⢡⠂⠥⠐⡀⢈⠳⣐⡉⠆⡜⠒⡄⢣⠉⢆⠱⡐⠈⢂⠓⡌⢣⠘⡡⢊⠐⡘⠄⠀⠂⡀⠂⠌⣠⡔⢂⠩⠐⢤⢋⠆⡀⠂⠔⡈⠆⠥⠁⠀⠠⠈⡐⠨⢘⠀⠀⠀⠀⠀" + echo "⢀⠣⢈⠱⡐⠀⡇⠢⢅⡍⠰⡉⡔⢡⠊⡄⢣⠘⡀⠀⢂⠐⠠⢁⠐⠀⢂⡉⠆⠀⠡⢐⣹⣾⣿⣿⡆⡣⢘⢢⢉⠂⠠⢁⡘⠤⣉⠒⠀⢂⠀⠀⠤⠑⢂⠂⠀⠀⢀⠀" + echo "⡀⢃⠆⠐⡈⠡⢌⡑⠢⢌⠱⡐⢌⠢⡑⢌⠂⡅⢂⠁⡀⠂⢁⠠⠈⢀⠀⡜⠀⢈⠐⢿⣿⣿⣿⣿⡇⡅⡌⠆⠌⠀⠐⠠⡐⢢⠐⡀⠈⢀⠀⠀⠠⢁⠂⠌⠀⢀⢰⣤" + echo "⡘⠆⠌⠀⠠⢁⠢⠌⡁⠎⢠⠑⡈⠆⡑⢂⠥⢘⠠⢂⠀⠄⠂⠀⡀⠂⠠⢌⠀⣢⡍⢎⡻⣿⣿⣿⡇⣼⣿⣎⠀⠀⢂⠡⠐⢂⠡⠀⢀⠂⠌⠀⠀⠠⠀⠌⠀⠀⠂⠹" + echo "" + echo "" + exit 1 +fi + +# ---------- Merge compile_commands.json ---------- +# Each package gets its own compile_commands.json under build//. +# Merge them into a single file at the workspace root for clangd / IDEs. +echo "[MERGE] Combining compile_commands.json files..." +python3 -c " +import json, glob, pathlib + +ws = pathlib.Path('$WS_DIR') +combined = [] +for f in sorted(ws.glob('build/*/compile_commands.json')): + combined.extend(json.loads(f.read_text())) + +out = ws / 'compile_commands.json' +out.write_text(json.dumps(combined, indent=2)) +print(f' -> Wrote {len(combined)} entries to {out}') +" + +# ---------- Source ---------- +source "$WS_DIR/install/setup.bash" + +echo "" +echo " ╔══════════════════════════════════════════════════╗" +echo " ║ BUILD SUCCESSFUL ║" +echo " ╚══════════════════════════════════════════════════╝" +echo "" +echo "⠀⠀⢀⠀⣠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢀⠀⣿⡂⢹⡇⠀⠀⣰⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢸⡇⢸⣇⢸⣇⠀⢀⣿⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢾⠀⠀⣯⡀⡆⠀⠀" +echo "⢸⣷⢸⣇⣸⣇⠀⣾⠏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣀⣀⣠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢳⣂⠀⣿⡄⢸⡀⣤" +echo "⢠⣿⣿⣿⣿⣿⣿⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣾⣿⣿⣊⡝⠛⠙⠂⠄⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣦⣼⣷⣼⣁⠼" +echo "⢸⣿⣿⣿⣿⣿⣿⣀⢀⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⣿⣿⣿⡻⣥⢋⡔⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠻⣿⣂⣜⣿⡟⢿⣿⣿⣄" +echo "⠈⣿⣿⣿⣿⣿⣿⣿⠿⠋⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⣿⣿⣿⣷⢯⣿⣾⡔⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠙⢪⣷⣿⢿⣿⣿" +echo "⠀⣿⣿⣟⢿⠿⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⣿⡟⠛⠉⡉⢸⡉⠁⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢢⣽⣗⣿⠇" +echo "⠀⣿⣿⣿⡏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠺⣿⡇⣤⡤⢔⡿⣇⠀⢦⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⣿⣯⠀" +echo "⠘⡟⣛⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⡇⣿⣿⠗⡲⠏⠟⠿⠀⠈⠓⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⠍⠁⠁⠀" +echo "⠃⡜⡠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⣼⣿⡟⢡⡿⠿⠷⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⣟⠒⠂⠂" +echo "⠐⢐⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⠸⣡⢶⣿⣟⡃⠀⠘⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡇⠀⡀⠀" +echo "⢠⡏⠀⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⡰⢨⠣⠉⠉⠋⠉⠀⠀⠀⠀⢈⠀⡂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⡿⠀⠀⠀⠀" +echo "⢺⡇⢸⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣽⡿⢛⢭⠏⣢⠍⠈⠖⠀⠀⠒⣶⢦⡁⠂⠀⠀⠀⠀⠀⠯⠤⣤⣴⢶⣍⠝⣯⣦⡀⠀⠀⠀⠀⠀⠀⠀⠀⢌⣿⠱⠀⠀⠀⠀⠀" +echo "⣯⣯⠸⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠄⠀⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠂⠀⠀⠏⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠧⠍⠶⠤⠈⣆⠀⠀⠀⠀⠀⠀⠀⣷⡻⠀⣼⠀⠀⠀" +echo "⣯⣨⡀⢀⡠⠤⣐⠤⣀⣰⠔⠊⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠑⠐⠐⠢⠺⠥⡾⠉⡠⠀⠀⠀" +echo "⠋⠙⠈⠉⠉⠁⠈⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠓⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠇⣣⡁⢶⣠⢀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⢶⠀⡶⣲⠀⣆⡒⣰⠒⢦⢰⠀⢰⡆⣴⠐⣶⠒⣐⣒⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⣴⣺⣿⣿⣿⠛" +echo "⠀⠀⠑⢌⠻⣗⣔⠉⡅⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠞⠚⠃⠻⠴⠃⠦⠝⠘⠤⠎⠸⠤⠘⠧⠞⠀⠛⠀⠰⠤⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡟⣾⣿⣿⣿⠃⠀" +echo "⠀⠀⠀⠀⠉⠢⠁⠀⠀⠀⠀⢀⣤⣤⣤⣄⠀⠀⢠⣤⠀⠀⣤⣄⠀⠀⠀⣤⣤⠀⢠⣤⣤⣤⣤⣤⡄⢠⣤⣄⠀⠀⠀⠀⣤⣤⡄⠀⠀⠀⢠⣤⡄⠀⠀⠀⢘⡮⡝⣿⣿⡿⢆⠁⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⠏⠉⠉⢿⣷⠀⢸⣿⠀⠠⣿⣿⣧⡀⠀⣿⣿⠀⢸⣿⡏⠉⠉⠉⠁⢼⣿⣿⡄⠀⠀⢸⡿⣿⡇⠀⠀⢀⣿⢻⣷⠀⠀⠀⠞⡜⣹⣿⣿⡙⢆⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⠀⠀⠀⠀⠀⠀⢸⣿⠀⠐⣿⡯⢻⣷⡀⣿⣿⠀⢸⣿⣷⣶⣶⡆⠀⢺⣿⠹⣿⡀⢠⣿⠃⣿⡇⠀⠀⣾⡟⠀⢿⣧⠀⠀⠀⠠⢽⣿⣯⡙⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢿⣿⡀⠀⠀⣠⣤⠀⢸⣿⠀⢈⣿⡧⠀⠹⣿⣿⣿⠀⢸⣿⡇⠀⠀⠀⠀⢸⣿⡄⢻⣧⣾⡏⢠⣿⡇⠀⣼⣿⣷⣶⣾⣿⣇⠀⠀⠀⠘⣿⢣⠜⠁⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣿⣶⣾⣿⠏⠀⢸⣿⠀⠀⣿⡷⠀⠀⠹⣿⣿⠀⢸⣿⣿⣿⣿⣿⡆⢸⣿⡆⠀⢿⡿⠀⢰⣿⡇⢀⣿⡏⠀⠀⠀⢹⣿⡀⠀⠀⠀⠀⠈⡆⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠉⠉⠀⠀⠀⠈⠉⠀⠀⠉⠁⠀⠀⠀⠉⠉⠀⠈⠉⠉⠈⠉⠉⠁⠈⠉⠀⠀⠈⠁⠀⠀⠉⠁⠈⠉⠀⠀⠀⠀⠈⠉⠁⠐⡀⠀⠀⠀⠀⠀⠀⠀" +echo "" +echo "" +echo " Remember to source your new changes bossman! " +echo "" diff --git a/utility_scripts/build_ws_topside.sh b/utility_scripts/build_ws_topside.sh new file mode 100755 index 000000000..a4b8fe0c1 --- /dev/null +++ b/utility_scripts/build_ws_topside.sh @@ -0,0 +1,179 @@ +#!/bin/bash +set -e + +WS_DIR="$HOME/vscopium/ros2_ws" +SRC_DIR="$WS_DIR/src" +GITHUB_ORG="https://github.com/vortexntnu" + +# Repos to clone +REPOS=( + "stonefish_ros2" + "vortex-auv" + "vortex-msgs" + "vortex-stonefish-interface" + "vortex-stonefish-sim" + "vortex-utils" + "vortex-ci" +) + +# ---- Install dependencies ---- + +echo "[DEPS] Upgrading pip..." +python3 -m pip install --upgrade pip + +echo "[DEPS] Installing Python packages..." +python3 -m pip install numpy pynput wheel + +echo "[DEPS] Installing ROS apt packages..." +ROS_DISTRO="${ROS_DISTRO:-humble}" +sudo apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-xacro \ + ros-${ROS_DISTRO}-joy + +# ------------ Clone missing repos ------------ +mkdir -p "$SRC_DIR" + +for repo in "${REPOS[@]}"; do + if [ -d "$SRC_DIR/$repo" ]; then + echo "[SKIP] $SRC_DIR/$repo already exists" + else + echo "[CLONE] $GITHUB_ORG/$repo.git -> $SRC_DIR/$repo" + git clone "$GITHUB_ORG/$repo.git" "$SRC_DIR/$repo" + fi +done + +# ---------- Build ---------- +cd "$WS_DIR" + +# Default packages to build +DEFAULT_PKGS=( + vortex_msgs + vortex_utility_nodes + vortex_utils + vortex_utils_ros + stonefish_ros2 + vortex_sim_interface + stonefish_sim + stonefish_sim_interface + thrust_allocator_auv + thruster_interface_auv + dp_adapt_backs_controller + pid_controller_dp + reference_filter_dp + keyboard_joy + joystick_interface_auv + auv_setup + operation_mode_manager + los_guidance +) + +# Use arguments if provided, otherwise use defaults +if [ $# -gt 0 ]; then + PKGS=("$@") +else + PKGS=("${DEFAULT_PKGS[@]}") +fi + +BUILD_FAILED=0 +echo "[BUILD] Building packages: ${PKGS[*]}" +colcon build \ + --packages-up-to "${PKGS[@]}" \ + --symlink-install \ + --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || + BUILD_FAILED=1 + +# ---------- Handle result ---------- +if [ $BUILD_FAILED -eq 1 ]; then + echo "" + echo " ╔══════════════════════════════════════════════════╗ " + echo " ║ BUILD FAILED ║ " + echo " ╚══════════════════════════════════════════════════╝ " + echo "" + echo "⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⡖⢀⠀⠒⠒⠒⠒⠒⠒⠒⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠿⠛⠙⠉⠉⠉⠉⠉⠉⠉⠙⠃⠀⠀⠠⠄⠂⠐⠀⠀⠀⠀⠀⠀⠀⠉⠁⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡿⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⠀⠀⡀⣀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⢀⣶⢿⣵⣮⣞⣶⣆⡶⣴⣀⠀⠀⠀⠀⠀⠀⠡⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠉⠉⠁⠂⠐⠂⡐⠈⠻⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠓⢀⡖⠀⢠⣴⣿⣿⣿⣿⣿⣿⣿⣿⣿⣶⣭⢚⠤⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠅" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠉⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⣾⠃⣴⣿⣿⢻⣿⣿⣿⣿⣿⣿⣿⣿⣿⡎⣧⢣⡅⢣⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠐⠨⣿⣿⣿⣿⣿⣿⣿⠿⠋⠀⠉⢠⢿⣟⣯⣿⣾⣿⣿⣿⣿⣿⣿⣿⣿⡿⣜⠮⡜⣌⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠚⠿⠿⠿⠿⠿⠏⠀⠂⣴⣲⢠⣛⣾⣿⣿⣿⣿⣿⢿⣿⣿⣿⣿⣿⣿⣯⡟⡴⢂⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠔" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠈⠷⡙⢰⣉⡴⣀⡉⠉⠋⠙⢉⣿⡟⠛⠛⠋⠛⠙⡉⠘⠁⠀⢀⣞⠀⠀⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠡⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⡀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠃⠀⠉⠘⠁⠀⢀⣠⣾⣧⡀⠀⠀⠐⠃⠂⠁⠀⠀⠠⠈⠄⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠂" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⠠⠀⠀⠀⠀⠀⠀⠀⢀⣋⢶⣠⣤⣤⡶⢮⣿⣿⣿⣷⠄⢦⣄⣀⡀⢄⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠁⠒⣀⠀⠀⠀⠀⠀⠀⠜⣿⡿⣿⡟⣡⡿⢿⣿⣿⢿⡒⢌⠻⣯⠗⡎⠄⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠀⠒⠤⣄⣀⢀⡀⠀⠀⢙⢣⡽⡅⠀⠀⠀⠀⠀⠀⢈⡖⠁⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⡶⡲⡒⡒⣴⡶⣒⣖⢶⣶" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠈⠄⡁⠢⠐⠠⢌⠁⠁⠀⠱⣈⠎⢙⡳⣍⠾⠶⡲⠦⣐⠎⠲⠀⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⠓⠔⠥⠇⠛⠆⠋⠭⠚⠛" + echo "⠀⠀⠐⠀⠀⠀⠀⢀⠀⠀⠂⢌⡐⢠⠒⣤⣁⠎⠅⣈⡀⠀⠀⠀⠑⢪⠀⡈⣈⣁⡀⠁⣠⡁⠀⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⢀⣄⣐⣠⡈⠤⡁⠆⡑⣀⠉⠆⠉⢂⡙⢀⣂⣤⠚⣵⢯⡄⠀⠀⠀⠀⠡⠀⠀⠉⠘⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠈⣨⣧⣶⣟⠷⢛⣡⢤⡲⡜⢮⠌⣍⢿⣦⠐⣈⢷⡌⠞⡜⢆⠀⠀⠀⠀⠡⣀⢄⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣾⣿⠟⣴⣞⣯⠻⣜⣣⢟⣹⢂⢯⡘⢎⡞⣦⠀⢂⠙⢦⠉⢄⠊⢀⠀⠀⠀⠀⠊⠐⠁⠈⠀⠀⠀⠀⠀⠀⠀⠀⡀⢀⠠⠀⠀⡣⢤⡉⢄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⡧⣏⢳⡬⢷⡙⢆⡳⢎⡳⢜⡢⢝⡲⢸⢥⠣⢌⠢⣤⣉⠢⠄⡄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⢆⡱⠌⢀⠐⢢⢍⠢⢈⠄⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⡟⣠⠻⣄⠻⣜⢻⡘⢣⡛⠼⣀⠻⡘⡤⢛⡄⢿⡀⢧⣛⢧⢇⡄⠀⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠀⢠⡘⠄⡤⢀⠄⡀⠘⡄⠇⡘⠠⣀⠀⠀⠀⠀⠀⠀⠀" + echo "⠐⣡⠛⣌⠳⣌⠳⣌⠣⡜⡱⢌⢣⠱⢌⠣⡜⢢⡑⢎⡝⣊⠞⡼⣉⠖⡠⢐⠀⠀⠀⠀⠀⠀⠀⠀⠀⢐⠀⡘⠤⣌⠳⠀⡌⠎⡄⠀⠘⡀⣁⠒⢠⠂⠀⠀⠀⠀⠀⠀" + echo "⠠⢡⠉⡄⠣⢌⠳⡄⡓⢬⡐⠎⣄⠣⢊⠱⡘⡄⢙⢢⠹⣤⢋⡴⠡⢎⠱⡈⠆⠀⢀⠀⠀⠀⠀⢀⠀⡜⠠⣘⠲⢌⡱⠠⡑⢌⠰⡡⠀⠐⢠⠘⠠⢀⠃⠀⠀⠀⠀⠀" + echo "⢡⠂⠥⠐⡀⢈⠳⣐⡉⠆⡜⠒⡄⢣⠉⢆⠱⡐⠈⢂⠓⡌⢣⠘⡡⢊⠐⡘⠄⠀⠂⡀⠂⠌⣠⡔⢂⠩⠐⢤⢋⠆⡀⠂⠔⡈⠆⠥⠁⠀⠠⠈⡐⠨⢘⠀⠀⠀⠀⠀" + echo "⢀⠣⢈⠱⡐⠀⡇⠢⢅⡍⠰⡉⡔⢡⠊⡄⢣⠘⡀⠀⢂⠐⠠⢁⠐⠀⢂⡉⠆⠀⠡⢐⣹⣾⣿⣿⡆⡣⢘⢢⢉⠂⠠⢁⡘⠤⣉⠒⠀⢂⠀⠀⠤⠑⢂⠂⠀⠀⢀⠀" + echo "⡀⢃⠆⠐⡈⠡⢌⡑⠢⢌⠱⡐⢌⠢⡑⢌⠂⡅⢂⠁⡀⠂⢁⠠⠈⢀⠀⡜⠀⢈⠐⢿⣿⣿⣿⣿⡇⡅⡌⠆⠌⠀⠐⠠⡐⢢⠐⡀⠈⢀⠀⠀⠠⢁⠂⠌⠀⢀⢰⣤" + echo "⡘⠆⠌⠀⠠⢁⠢⠌⡁⠎⢠⠑⡈⠆⡑⢂⠥⢘⠠⢂⠀⠄⠂⠀⡀⠂⠠⢌⠀⣢⡍⢎⡻⣿⣿⣿⡇⣼⣿⣎⠀⠀⢂⠡⠐⢂⠡⠀⢀⠂⠌⠀⠀⠠⠀⠌⠀⠀⠂⠹" + echo "" + echo "" + exit 1 +fi + +# ---------- Merge compile_commands.json ---------- +# Each package gets its own compile_commands.json under build//. +# Merge them into a single file at the workspace root for clangd / IDEs. +echo "[MERGE] Combining compile_commands.json files..." +python3 -c " +import json, glob, pathlib + +ws = pathlib.Path('$WS_DIR') +combined = [] +for f in sorted(ws.glob('build/*/compile_commands.json')): + combined.extend(json.loads(f.read_text())) + +out = ws / 'compile_commands.json' +out.write_text(json.dumps(combined, indent=2)) +print(f' -> Wrote {len(combined)} entries to {out}') +" + +# ---------- Source ---------- +source "$WS_DIR/install/setup.bash" + +echo "" +echo " ╔══════════════════════════════════════════════════╗" +echo " ║ BUILD SUCCESSFUL ║" +echo " ╚══════════════════════════════════════════════════╝" +echo "" +echo "⠀⠀⢀⠀⣠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢀⠀⣿⡂⢹⡇⠀⠀⣰⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢸⡇⢸⣇⢸⣇⠀⢀⣿⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢾⠀⠀⣯⡀⡆⠀⠀" +echo "⢸⣷⢸⣇⣸⣇⠀⣾⠏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣀⣀⣠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢳⣂⠀⣿⡄⢸⡀⣤" +echo "⢠⣿⣿⣿⣿⣿⣿⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣾⣿⣿⣊⡝⠛⠙⠂⠄⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣦⣼⣷⣼⣁⠼" +echo "⢸⣿⣿⣿⣿⣿⣿⣀⢀⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⣿⣿⣿⡻⣥⢋⡔⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠻⣿⣂⣜⣿⡟⢿⣿⣿⣄" +echo "⠈⣿⣿⣿⣿⣿⣿⣿⠿⠋⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⣿⣿⣿⣷⢯⣿⣾⡔⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠙⢪⣷⣿⢿⣿⣿" +echo "⠀⣿⣿⣟⢿⠿⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⣿⡟⠛⠉⡉⢸⡉⠁⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢢⣽⣗⣿⠇" +echo "⠀⣿⣿⣿⡏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠺⣿⡇⣤⡤⢔⡿⣇⠀⢦⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⣿⣯⠀" +echo "⠘⡟⣛⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⡇⣿⣿⠗⡲⠏⠟⠿⠀⠈⠓⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⠍⠁⠁⠀" +echo "⠃⡜⡠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⣼⣿⡟⢡⡿⠿⠷⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⣟⠒⠂⠂" +echo "⠐⢐⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⠸⣡⢶⣿⣟⡃⠀⠘⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡇⠀⡀⠀" +echo "⢠⡏⠀⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⡰⢨⠣⠉⠉⠋⠉⠀⠀⠀⠀⢈⠀⡂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⡿⠀⠀⠀⠀" +echo "⢺⡇⢸⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣽⡿⢛⢭⠏⣢⠍⠈⠖⠀⠀⠒⣶⢦⡁⠂⠀⠀⠀⠀⠀⠯⠤⣤⣴⢶⣍⠝⣯⣦⡀⠀⠀⠀⠀⠀⠀⠀⠀⢌⣿⠱⠀⠀⠀⠀⠀" +echo "⣯⣯⠸⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠄⠀⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠂⠀⠀⠏⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠧⠍⠶⠤⠈⣆⠀⠀⠀⠀⠀⠀⠀⣷⡻⠀⣼⠀⠀⠀" +echo "⣯⣨⡀⢀⡠⠤⣐⠤⣀⣰⠔⠊⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠑⠐⠐⠢⠺⠥⡾⠉⡠⠀⠀⠀" +echo "⠋⠙⠈⠉⠉⠁⠈⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠓⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠇⣣⡁⢶⣠⢀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⢶⠀⡶⣲⠀⣆⡒⣰⠒⢦⢰⠀⢰⡆⣴⠐⣶⠒⣐⣒⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⣴⣺⣿⣿⣿⠛" +echo "⠀⠀⠑⢌⠻⣗⣔⠉⡅⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠞⠚⠃⠻⠴⠃⠦⠝⠘⠤⠎⠸⠤⠘⠧⠞⠀⠛⠀⠰⠤⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡟⣾⣿⣿⣿⠃⠀" +echo "⠀⠀⠀⠀⠉⠢⠁⠀⠀⠀⠀⢀⣤⣤⣤⣄⠀⠀⢠⣤⠀⠀⣤⣄⠀⠀⠀⣤⣤⠀⢠⣤⣤⣤⣤⣤⡄⢠⣤⣄⠀⠀⠀⠀⣤⣤⡄⠀⠀⠀⢠⣤⡄⠀⠀⠀⢘⡮⡝⣿⣿⡿⢆⠁⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⠏⠉⠉⢿⣷⠀⢸⣿⠀⠠⣿⣿⣧⡀⠀⣿⣿⠀⢸⣿⡏⠉⠉⠉⠁⢼⣿⣿⡄⠀⠀⢸⡿⣿⡇⠀⠀⢀⣿⢻⣷⠀⠀⠀⠞⡜⣹⣿⣿⡙⢆⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⠀⠀⠀⠀⠀⠀⢸⣿⠀⠐⣿⡯⢻⣷⡀⣿⣿⠀⢸⣿⣷⣶⣶⡆⠀⢺⣿⠹⣿⡀⢠⣿⠃⣿⡇⠀⠀⣾⡟⠀⢿⣧⠀⠀⠀⠠⢽⣿⣯⡙⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢿⣿⡀⠀⠀⣠⣤⠀⢸⣿⠀⢈⣿⡧⠀⠹⣿⣿⣿⠀⢸⣿⡇⠀⠀⠀⠀⢸⣿⡄⢻⣧⣾⡏⢠⣿⡇⠀⣼⣿⣷⣶⣾⣿⣇⠀⠀⠀⠘⣿⢣⠜⠁⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣿⣶⣾⣿⠏⠀⢸⣿⠀⠀⣿⡷⠀⠀⠹⣿⣿⠀⢸⣿⣿⣿⣿⣿⡆⢸⣿⡆⠀⢿⡿⠀⢰⣿⡇⢀⣿⡏⠀⠀⠀⢹⣿⡀⠀⠀⠀⠀⠈⡆⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠉⠉⠀⠀⠀⠈⠉⠀⠀⠉⠁⠀⠀⠀⠉⠉⠀⠈⠉⠉⠈⠉⠉⠁⠈⠉⠀⠀⠈⠁⠀⠀⠉⠁⠈⠉⠀⠀⠀⠀⠈⠉⠁⠐⡀⠀⠀⠀⠀⠀⠀⠀" +echo "" +echo "" +echo " Remember to source your new changes bossman! " +echo "" diff --git a/utility_scripts/launch_drone_topside.sh b/utility_scripts/launch_drone_topside.sh new file mode 100755 index 000000000..80681499d --- /dev/null +++ b/utility_scripts/launch_drone_topside.sh @@ -0,0 +1,57 @@ +#!/bin/bash +# Launch drone simulation stack in a tmux session + +SESSION="drone_launch" +S="source ~/vscopium/ros2_ws/install/setup.bash" + +# Kill existing session if it exists +tmux kill-session -t "$SESSION" 2>/dev/null + +# Launch Foxglove Studio only if not already running +if ! pgrep -f foxglove-studio &>/dev/null; then + foxglove-studio &>/dev/null & +fi + +# ============================================= +# Window 1: sim (4 equal panes) +# ============================================= +tmux new-session -d -s "$SESSION" -n "sim" + +# Grab the initial pane ID +PANE_SIM=$(tmux list-panes -t "$SESSION:sim" -F '#{pane_id}') + +# Stonefish simulation (top-left) +tmux send-keys -t "$PANE_SIM" "clear && $S && ros2 launch stonefish_sim simulation.launch.py drone:=nautilus" Enter + +# Split right -> Keyboard joy (top-right) +PANE_JOY=$(tmux split-window -h -t "$PANE_SIM" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_JOY" "clear && $S && ros2 launch keyboard_joy keyboard_joy_node.launch.py" Enter + +# Split sim pane down -> AUV setup (bottom-left) +PANE_DP=$(tmux split-window -v -t "$PANE_SIM" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_DP" "clear && $S && ros2 launch auv_setup dp.launch.py" Enter + +# Split joy pane down -> Drone sim (bottom-right) +PANE_DRONE=$(tmux split-window -v -t "$PANE_JOY" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_DRONE" "clear && $S && ros2 launch stonefish_sim drone_sim.launch.py" Enter + +# Force equal pane sizes +tmux select-layout -t "$SESSION:sim" tiled + +# ============================================= +# Window 2: tools (2 panes) +# ============================================= +tmux new-window -t "$SESSION" -n "tools" + +PANE_FOX=$(tmux list-panes -t "$SESSION:tools" -F '#{pane_id}') +tmux send-keys -t "$PANE_FOX" "clear && $S && ros2 launch foxglove_bridge foxglove_bridge_launch.xml" Enter + +# Split down -> Message publisher +PANE_MSG=$(tmux split-window -v -t "$PANE_FOX" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_MSG" "clear && $S && ros2 launch vortex_utility_nodes message_publisher.launch.py" Enter + +# ============================================= +# Focus first window and attach +# ============================================= +tmux select-window -t "$SESSION:sim" +tmux attach-session -t "$SESSION" diff --git a/utility_scripts/launch_drone_vehicle.sh b/utility_scripts/launch_drone_vehicle.sh new file mode 100755 index 000000000..1ddf250e4 --- /dev/null +++ b/utility_scripts/launch_drone_vehicle.sh @@ -0,0 +1,128 @@ +#!/bin/bash +# Launch drone vehicle stack in a tmux session +# Usage: ./launch_drone_vehicle.sh [--ori_type quat|euler] [--controller_type pid|adapt|adapt_quat] + +SESSION="drone_vehicle" + +# ============================================= +# Parse arguments +# ============================================= +ORI_TYPE="quat" +CONTROLLER_TYPE="adapt_quat" + +while [[ $# -gt 0 ]]; do + case "$1" in + --ori_type | -o) + ORI_TYPE="$2" + shift 2 + ;; + --controller_type | -c) + CONTROLLER_TYPE="$2" + shift 2 + ;; + *) + echo "Unknown argument: $1" + echo "Usage: $0 [--ori_type quat|euler] [--controller_type pid|adapt|adapt_quat]" + exit 1 + ;; + esac +done + +# Validate +if [[ "$ORI_TYPE" != "quat" && "$ORI_TYPE" != "euler" ]]; then + echo "Error: ori_type must be 'quat' or 'euler' (got: $ORI_TYPE)" + exit 1 +fi + +if [[ "$CONTROLLER_TYPE" != "pid" && "$CONTROLLER_TYPE" != "adapt" && "$CONTROLLER_TYPE" != "adapt_quat" ]]; then + echo "Error: controller_type must be 'pid', 'adapt' or 'adapt_quat' (got: $CONTROLLER_TYPE)" + exit 1 +fi + +# Cross-validate: pid uses quat reference filter, adaptive uses euler +if [[ "$CONTROLLER_TYPE" == "pid" && "$ORI_TYPE" != "quat" ]]; then + echo "Warning: pid controller uses quaternion representation — consider --ori_type quat" +fi +if [[ "$CONTROLLER_TYPE" == "adapt" && "$ORI_TYPE" != "euler" ]]; then + echo "Warning: adaptive controller uses euler representation — consider --ori_type euler" +fi +if [[ "$CONTROLLER_TYPE" == "adapt_quat" && "$ORI_TYPE" != "quat" ]]; then + echo "Warning: adapt_quat controller uses quaternion representation — consider --ori_type quat" +fi + +# Select the DP launch file +if [[ "$CONTROLLER_TYPE" == "adapt_quat" ]]; then + DP_LAUNCH="auv_setp dp_quat.launch.py" +elif [[ "$CONTROLLER_TYPE" == "pid" ]]; then + DP_LAUNCH="auv_setp dp.launch.py controller_type:=pid orientation_mode:=$ORI_TYPE" +else + DP_LAUNCH="auv_setp dp.launch.py controller_type:=adaptive orientation_mode:=$ORI_TYPE" +fi + +echo "[LAUNCH] ori_type=$ORI_TYPE controller_type=$CONTROLLER_TYPE dp_launch=$DP_LAUNCH" + +# Kill existing session if it exists +tmux kill-session -t "$SESSION" 2>/dev/null + +# ============================================= +# Window 1: control (4 equal panes) +# ============================================= +tmux new-session -d -s "$SESSION" -n "control" + +PANE_C1=$(tmux list-panes -t "$SESSION:control" -F '#{pane_id}') +tmux send-keys -t "$PANE_C1" "s && ros2 launch auv_setup thruster.launch.py" Enter + +PANE_C2=$(tmux split-window -h -t "$PANE_C1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_C2" "s && ros2 launch operation_mode_manager operation_mode_manager.launch.py" Enter + +PANE_C3=$(tmux split-window -v -t "$PANE_C1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_C3" "s && ros2 launch $DP_LAUNCH" Enter + +PANE_C4=$(tmux split-window -v -t "$PANE_C2" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_C4" "s" Enter + +tmux select-layout -t "$SESSION:control" tiled + +# ============================================= +# Window 2: perception (4 equal panes) +# ============================================= +tmux new-window -t "$SESSION" -n "perception" + +PANE_P1=$(tmux list-panes -t "$SESSION:perception" -F '#{pane_id}') +tmux send-keys -t "$PANE_P1" "s && ros2 launch auv_setup nucleus_odom_transformer.launch.py" Enter + +PANE_P2=$(tmux split-window -h -t "$PANE_P1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_P2" "s" Enter + +PANE_P3=$(tmux split-window -v -t "$PANE_P1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_P3" "s" Enter + +PANE_P4=$(tmux split-window -v -t "$PANE_P2" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_P4" "s" Enter + +tmux select-layout -t "$SESSION:perception" tiled + +# ============================================= +# Window 3: misc (4 equal panes) +# ============================================= +tmux new-window -t "$SESSION" -n "misc" + +PANE_M1=$(tmux list-panes -t "$SESSION:misc" -F '#{pane_id}') +tmux send-keys -t "$PANE_M1" "s" Enter + +PANE_M2=$(tmux split-window -h -t "$PANE_M1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_M2" "s" Enter + +PANE_M3=$(tmux split-window -v -t "$PANE_M1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_M3" "s" Enter + +PANE_M4=$(tmux split-window -v -t "$PANE_M2" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_M4" "s" Enter + +tmux select-layout -t "$SESSION:misc" tiled + +# ============================================= +# Focus control window and attach +# ============================================= +tmux select-window -t "$SESSION:control" +tmux attach-session -t "$SESSION"