diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index 3a84897c4..a2111627e 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -5,7 +5,8 @@ from launch.actions import ( OpaqueFunction, ) -from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode from auv_setup.launch_arg_common import ( declare_drone_and_namespace_args, @@ -15,45 +16,52 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) - - velocity_lqr_config = os.path.join( - get_package_share_directory("velocity_controller_lqr"), - "config", - "param_velocity_controller_lqr.yaml", - ) - - los_config = os.path.join( - get_package_share_directory("los_guidance"), - "config", - "guidance_params.yaml", - ) - drone_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - f"{drone}.yaml", + get_package_share_directory('auv_setup'), + 'config', + 'robots', + f'{drone}.yaml', ) - - los_node = Node( - package="los_guidance", - executable="los_guidance_node", - name="los_guidance_node", - namespace=namespace, - parameters=[drone_params, los_config], - output="screen", + velocity_control_params = os.path.join( + get_package_share_directory('velocity_controller'), + 'config', + f'{drone}_params.yaml', ) - - lqr_node = Node( - package="velocity_controller_lqr", - executable="velocity_controller_lqr_node.py", - name="velocity_controller_lqr_node", + los_config = os.path.join( + get_package_share_directory('los_guidance'), + 'config', + 'guidance_params.yaml', + ) + container = ComposableNodeContainer( + name='autopilot_container', namespace=namespace, - output="screen", - parameters=[drone_params, velocity_lqr_config], + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='velocity_controller', + plugin='velocity_node', + name='velocity_controller_node', + namespace=namespace, + parameters=[velocity_control_params, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package='los_guidance', + plugin='vortex::guidance::los::LosGuidanceNode', + name='los_guidance_node', + namespace=namespace, + parameters=[ + drone_params, + {"los_config_file": los_config, "time_step": 0.1}, + ], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output='screen', + arguments=['--ros-args', '--log-level', 'error'], ) - - return [los_node, lqr_node] + return [container] def generate_launch_description(): diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt new file mode 100644 index 000000000..4184c36ff --- /dev/null +++ b/control/velocity_controller/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(velocity_controller) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(ct_optcon REQUIRED) +find_package(ct_core REQUIRED) +find_package(casadi REQUIRED) + +include_directories( + include +) + + +set(LIB_NAME velocity_controller_component) +add_library(${LIB_NAME} SHARED + src/velocity_controller_ros.cpp + src/PID_controller.cpp + src/LQR_setup.cpp + src/utilities.cpp + src/ct_instantiations.cpp + src/control_manager.cpp + src/3DOF_PID.cpp +) +rclcpp_components_register_nodes(${LIB_NAME} "Velocity_node") +ament_target_dependencies(${LIB_NAME} + rclcpp + rclcpp_components + rclcpp_lifecycle + lifecycle_msgs + std_msgs + vortex_msgs + geometry_msgs + nav_msgs + vortex_utils +) +target_link_libraries(${LIB_NAME} Eigen3::Eigen casadi::casadi ct_optcon ct_core) +install(TARGETS + ${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(tests) +endif() + +ament_package() diff --git a/control/velocity_controller/README.md b/control/velocity_controller/README.md new file mode 100644 index 000000000..214dc58fe --- /dev/null +++ b/control/velocity_controller/README.md @@ -0,0 +1,201 @@ +# velocity_controller + +ROS2 lifecycle node for velocity control of an AUV (autonomous underwater vehicle). Supports two control strategies — a PID controller and an LQR controller. + +--- + +## Overview + +The package implements a `Velocity_node` that subscribes to odometry and guidance inputs, computes thrust commands, and publishes them as `WrenchStamped` messages. The node is managed as a ROS2 lifecycle node, meaning it can be managed by a lifecycle manager, however if you do not want to use a lifecycle manager you can change the parameter autostart in the parameter file so that it automatically goes into active state. + +The LQR controller linearizes the vehicle dynamics around the current state at each timestep (gain-scheduled LQR), using a body-frame model that includes linear hydrodynamic damping, Coriolis effects, and integral action for steady-state error rejection. The PID controller serves as a simpler backup. + +--- + +## Dependencies +//TODO(henrimha): fix the dependencies +| Dependency | Purpose | +|---|---| +| `rclcpp` / `rclcpp_lifecycle` | ROS2 node and lifecycle management | +| `Eigen3` | Matrix math for LQR | +| `control_toolbox` (`ct::optcon`) | Riccati equation solver for LQR gain | +| `CasADi` | Used in utilities (NMPC-related) | +| `vortex_msgs` | Custom guidance message (`LOSGuidance`) | +| `nav_msgs` | Odometry input | +| `geometry_msgs` | Thrust output (`WrenchStamped`) | + +--- + +## Topics + +| Topic | Type | Direction | Description | +|---|---|---|---| +| `topic_thrust` | `geometry_msgs/WrenchStamped` | Published | Force and torque commands to thruster allocator | +| `topic_guidance` | `vortex_msgs/LOSGuidance` | Subscribed | Desired surge, pitch, yaw from guidance system | +| `topic_odometry` | `nav_msgs/Odometry` | Subscribed | Current vehicle state from state estimator | + +Topic names are configurable via ROS2 global parameter file. + +--- + +## Parameters + +All parameters are loaded in the constructor via `get_new_parameters()`. + +### Controller selection + +| Parameter | Type | Description | +|---|---|---| +| `controller_type` | `int` | `1` = PID, `2` = LQR | +| `publish_rate` | `int` | Control loop frequency in Hz | +| `max_force` | `double` | Saturation limit applied to all outputs (N / Nm) | + +### LQR parameters + +| Parameter | Type | Dimension | Description | +|---|---|---|---| +| `Q` | `double[]` | 8 | Diagonal of state weight matrix. States: `[surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw]` | +| `R` | `double[]` | 3 | Diagonal of input weight matrix. Inputs: `[Fx, Ty, Tz]` | +| `inertia_matrix` | `double[]` | 36 | Row-major 6x6 rigid body inertia matrix | +| `dampening_matrix_low` | `double[]` | 36 | Row-major 6x6 hydrodynamic damping matrix at low speed | + +### PID parameters + +Each axis (surge, pitch, yaw) takes a parameter vector of the form `[Kp, Ki, Kd]`. + +| Parameter | Description | +|---|---| +| `surge_params` | PID gains for surge | +| `pitch_params` | PID gains for pitch | +| `yaw_params` | PID gains for yaw | + +### Behaviour flags + +| Parameter | Type | Description | +|---|---|---| +| `reset_on_new_ref` | `bool` | Reset integrators when a new guidance reference arrives | +| `anti_overshoot` | `bool` | Enable anti-overshoot logic | +| `odometry_dropout_guard` | `bool` | Stop publishing if odometry stops arriving | +| `auto_start` | `bool` | If true, node self-transitions to active on startup | + + +--- + +## Lifecycle states + +The node uses the standard ROS2 managed lifecycle: + +``` +Unconfigured → [configure] → Inactive → [activate] → Active + ← [deactivate] ← +``` + +If `auto_start` is set to `true` in the parameters, the node will automatically call configure and activate itself after startup without needing an external lifecycle manager. + +To manually manage the node: + +```bash +# Configure +ros2 lifecycle set /velocity_controller configure + +# Activate +ros2 lifecycle set /velocity_controller activate + +# Deactivate +ros2 lifecycle set /velocity_controller deactivate + +# Cleanup +ros2 lifecycle set /velocity_controller cleanup + +# Shutdown +ros2 lifecycle set /velocity_controller shutdown + +``` + +--- + +## Controller details + +### LQR + +The LQR controller uses an 8-state augmented model in the body frame: + +``` +x = [surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw] +u = [Fx, Ty, Tz] +``` + +The system matrix `A` is re-linearized around the current state every control timestep. Guidance references in NED are converted to body-frame errors using the rotation matrix method before being passed to the controller — not by angle subtraction. + +The gain `K` is computed by solving the continuous-time algebraic Riccati equation via `ct::optcon::LQR`. The control law is: + +``` +u = K * x_error +``` +//TODO(henrimha): give the riccatti equation +where `ct::optcon` produces `K` such that this is equivalent to `u = -K * (x - x_ref)`. + +If the Riccati solver fails (e.g. due to an unstabilizable operating point), the node automatically falls back to PID and logs an error. + +### PID + +Three independent PID controllers run on surge, pitch, and yaw. Each supports anti-windup via integrator clamping. The derivative term can be computed either from the error signal or from a separately provided error derivative, depending on which `calculate_thrust` overload is called. + +--- + +## Building + +```bash +colcon build --packages-select velocity_controller --symlink-install +source install/setup.bash +``` +or with + +```bash +colcon build --packages-up-to velocity_controller --symlink-install +source install/setup.bash +``` +Done in root of workspace +--- + +## Running + +Via a launch file with a parameter file: + +```bash +ros2 launch velocity_controller velocity_controller.launch.py +``` + +--- + +## Tests +There are system tests and a helper node that generates a reference for the controller to follow. +Tests are build like this: +```bash +colcon build --packages-select velocity_controller --symlink-install --cmake-args -DBUILD_TESTING=ON +source install/setup.bash +``` +System tests are run with the command + +```bash +colcon test +``` + +Helper node is run with + +```bash +ros2 launch velocity_controller VCnTest.launch.py +``` + +## Notes for new team members + +- The guidance input is expected in NED frame (north-east-down). The controller handles the NED-to-body conversion internally. +- All angle errors are wrapped to `[-π, π]` using `ssa()` (smallest signed angle) before being fed to the controller. +- The LQR Q matrix ordering matters — the 8 diagonal values correspond exactly to `[surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw]` in that order. +- If the vehicle behaves oddly, check that `interval_` (the control timestep) is being set correctly — a value of `0` disables integral action silently. + +## Adding new controllers +After adding the hpp file, add the calculation to calc_thrust function in a new switch case, add to the reset_controller function, with options to reset only one integral, lastly update documentation. Remember to initialize correctly, either in 'on_configure' or in constructor, add the appropriate parameters, and update all the {drone}_params.yaml files. + +## Adding new drones +Copy a {drone}_params.yaml file and change the name to the new name of the drone. Add the appropriate matrices, and tune to satisfying behaviour. diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml new file mode 100644 index 000000000..f3513edd2 --- /dev/null +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + + 3DOF_PID_params: + surge: [500.0,50.0,5.0] + pitch: [60.0,8.0,12.0] + yaw: [10.0,1.0,5.0] + + LQR_params: + Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] + R: [0.02,3.1,3.10] + #TODO(henrimha): move these to the global parameter file + dampening_matrix_low: [104.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] + dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] + + + Node_settings: #Settings for the controller + auto_start: true #0 for no, 1 for yes + reset_on_new_ref: true + odometry_dropout_guard: true + Control_manager_settings: + anti_overshoot: false + publish_rate: 100 #ms + max_force: 99.5 + controller_type: 1 #1 PID 2 LQR + diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml new file mode 100644 index 000000000..479649d35 --- /dev/null +++ b/control/velocity_controller/config/orca_params.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + + 3DOF_PID_params: + surge: [300.0,10.0,5.0] + pitch: [60.0,8.0,12.0] + yaw: [10.0,1.0,5.0] + + LQR_params: + Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] + R: [0.02,3.1,3.1] + #TODO(henrimha): move these to the global parameter file + dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] + dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] + + + Node_settings: #Settings for the controller + auto_start: false #0 for no, 1 for yes + reset_on_new_ref: true + odometry_dropout_guard: true + Control_manager_settings: + anti_overshoot: false + publish_rate: 100 #ms + max_force: 99.5 + controller_type: 2 #1 PID 2 LQR diff --git a/control/velocity_controller/include/tests/test_VC.hpp b/control/velocity_controller/include/tests/test_VC.hpp new file mode 100644 index 000000000..a757daa03 --- /dev/null +++ b/control/velocity_controller/include/tests/test_VC.hpp @@ -0,0 +1,58 @@ +#ifndef TEST_VC_HPP_ +#define TEST_VC_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "nav_msgs/msg/odometry.hpp" +#include "velocity_controller/utilities.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" + +/** + @brief A class that sends a reference signal to the velocity controller +*/ +class test_velocity_controller : public rclcpp::Node { + public: + explicit test_velocity_controller(); + test_velocity_controller(const test_velocity_controller&) = delete; // no copy constructor + test_velocity_controller& operator=(const test_velocity_controller&) = delete; // no copy assignment + test_velocity_controller(test_velocity_controller&&) = delete; // no move constructor + test_velocity_controller& operator=(test_velocity_controller&&) = delete; // no move assignment + ~test_velocity_controller()=default; + private: + /** + * @brief Publishes a reference signal to the reference topic of the velocity controller. + */ + void send_reference(); + /** + * @brief Subscribes to the odometry topic and prints the current state (in euler angles) of the vehicle for debugging. + */ + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); + + // Subscribers and publishers + rclcpp::Publisher::SharedPtr publisher_guidance; + rclcpp::Publisher::SharedPtr publisher_state; + rclcpp::Subscription::SharedPtr subscription_state; + // Timers + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Clock::SharedPtr clock_; + // Messages + vortex_msgs::msg::LOSGuidance reference_msg; + // Topics + std::string topic_guidance; + std::string topic_state = "/state"; + std::string topic_odometry; + + /** + * @brief The total time elapsed since the start of the simulation. Used to calculate the reference signal as a function of time. + */ + + double totaltime = 0; +}; + +#endif // TEST_VC_HPP_ \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/control_manager.hpp b/control/velocity_controller/include/velocity_controller/control_manager.hpp new file mode 100644 index 000000000..5346ceb7d --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/control_manager.hpp @@ -0,0 +1,32 @@ +#ifndef VELOCITY_CONTROLLER__CONTROL_MANAGER_HPP_ +#define VELOCITY_CONTROLLER__CONTROL_MANAGER_HPP_ +#include "velocity_controller/lib/controller.hpp" +#include "velocity_controller/utilities.hpp" +#include "velocity_controller/lib/3DOF_PID.hpp" +#include "velocity_controller/lib/LQR_setup.hpp" +struct control_manager_params{ + int control_type; // 1 3DOF PID, 2 3DOF LQR + bool anti_overshoot; + bool fallback; + control_manager_params(int control_type_val, bool anti_overshoot_val, bool fallback_val) + : control_type(control_type_val), anti_overshoot(anti_overshoot_val), fallback(fallback_val) {} + control_manager_params() = default; +}; +class control_manager{ + public: + control_manager(control_manager_params params); + bool switch_controller(); + void shutdown_controller(); + geometry_msgs::msg::WrenchStamped get_output(Guidance_data guidanceState, State State); + void initialize_3DOF_controller(PID_3DOF_params params); + void initialize_LQR_controller(LQR_params params); + bool get_validity(); + void reset_controllers(int nr = 0); + private: + control_manager_params params_; + std::unique_ptr controller_3DOF=nullptr; + std::unique_ptr controller_LQR=nullptr; + + +}; +#endif // VELOCITY_CONTROLLER__CONTROL_MANAGER_HPP_ \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp b/control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp new file mode 100644 index 000000000..7851df7c5 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/lib/3DOF_PID.hpp @@ -0,0 +1,30 @@ +#ifndef VELOCITY_CONTROLLER__3DOF_PID_HPP_ +#define VELOCITY_CONTROLLER__3DOF_PID_HPP_ + +#include "velocity_controller/lib/controller.hpp" +#include "velocity_controller/utilities.hpp" +#include +#include "velocity_controller/lib/PID_controller.hpp" +struct PID_3DOF_params{ + std::vector surge; + std::vector pitch; + std::vector yaw; + double dt; + double max_force; + double min_force; +}; +class PID_3DOF : public controller { + public: + PID_3DOF(PID_3DOF_params params); + geometry_msgs::msg::WrenchStamped calculate_thrust(State state, State error_state) override; + void reset_controller(int nr=0) override; + ///bool set_parameters(PID_params surge_params, PID_params pitch_params, PID_params yaw_params); + + private: + PID_controller surge_controller; + PID_controller pitch_controller; + PID_controller yaw_controller; +}; + + +#endif // VELOCITY_CONTROLLER__3DOF_PID_HPP_ \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/lib/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/lib/LQR_setup.hpp new file mode 100644 index 000000000..940042658 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/lib/LQR_setup.hpp @@ -0,0 +1,62 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include "velocity_controller/lib/controller.hpp" +#include "velocity_controller/utilities.hpp" +//TODO(henrimha): Make the constructor take in a LQR_params struct and make a LQR_params struct in the header file +//TODO(henrimha): figure out how to hold the matrices in the class, and what to take in as parameters +struct LQR_params{ + std::vector Q; + std::vector R; + std::vector inertia_matrix; + double max_force; + std::vector D_low; + std::vector D_high; + double interval; + LQR_params(std::vector Q_val, std::vector R_val, std::vector inertia_matrix_val, double max_force_val, std::vector D_low_val, std::vector D_high_val, double interval_val) + : Q(Q_val), R(R_val), inertia_matrix(inertia_matrix_val), max_force(max_force_val), D_low(D_low_val), D_high(D_high_val), interval(interval_val) {}; + LQR_params() = default; +}; +class LQRController : public controller{ + public: + LQRController(LQR_params params); + void reset_controller(int nr = 0) override; + geometry_msgs::msg::WrenchStamped calculate_thrust(State state, State error_state) override; + + private: + Eigen::Matrix linearize(State states); + Eigen::Matrix coriolis(const State& s); + + std::tuple saturate(double value, + bool windup, + double limit); + double anti_windup(double error, double integral_sum, bool windup); + Eigen::Vector saturate_input(Eigen::Vector u); + + Eigen::Vector update_error(const State& error_state, + const State& state); + LQR_params params_; + double integral_error_surge; + double integral_error_pitch; + double integral_error_yaw; + bool surge_windup; + bool pitch_windup; + bool yaw_windup; + Eigen::Matrix Q; + Eigen::Matrix R; + Eigen::Matrix B; + Eigen::Matrix D; + double mass, Ixx, Iyy, Izz; + + Eigen::Matrix inertia_matrix_inv; + Eigen::Matrix state_weight_matrix; + Eigen::Matrix3d input_weight_matrix; + Eigen::Matrix augmented_system_matrix; + Eigen::Matrix augmented_input_matrix; +}; diff --git a/control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp b/control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp new file mode 100644 index 000000000..51d2b1105 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/lib/PID_controller.hpp @@ -0,0 +1,43 @@ +#pragma once +#include +#include +#include +#include +#include +//TODO(henrimha): Make the constructor take in a PID_params struct and make a PID_params struct in the header file +struct PID_params{ + double k_p, k_i, k_d; + double dt; + double max_output; + double min_output; + const std::vector& operator=(const std::vector& params); + // Constructor from gains vector and other params + //TODO(henrimha): change the vector to an array + PID_params(const std::vector& gains, double dt_val, double max_out, double min_out) + : k_p(gains[0]), k_i(gains[1]), k_d(gains[2]), dt(dt_val), max_output(max_out), min_output(min_out) {} + + PID_params() = default; + +}; + +class PID_controller { + public: + explicit PID_controller(PID_params params); + /** @brief Calculates the thrust based on the error and internal parameters, with a default derivative of 0*/ + /** @brief Calculates the thrust based on the error and internal parameters*/ + double calculate_thrust(double error); + /** @brief Calculates the thrust based on the error with external derivative */ + double calculate_thrust(double error, double error_d); + /** @brief Resets the all internal states, inlcuding integral, output and previous error */ + void reset_controller(); + /** @brief Returns the validity of the controller */ + bool get_validity(){return valid;}; + + private: + /** @brief internal variables for calculation */ + PID_params params_; + double integral = 0; + double previous_error = 0; + /** @brief Indicates if the controller is initialized properly*/ + bool valid = false; +}; diff --git a/control/velocity_controller/include/velocity_controller/lib/controller.hpp b/control/velocity_controller/include/velocity_controller/lib/controller.hpp new file mode 100644 index 000000000..6a4b622cc --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/lib/controller.hpp @@ -0,0 +1,20 @@ +#ifndef VELOCITY_CONTROLLER__CONTROLLER_HPP_ +#define VELOCITY_CONTROLLER__CONTROLLER_HPP_ +#include +#include "velocity_controller/utilities.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" + +class controller{ + public: + controller()=default; + virtual geometry_msgs::msg::WrenchStamped calculate_thrust(State state, State error_state) = 0; + virtual void reset_controller(int nr=0) = 0; + bool get_validity(){return valid;}; + ~controller()=default; + protected: + bool valid=false; +}; + + + +#endif // VELOCITY_CONTROLLER__CONTROLLER_HPP_ \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp new file mode 100644 index 000000000..9060bc5da --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -0,0 +1,67 @@ +#ifndef UTILITIES_HPP_ +#define UTILITIES_HPP_ +#include +#include +#include +#include +#include +#include "std_msgs/msg/float64_multi_array.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" + +struct angle { + double phit = 0.0; + double thetat = 0.0; + double psit = 0.0; +}; +angle quaternion_to_euler_angle(double w, double x, double y, double z); + +class State { + public: + double surge = 0.0, sway = 0.0, heave = 0.0, roll_rate = 0.0, + pitch_rate = 0.0, + yaw_rate = 0.0; // roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; + double roll = 0.0, pitch = 0.0, yaw = 0.0; // phi, theta, psi + double w = 0.0, x = 0.0, y = 0.0, z = 0.0; + explicit State(double surge = 0, double pitch = 0, double yaw = 0) + : surge{surge}, pitch{pitch}, yaw{yaw} {} + + State operator=(int n) { + if (n) { + surge = 0.0, sway = 0.0, heave = 0.0, roll_rate = 0.0, + pitch_rate = 0.0, yaw_rate = 0.0, roll = 0.0, pitch = 0.0, + yaw = 0.0; + } + return *this; + } + State operator=(nav_msgs::msg::Odometry::SharedPtr rhs); + angle get_angle(); +}; +class Guidance_data { + public: + double surge = 0.0; + double pitch = 0.0; + double yaw = 0.0; + Guidance_data(double surge, double pitch, double yaw) + : surge{surge}, pitch{pitch}, yaw{yaw} {}; + Guidance_data() : surge{0.0}, pitch{0.0}, yaw{0.0} {}; + Guidance_data& operator=( + const vortex_msgs::msg::LOSGuidance::SharedPtr& msg); +}; + +Eigen::Vector3d body_rates_to_euler_rates(double roll, + double pitch, + double p, + double q, + double r); +angle angle_NED_to_body(double roll_des, + double pitch_des, + double yaw_des, + double roll, + double pitch, + double yaw); +angle angle_NED_to_body(angle desired, angle state); +inline angle angle_NED_to_body(angle desired, angle state) { + return angle_NED_to_body(desired.phit, desired.thetat, desired.psit, + state.phit, state.thetat, state.psit); +} +#endif // UTILITIES_HPP_ \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller_ros.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller_ros.hpp new file mode 100644 index 000000000..3ae26300c --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/velocity_controller_ros.hpp @@ -0,0 +1,97 @@ +#ifndef VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ +#define VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ +//#include +//#include +#include +#include +#include +//#include +//#include +//#include +#include +#include "nav_msgs/msg/odometry.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" +#include +#include "velocity_controller/control_manager.hpp" + +struct Node_settings{ + //special settings + bool auto_start; + bool reset_on_new_ref; + bool odometry_dropout_guard; + + // Variables for timers + int publish_rate=100; //ms + + // Variables for topics + std::string topic_thrust; + std::string topic_guidance; + std::string topic_killswitch; + std::string topic_odometry; +}; +class Velocity_node : public rclcpp_lifecycle::LifecycleNode { + public: + explicit Velocity_node(const rclcpp::NodeOptions& options); + Velocity_node(const Velocity_node&) = delete; // no copy constructor + Velocity_node& operator=(const Velocity_node&) =delete; // no copy assignment + Velocity_node(Velocity_node&&) = delete; // no move constructor + Velocity_node& operator=(Velocity_node&&) = delete; // no move assignment + + private: + void get_new_parameters(); + void initialize_controllers(); + + // Timer functions + void publish_thrust(); + + // Callback functions + void guidance_callback( + const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr); + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); + + // Publisher instance + rclcpp::Publisher::SharedPtr + publisher_thrust; + + // Timer instance + rclcpp::TimerBase::SharedPtr timer_calculation; + rclcpp::TimerBase::SharedPtr startup_timer_; + // Subscriber instance + rclcpp::Subscription::SharedPtr + subscriber_Odometry; + rclcpp::Subscription::SharedPtr + subscriber_guidance; + + + Node_settings node_settings; + // Control manager instance + std::unique_ptr control_manager_ptr; + // Stored wrenches values + vortex_msgs::msg::LOSGuidance reference_in; + geometry_msgs::msg::WrenchStamped thrust_out; + Guidance_data guidance_values; + State current_state; + + std::atomic_bool should_exit_{false}; + //bool anti_overshoot; + int publish_counter = 0; + bool first_start = true; + //int controller_type; // 1 PID, 2 LQR + + // States + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State&) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State& state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State& state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State&) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State& state) override; + + //void reset_controllers(int nr = 0); + rclcpp::QoS pub_QoS; + rclcpp::QoS sub_QoS; +}; +#endif // VELOCITY_CONTROLLER__VELOCITY_CONTROLLER_HPP_ diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py new file mode 100644 index 000000000..efda55f7f --- /dev/null +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -0,0 +1,76 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, + TimerAction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +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) + global_share = get_package_share_directory('auv_setup') + config_path_global = os.path.join(global_share, 'config', 'robots', f'{drone}.yaml') + + stonefish_dir = get_package_share_directory('stonefish_sim') + + stonefish_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') + ), + launch_arguments={ + "drone": drone, + 'scenario': 'nautilus_no_gpu', + 'rendering_quality': 'low', + 'rendering': 'true', + }.items(), + ) + + drone_sim = TimerAction( + period=12.0, + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'drone_sim.launch.py') + ) + ) + ], + ) + + node_name_arg = DeclareLaunchArgument( + 'node_name_1', + default_value='test_vc_node', + description='Name of the test velocity controller node', + ) + test_vc_name = LaunchConfiguration('node_name_1') + + return [ + stonefish_sim, + drone_sim, + node_name_arg, + Node( + package='velocity_controller', + executable='test_vc_node', + name=test_vc_name, + namespace=namespace, + output='screen', + parameters=[config_path_global], + ), + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) diff --git a/control/velocity_controller/launch/velocity_controller.launch.py b/control/velocity_controller/launch/velocity_controller.launch.py new file mode 100644 index 000000000..50c7aba59 --- /dev/null +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -0,0 +1,58 @@ +import os + +# from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + +# from launch.actions import IncludeLaunchDescription +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, +) +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + pkg_share = get_package_share_directory('velocity_controller') + global_share = get_package_share_directory('auv_setup') + config_path_local = os.path.join(pkg_share, 'config', f'{drone}_params.yaml') + config_path_global = os.path.join(global_share, 'config', 'robots', f"{drone}.yaml") + + #return [ + # Node( + # package='velocity_controller', + # executable='velocity_node', + # name="velocity_controller_node", + # namespace=namespace, + # parameters=[config_path_local, config_path_global], + # ) + #] + return [ + ComposableNodeContainer( + name='velocity_container', + namespace=namespace, + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='velocity_controller', + plugin='Velocity_node', # must match the class name registered + name='velocity_controller_node', + namespace=namespace, + parameters=[config_path_local, config_path_global], + ), + ], + output='screen', + ) +] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml new file mode 100644 index 000000000..3eea35266 --- /dev/null +++ b/control/velocity_controller/package.xml @@ -0,0 +1,36 @@ + + + + velocity_controller + 0.0.0 + TODO: Package description + henrik + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + vortex_msgs + geometry_msgs + nav_msgs + vortex_utils + rclcpp_lifecycle + rclcpp_components + lifecycle_msgs + auv_setup + ct_optcon + ct_core + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + + + + + diff --git a/control/velocity_controller/src/3DOF_PID.cpp b/control/velocity_controller/src/3DOF_PID.cpp new file mode 100644 index 000000000..c69301a35 --- /dev/null +++ b/control/velocity_controller/src/3DOF_PID.cpp @@ -0,0 +1,39 @@ +#include "velocity_controller/lib/3DOF_PID.hpp" +//#include "velocity_controller/lib/PID_controller.hpp" +#include + +PID_3DOF::PID_3DOF(PID_3DOF_params params) + : surge_controller(PID_params(params.surge,params.dt,params.max_force,params.min_force)), pitch_controller(PID_params(params.pitch,params.dt,params.max_force,params.min_force)), yaw_controller(PID_params(params.yaw,params.dt,params.max_force,params.min_force)) {}; + +geometry_msgs::msg::WrenchStamped PID_3DOF::calculate_thrust(State state, State error_state) { + geometry_msgs::msg::WrenchStamped u; + u.wrench.force.set__x(surge_controller.calculate_thrust(error_state.surge)); + u.wrench.torque.set__y(pitch_controller.calculate_thrust(error_state.pitch,error_state.yaw_rate)); + u.wrench.torque.set__z(yaw_controller.calculate_thrust(error_state.yaw, error_state.yaw_rate)); + if(surge_controller.get_validity() && pitch_controller.get_validity() && yaw_controller.get_validity()){ + valid = true; + } else { + valid = false; + } + return u; +} + +void PID_3DOF::reset_controller(int nr){ + switch (nr){ + case 0: + surge_controller.reset_controller(); + pitch_controller.reset_controller(); + yaw_controller.reset_controller(); + break; + case 1: + surge_controller.reset_controller(); + break; + case 2: + pitch_controller.reset_controller(); + break; + case 3: + yaw_controller.reset_controller(); + break; + } + return; +} \ No newline at end of file diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp new file mode 100644 index 000000000..a0df84d1f --- /dev/null +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -0,0 +1,216 @@ +#include "velocity_controller/lib/LQR_setup.hpp" +#include +#include +#include +#include +// #include +#include +#include +// #include "rclcpp/rclcpp.hpp" +#include +// #include "velocity_controller/PID_setup.hpp" +#include "ct/optcon/lqr/LQR.hpp" +#include "velocity_controller/utilities.hpp" +// #include "vortex/utils/math.hpp" + +LQRController::LQRController(LQR_params params) : params_(params) { + inertia_matrix_inv.setZero(); + if (params_.interval <= 0){ + valid = false; + return; + } + else if (params_.Q.size() != 8) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "The Q matrix has the wrong amount of elements"); + valid = false; + return; + } + else if (params_.R.size() != 3) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "The R matrix has the wrong amount of elements"); + valid = false; + return; + } + else if (params_.inertia_matrix.size() != 36) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "The M matrix has the wrong amount of elements"); + valid = false; + return; + + } + else if (params_.max_force < 0) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "The max_force need to be >0"); + valid = false; + return; + } + Q.diagonal() = Eigen::Map(params_.Q.data(), params_.Q.size()); + R.diagonal() = Eigen::Map(params_.R.data(), params_.R.size()); + Ixx = params_.inertia_matrix.at(6 * 3 + 3); + Iyy = params_.inertia_matrix.at(4 * 6 + 4); + Izz = params_.inertia_matrix.at(5 * 6 + 5); + mass = params_.inertia_matrix.at(0); + + Eigen::Matrix inertia_matrix = + Eigen::Map>(params_.inertia_matrix.data(), 6, + 6); + D = Eigen::Map>(params_.D_low.data(), 6, 6); + inertia_matrix_inv = inertia_matrix.inverse(); + + Eigen::Matrix B_t = + inertia_matrix_inv * (Eigen::Matrix() << 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1) + .finished(); + Eigen::Matrix B_m = Eigen::Matrix::Zero(); + B_m.block<6, 3>(0, 0) = B_t; + std::vector> swaplines{{1, 7}, {2, 8}, {3, 4}, {4, 5}}; + for (int64_t i = 0; i < swaplines.size(); i++) { + B_m.row(swaplines[i][0]).swap(B_m.row(swaplines[i][1])); + } + B.block<5, 3>(0, 0) = B_m.block<5, 3>(0, 0); + reset_controller(); + valid = true; +} + +std::tuple LQRController::saturate(double value, + bool windup, + double limit) { + if (abs(value) > limit) { + windup = true; + value = limit * (value / abs(value)); + } else { + windup = false; + } + return {windup, value}; +} + +double LQRController::anti_windup(double error, + double integral_sum, + bool windup) { + if (!windup) { + integral_sum += error * params_.interval; + } + return integral_sum; +} + +Eigen::Matrix LQRController::linearize(State s) { + Eigen::Matrix D_ = Eigen::Matrix::Zero(); + + D_ = -inertia_matrix_inv * D; // Assuming linear dampening for now + + Eigen::Matrix C = coriolis(s); + + D_ -= inertia_matrix_inv * C; // To avoid unnecessary allocation + + Eigen::Matrix T = Eigen::Matrix::Identity(); + Eigen::Matrix A; + A.block<6, 6>(0, 0) = D_; + A.block<3, 3>(0, 6) = A.block<3, 3>(6, 0) = A.block<3, 3>(6, 6) = + Eigen::Matrix3d::Zero(); + A.block<3, 3>(6, 3) = T; + std::vector> swaplines{{1, 7}, {2, 8}, {3, 4}, {4, 5}}; + for (int64_t i = 0; i < swaplines.size(); i++) { + A.row(swaplines[i][0]).swap(A.row(swaplines[i][1])); + A.col(swaplines[i][0]).swap(A.col(swaplines[i][1])); + } + + Eigen::Matrix ret; + ret.setZero(); + ret.block<5, 5>(0, 0) = A.block<5, 5>(0, 0); + ret.block<3, 3>(5, 0) = Eigen::Matrix3d::Identity(); + + return ret; +} +Eigen::Vector LQRController::update_error(const State& error_state, + const State& state) { + double surge_error = error_state.surge; + double pitch_error = error_state.pitch; + double yaw_error = error_state.yaw; + integral_error_surge = + anti_windup(surge_error, integral_error_surge, surge_windup); + integral_error_pitch = + anti_windup(pitch_error, integral_error_pitch, pitch_windup); + integral_error_yaw = anti_windup(yaw_error, integral_error_yaw, yaw_windup); + + Eigen::Vector state_error = { + surge_error, pitch_error, yaw_error, + -state.pitch_rate, -state.yaw_rate, integral_error_surge, + integral_error_pitch, integral_error_yaw}; + return state_error; +} +Eigen::Vector LQRController::saturate_input( + Eigen::Vector u) { + double force_x, torque_y, torque_z; + std::tie(surge_windup, force_x) = saturate(u[0], surge_windup, params_.max_force); + std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, params_.max_force); + std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, params_.max_force); + return {force_x, torque_y, torque_z}; +} +geometry_msgs::msg::WrenchStamped LQRController::calculate_thrust(State state, State error_state) { + ct::optcon::LQR<8, 3> lqr; + Eigen::Matrix K_l; + bool INFO = lqr.compute(Q, R, linearize(state), B, K_l, true, false); + if (INFO == 0) + valid=false; + Eigen::Matrix state_error = update_error(error_state, state); + //TODO:(henrimha) fix how i return the value here + Eigen::Vectoru = saturate_input((K_l * state_error)); + geometry_msgs::msg::WrenchStamped wrench; + wrench.wrench.force.x = u[0]; + wrench.wrench.torque.y = u[1]; + wrench.wrench.torque.z = u[2]; + return wrench; +} +void LQRController::reset_controller(int nr) { + if (nr == 0 || nr == 1) { + integral_error_surge = 0.0; + surge_windup = false; + } + if (nr == 0 || nr == 2) { + integral_error_pitch = 0.0; + pitch_windup = false; + } + if (nr == 0 || nr == 3) { + integral_error_yaw = 0.0; + yaw_windup = false; + } + + return; +} + +// TODO(henrimha): double check the matrices here +Eigen::Matrix LQRController::coriolis(const State& s) { + double u = s.surge; + double v = s.sway; + double w = s.heave; + double p = s.roll_rate; + double q = s.pitch_rate; + double r = s.yaw_rate; + Eigen::Matrix C = Eigen::Matrix::Zero(); + + // Top-right block (translational-rotational coupling) + C(0, 4) = mass * w; + C(0, 5) = -mass * v; + C(1, 3) = -mass * w; + C(1, 5) = mass * u; + C(2, 3) = mass * v; + C(2, 4) = -mass * u; + + // Bottom-left block (rotational-translational coupling) + C(3, 1) = mass * w; + C(3, 2) = -mass * v; + C(4, 0) = -mass * w; + C(4, 2) = mass * u; + C(5, 0) = mass * v; + C(5, 1) = -mass * u; + + // Bottom-right block (rotational-rotational coupling) + C(3, 4) = Izz * r; + C(3, 5) = -Iyy * q; + C(4, 3) = -Izz * r; + C(4, 5) = Ixx * p; + C(5, 3) = Iyy * q; + C(5, 4) = -Ixx * p; + + return C; +} diff --git a/control/velocity_controller/src/PID_controller.cpp b/control/velocity_controller/src/PID_controller.cpp new file mode 100644 index 000000000..577582614 --- /dev/null +++ b/control/velocity_controller/src/PID_controller.cpp @@ -0,0 +1,74 @@ +#include "velocity_controller/lib/PID_controller.hpp" + +// TODO(henrimha): kanskje forbedre integrasjon og derivasjons beregningene +// TODO(henrimha): check for more errors, f.example Nan or very high integral +double PID_controller::calculate_thrust(double error) { + if (!valid) + return 0; + // P + I + D + integral += error * params_.dt; + double output = + params_.k_p * error + params_.k_i * integral + params_.k_d * (error - previous_error) / params_.dt; + previous_error = error; + // Saturation + //TODO(henrimha): add feature that allows integral to compound if the output is saturated, but only if the error is in the same direction as the output, otherwise it should anti-windup + if (output > params_.max_output) { + output = params_.max_output; + integral -= error * params_.dt; // anti-wind up + + } else if (output < params_.min_output) { + output = params_.min_output; + integral -= error * params_.dt; // anti-wind up + } + return output; +} +double PID_controller::calculate_thrust(double error, double error_d) { + if (!valid) + return 0; + integral += error * params_.dt; // anti-wind up + // P + I + D + double output = params_.k_p * error + params_.k_i * integral + params_.k_d * error_d; + previous_error = error; + // Saturation + if (output > params_.max_output) { + output = params_.max_output; + integral -= error * params_.dt; // anti-wind up + + } else if (output < params_.min_output) { + output = params_.min_output; + integral -= error * params_.dt; // anti-wind up + } + return output; +} +void PID_controller::reset_controller() { + integral = 0.0; + previous_error = 0.0; +} + +PID_controller::PID_controller(PID_params params){ + if (params.dt <= 0&¶ms.max_output < params.min_output) { + valid = false; + //TODO:(henrimha) throw an error or log an error message + return; + } + params_ = params; + valid = true; +} + +const std::vector& PID_params::operator=(const std::vector& params) { + if (params.size() == 6){ + throw std::invalid_argument("Invalid parameter size for PID_params"); + k_p = params[0]; + k_i = params[1]; + k_d = params[2]; + dt = params[3]; + max_output = params[4]; + min_output = params[5]; + } + else if(params.size() == 3) { + k_p = params[0]; + k_i = params[1]; + k_d = params[2]; + } + return params; +} \ No newline at end of file diff --git a/control/velocity_controller/src/control_manager.cpp b/control/velocity_controller/src/control_manager.cpp new file mode 100644 index 000000000..e10bc1ec1 --- /dev/null +++ b/control/velocity_controller/src/control_manager.cpp @@ -0,0 +1,74 @@ +#include "velocity_controller/control_manager.hpp" +#include "velocity_controller/lib/3DOF_PID.hpp" +#include "velocity_controller/lib/controller.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" +#include "vortex/utils/math.hpp" + +control_manager::control_manager(control_manager_params params) : params_(params) {} + +bool control_manager::switch_controller() { + // Implementation for switching controllers + return true; +} + +void control_manager::shutdown_controller() { + controller_3DOF.reset(); + controller_LQR.reset(); +} + +geometry_msgs::msg::WrenchStamped control_manager::get_output(Guidance_data guidance_values, State current_state) { + // TODO(henrimha): Do I need ssa here? + angle ref_in_body = + angle_NED_to_body({0, vortex::utils::math::ssa(guidance_values.pitch), + vortex::utils::math::ssa(guidance_values.yaw)}, + current_state.get_angle()); + State error_state_body; + error_state_body.surge = guidance_values.surge - current_state.surge; + error_state_body.pitch = -ref_in_body.thetat; + error_state_body.yaw = -ref_in_body.psit; + + if (params_.anti_overshoot) { + if (abs(error_state_body.yaw) < std::numbers::pi / 2 || + abs(error_state_body.pitch) < std::numbers::pi / 2) { + error_state_body.surge = + guidance_values.surge * cos(error_state_body.yaw) * cos(error_state_body.pitch); + } + } + switch (params_.control_type) { + case 1: + return controller_3DOF->calculate_thrust(current_state, error_state_body); + case 2: + return controller_LQR->calculate_thrust(current_state, error_state_body); + default: + return geometry_msgs::msg::WrenchStamped(); + } +} +void control_manager::initialize_3DOF_controller(PID_3DOF_params params) { + controller_3DOF = std::make_unique(params); +} + +void control_manager::initialize_LQR_controller(LQR_params params) { + controller_LQR = std::make_unique(params); +} + +bool control_manager::get_validity() { + switch (params_.control_type) { + case 1: + return controller_3DOF->get_validity(); + case 2: + return controller_LQR->get_validity(); + default: + return false; + } +} + +void control_manager::reset_controllers(int nr) { + switch (params_.control_type) { + case 1: + controller_3DOF->reset_controller(nr); + break; + case 2: + controller_LQR->reset_controller(nr); + break; + } +} \ No newline at end of file diff --git a/control/velocity_controller/src/ct_instantiations.cpp b/control/velocity_controller/src/ct_instantiations.cpp new file mode 100644 index 000000000..ac8ff543c --- /dev/null +++ b/control/velocity_controller/src/ct_instantiations.cpp @@ -0,0 +1,6 @@ +// This file exists ONLY to emit Control Toolbox symbols + +#include + +template class ct::optcon::LQR<8, 3>; +template class ct::optcon::CARE<8, 3>; diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp new file mode 100644 index 000000000..31a27453c --- /dev/null +++ b/control/velocity_controller/src/utilities.cpp @@ -0,0 +1,115 @@ +#include "velocity_controller/utilities.hpp" +#include +#include +#include +#include "Eigen/Dense" + +angle quaternion_to_euler_angle(double w, double x, double y, double z) { + double ysqr = y * y; + + double t0 = +2.0 * (w * x + y * z); + double t1 = +1.0 - 2.0 * (x * x + ysqr); + double phi = std::atan2(t0, t1); + + double t2 = +2.0 * (w * y - z * x); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + double theta = std::asin(t2); + + double t3 = +2.0 * (w * z + x * y); + double t4 = +1.0 - 2.0 * (ysqr + z * z); + double psi = std::atan2(t3, t4); + + return {phi, theta, psi}; +} + +State State::operator=(nav_msgs::msg::Odometry::SharedPtr rhs) { + w = rhs->pose.pose.orientation.w; + x = rhs->pose.pose.orientation.x; + y = rhs->pose.pose.orientation.y; + z = rhs->pose.pose.orientation.z; + + auto [r, p, y_] = quaternion_to_euler_angle(w, x, y, z); + roll = r; + pitch = p; + yaw = y_; + + // Angular velocity + roll_rate = rhs->twist.twist.angular.x; + pitch_rate = rhs->twist.twist.angular.y; + yaw_rate = rhs->twist.twist.angular.z; + // Velocity + surge = rhs->twist.twist.linear.x; + sway = rhs->twist.twist.linear.y; + heave = rhs->twist.twist.linear.z; + + return (*this); +} + +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, + double pitch, + double yaw) { + double cy = cos(yaw * 0.5); + double sy = sin(yaw * 0.5); + double cp = cos(pitch * 0.5); + double sp = sin(pitch * 0.5); + double cr = cos(roll * 0.5); + double sr = sin(roll * 0.5); + + geometry_msgs::msg::Quaternion q; + q.w = cr * cp * cy + sr * sp * sy; + q.x = sr * cp * cy - cr * sp * sy; + q.y = cr * sp * cy + sr * cp * sy; + q.z = cr * cp * sy - sr * sp * cy; + + return q; +} + +angle angle_NED_to_body(double roll_des, + double pitch_des, + double yaw_des, + double roll, + double pitch, + double yaw) { + double cr = std::cos(roll), sr = std::sin(roll); + double cp = std::cos(pitch), sp = std::sin(pitch); + double cy = std::cos(yaw), sy = std::sin(yaw); + + // R_current: NED to body for current attitude + Eigen::Matrix3d R_current; + R_current << cp * cy, cp * sy, -sp, sr * sp * cy - cr * sy, + sr * sp * sy + cr * cy, sr * cp, cr * sp * cy + sr * sy, + cr * sp * sy - sr * cy, cr * cp; + + double cr_d = std::cos(roll_des), sr_d = std::sin(roll_des); + double cp_d = std::cos(pitch_des), sp_d = std::sin(pitch_des); + double cy_d = std::cos(yaw_des), sy_d = std::sin(yaw_des); + + // R_desired: NED to body for desired attitude + Eigen::Matrix3d R_desired; + R_desired << cp_d * cy_d, cp_d * sy_d, -sp_d, + sr_d * sp_d * cy_d - cr_d * sy_d, sr_d * sp_d * sy_d + cr_d * cy_d, + sr_d * cp_d, cr_d * sp_d * cy_d + sr_d * sy_d, + cr_d * sp_d * sy_d - sr_d * cy_d, cr_d * cp_d; + + // R_error = R_desired * R_current^T + Eigen::Matrix3d R_error = R_desired * R_current.transpose(); + + // Extract euler angles from R_error — this gives the error in body frame + double pitch_err = std::asin(-R_error(2, 0)); + double roll_err = std::atan2(R_error(2, 1), R_error(2, 2)); + double yaw_err = std::atan2(R_error(1, 0), R_error(0, 0)); + + return {roll_err, pitch_err, yaw_err}; +} + +angle State::get_angle() { + return {roll, pitch, yaw}; +} +Guidance_data& Guidance_data::operator=( + const vortex_msgs::msg::LOSGuidance::SharedPtr& msg) { + surge = msg->surge; + pitch = msg->pitch; + yaw = msg->yaw; + return *this; +} diff --git a/control/velocity_controller/src/velocity_controller_ros.cpp b/control/velocity_controller/src/velocity_controller_ros.cpp new file mode 100644 index 000000000..07267d9e5 --- /dev/null +++ b/control/velocity_controller/src/velocity_controller_ros.cpp @@ -0,0 +1,234 @@ +#include "velocity_controller/velocity_controller_ros.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "velocity_controller/control_manager.hpp" +#include "velocity_controller/utilities.hpp" +#include "velocity_controller/lib/3DOF_PID.hpp" + +Velocity_node::Velocity_node(const rclcpp::NodeOptions& options) + : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle", options), + pub_QoS(10), + sub_QoS(10) { + get_new_parameters(); + initialize_controllers(); + pub_QoS.keep_last(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + sub_QoS.keep_last(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + // Automatically start in activate if auto_start is true + if (node_settings.auto_start) { + startup_timer_ = + create_wall_timer(std::chrono::milliseconds(0), [this]() { + startup_timer_->cancel(); + trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + }); + } + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); + + return; +} +//TODO(henrimha): Split the ROS part and the c++ part into seperate files/classes +void Velocity_node::publish_thrust() { + if (node_settings.odometry_dropout_guard) { + publish_counter++; + if (publish_counter >= 100) { + control_manager_ptr->reset_controllers(); + RCLCPP_WARN(this->get_logger(), "Odometry dropout, no thrust"); + return; + } + } + thrust_out = control_manager_ptr->get_output(guidance_values, current_state); + /*geometry_msgs::msg::WrenchStamped test; + test.wrench.force.set__y(25);*/ + publisher_thrust->publish(thrust_out); + return; +} + +// Callback functions +void Velocity_node::guidance_callback( + const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr) { + if (node_settings.reset_on_new_ref) { // On big step changes, reset the controllers to + // avoid big overshoots + if (abs(msg_ptr->surge - guidance_values.surge) >= 0.1) + control_manager_ptr->reset_controllers(1); + if (abs(msg_ptr->pitch - guidance_values.pitch) > std::numbers::pi / 4) + control_manager_ptr->reset_controllers(2); + if (abs(msg_ptr->yaw - guidance_values.yaw) > std::numbers::pi / 4) + control_manager_ptr->reset_controllers(3); + } + guidance_values = msg_ptr; // overloaded to fix all the internal states + + return; +} + +void Velocity_node::odometry_callback( + const nav_msgs::msg::Odometry::SharedPtr msg_ptr) { + publish_counter = 0; + current_state = msg_ptr; // overloaded to fix all the internal states + return; +} + +void Velocity_node::get_new_parameters() { + // topics + this->declare_parameter("topics.wrench_input"); + node_settings.topic_thrust = this->get_parameter("topics.wrench_input").as_string(); + this->declare_parameter("topics.guidance.los"); + node_settings.topic_guidance = this->get_parameter("topics.guidance.los").as_string(); + this->declare_parameter("topics.odom"); + node_settings.topic_odometry = this->get_parameter("topics.odom").as_string(); + + // Control manager settings + this->declare_parameter("Control_manager_settings.max_force"); + this->declare_parameter("Control_manager_settings.publish_rate"); + this->declare_parameter("Control_manager_settings.controller_type"); + this->declare_parameter("Control_manager_settings.anti_overshoot", true); + + //Node settings + this->declare_parameter("Node_settings.auto_start", false); + this->declare_parameter("Node_settings.reset_on_new_ref", true); + this->declare_parameter("Node_settings.odometry_dropout_guard", true); + + // PID Params + this->declare_parameter>("3DOF_PID_params.surge"); + this->declare_parameter>("3DOF_PID_params.pitch"); + this->declare_parameter>("3DOF_PID_params.yaw"); + + // LQR Parameters + this->declare_parameter>("LQR_params.Q"); + this->declare_parameter>("LQR_params.R"); + this->declare_parameter>("physical.mass_matrix"); + + // D + this->declare_parameter>("dampening_matrix_low"); + this->declare_parameter>("dampening_matrix_high"); + +} +void Velocity_node::initialize_controllers() { + //TODO(henrimha): parameter validation or in control manager + //Initialize Node + node_settings.auto_start = this->get_parameter("Node_settings.auto_start").as_bool(); + node_settings.reset_on_new_ref = this->get_parameter("Node_settings.reset_on_new_ref").as_bool(); + node_settings.publish_rate = this->get_parameter("Control_manager_settings.publish_rate").as_int(); + node_settings.odometry_dropout_guard = this->get_parameter("Node_settings.odometry_dropout_guard").as_bool(); + + + //Initialize control manager parameters + auto control_type = this->get_parameter("Control_manager_settings.controller_type").as_int(); + auto anti_overshoot = this->get_parameter("Control_manager_settings.anti_overshoot").as_bool(); + control_manager_params control_params(control_type, anti_overshoot, 1); + + + //Some general parameters + double max_force = this->get_parameter("Control_manager_settings.max_force").as_double(); + double dt = node_settings.publish_rate / 1000.0; // Convert ms to seconds + + // Initialize 3DOF_PID params + auto surge_gains = this->get_parameter("3DOF_PID_params.surge").as_double_array(); + auto pitch_gains = this->get_parameter("3DOF_PID_params.pitch").as_double_array(); + auto yaw_gains = this->get_parameter("3DOF_PID_params.yaw").as_double_array(); + PID_3DOF_params pid_3dof_params(surge_gains, pitch_gains, yaw_gains, dt, max_force, -max_force); + + //Initialize LQR controller params + auto Q = this->get_parameter("LQR_params.Q").as_double_array(); + auto R = this->get_parameter("LQR_params.R").as_double_array(); + auto inertia_matrix = this->get_parameter("physical.mass_matrix").as_double_array(); + auto D_low = this->get_parameter("dampening_matrix_low").as_double_array(); + auto D_high = this->get_parameter("dampening_matrix_high").as_double_array(); + LQR_params lqr_params(Q, R, inertia_matrix, max_force, D_low, D_high, dt); + + //Initalize all the controllers in control manager + control_manager_ptr = std::make_unique(control_params); + control_manager_ptr->initialize_3DOF_controller(pid_3dof_params); + control_manager_ptr->initialize_LQR_controller(lqr_params); + return; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_configure(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Configure VC"); + + // Publishers + publisher_thrust = create_publisher( + node_settings.topic_thrust, pub_QoS); + + // Subscribers + subscriber_Odometry = this->create_subscription( + node_settings.topic_odometry, sub_QoS, + std::bind(&Velocity_node::odometry_callback, this, + std::placeholders::_1)); + subscriber_guidance = + this->create_subscription( + node_settings.topic_guidance, sub_QoS, + std::bind(&Velocity_node::guidance_callback, this, + std::placeholders::_1)); + // Timer + if (first_start && node_settings.auto_start) { + startup_timer_ = + create_wall_timer(std::chrono::milliseconds(0), [this]() { + startup_timer_->cancel(); + trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + }); + } + first_start = false; + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_activate(const rclcpp_lifecycle::State& state) { + RCLCPP_INFO(get_logger(), "Activating..."); + timer_calculation = + this->create_wall_timer(std::chrono::milliseconds(node_settings.publish_rate), + std::bind(&Velocity_node::publish_thrust, this)); + auto ret = LifecycleNode::on_activate(state); + + return ret; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_deactivate(const rclcpp_lifecycle::State& state) { + RCLCPP_INFO(get_logger(), "Deactivating..."); + auto ret = LifecycleNode::on_deactivate(state); + timer_calculation.reset(); + control_manager_ptr->reset_controllers(); + return ret; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_cleanup(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(get_logger(), "Cleaning up..."); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_shutdown(const rclcpp_lifecycle::State& state) { + RCLCPP_INFO(get_logger(), "Shutting down from state %s", + state.label().c_str()); + if (timer_calculation) + timer_calculation->cancel(); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + should_exit_ = true; + return CallbackReturn::SUCCESS; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(Velocity_node) diff --git a/control/velocity_controller/tests/CMakeLists.txt b/control/velocity_controller/tests/CMakeLists.txt new file mode 100644 index 000000000..adbd6fe82 --- /dev/null +++ b/control/velocity_controller/tests/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.8) +include(GoogleTest) +find_package(ament_cmake_gtest REQUIRED) +#find_package(GTest REQUIRED) +find_package(yaml-cpp REQUIRED) + #find_package(tf2 REQUIRED) + +#Unit test +set(TEST_BINARY_NAME PID_test) + +ament_add_gtest(${TEST_BINARY_NAME} +test_PID.cpp +../src/utilities.cpp +../src/ct_instantiations.cpp +../src/PID_controller.cpp +) + +ament_add_gtest(LQR_test +test_LQR.cpp +../src/utilities.cpp +../src/ct_instantiations.cpp +../src/LQR_setup.cpp +) + +#TODO(hemrimha): need to clean up the include directories/dependencies and such +target_include_directories(${TEST_BINARY_NAME} PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) +target_include_directories(LQR_test PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) #Need to fix +ament_target_dependencies(${TEST_BINARY_NAME} +rclcpp +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) +ament_target_dependencies(LQR_test + rclcpp + vortex_utils + std_msgs + geometry_msgs + vortex_msgs + nav_msgs +) +target_compile_definitions(LQR_test PRIVATE +YAML_PATH="${PROJECT_SOURCE_DIR}/config/parameters.yaml") + +target_link_libraries(${TEST_BINARY_NAME} +Eigen3::Eigen +ct_optcon +ct_core +) +target_link_libraries(LQR_test + Eigen3::Eigen + ct_optcon + ct_core + yaml-cpp +) + + +#System tests + + +target_link_libraries( + ${TEST_BINARY_NAME} + ${LIB_NAME} + #yaml-cpp + #GTest::GTest + #spdlog::spdlog + Eigen3::Eigen + ct_optcon + ct_core +) +add_executable(test_vc_node +test_VC.cpp +../src/utilities.cpp +) + +target_include_directories(test_vc_node PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) + + +target_link_libraries(test_vc_node Eigen3::Eigen ct_optcon ct_core) + +ament_target_dependencies(test_vc_node +rclcpp +rclcpp_lifecycle +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) + +install(TARGETS test_vc_node +DESTINATION lib/${PROJECT_NAME} +) diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp new file mode 100644 index 000000000..39a28f69e --- /dev/null +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -0,0 +1,103 @@ +#include +#include +#include +#include +#include +#include "velocity_controller/utilities.hpp" +//TODO(henrimha): Use standard convention for test, number_type_purpose like thrust allocator +//TODO(henrimha): Need to implement more tests, use claude or other +//TODO(henrimha): fix testing suit + +/* +class LQR_test : public ::testing::Test { + protected: + double delta = 0.0001; + static inline YAML::Node cfg; + LQRController controller; + static void SetUpTestSuite() { + try { + cfg = YAML::LoadFile(YAML_PATH); + } catch (const YAML::Exception& e) { + FAIL() << "Failed to load YAML from '" << YAML_PATH + << "': " << e.what(); + } + } + void SetUp() override { + controller.set_matrices( + cfg["/**"]["ros__parameters"]["LQR_params"]["Q"] + .as>(), + cfg["/**"]["ros__parameters"]["LQR_params"]["R"] + .as>(), + cfg["/**"]["ros__parameters"]["inertia_matrix"] + .as>(), + cfg["/**"]["ros__parameters"]["max_force"].as(), + cfg["/**"]["ros__parameters"]["dampening_matrix_low"] + .as>()); + controller.reset_controller(); + controller.set_interval(0.01); + } + void TearDown() override {} +}; + +TEST_F(LQR_test, wrong_setup) { + // LQRController controller; + std::vector eight = {1, 2, 3, 4, 5, 6, 7, 8}; + std::vector six = {1, 2, 3, 4, 5, 6}; + std::vector thirty_six = { + 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, + 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36}; + std::vector three = {1, 2, 3}; + EXPECT_TRUE( + controller.set_matrices(eight, three, thirty_six, 100, thirty_six)); + EXPECT_FALSE( + controller.set_matrices(eight, eight, thirty_six, 100, thirty_six)); + EXPECT_FALSE( + controller.set_matrices(three, three, thirty_six, 100, thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight, three, eight, 100, thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight, three, thirty_six, 100, eight)); + EXPECT_FALSE(controller.set_matrices(eight, three, thirty_six, 100, eight)); + EXPECT_FALSE( + controller.set_matrices(eight, three, thirty_six, -100, thirty_six)); +} + +TEST_F(LQR_test, Direction) { + Guidance_data value; + State state{}; + value.surge = 0.2; + controller.calculate_thrust(state, value); + Eigen::Vector result = controller.get_thrust(); + EXPECT_GT(result(0), 0); +} +TEST_F(LQR_test, zero_input) { + State states{}; + states.surge = 1; + states.yaw = 0.2; + states.pitch = 0.3; + Guidance_data value; + value.surge = 1.0; + value.yaw = 0.2; + value.pitch = 0.3; + controller.calculate_thrust(states, value); + Eigen::Vector result = controller.get_thrust(); + EXPECT_NEAR(result(0), 0, delta); + EXPECT_NEAR(result(1), 0, delta); + EXPECT_NEAR(result(2), 0, delta); + controller.reset_controller(); + states.surge = 0; + states.pitch = 0; + states.yaw = 0; + value.surge = 0; + value.pitch = 0; + value.yaw = 0; + controller.calculate_thrust(states, value); + result = controller.get_thrust(); + EXPECT_NEAR(result(0), 0, delta); + EXPECT_NEAR(result(1), 0, delta); + EXPECT_NEAR(result(2), 0, delta); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +*/ \ No newline at end of file diff --git a/control/velocity_controller/tests/test_PID.cpp b/control/velocity_controller/tests/test_PID.cpp new file mode 100644 index 000000000..e767d1287 --- /dev/null +++ b/control/velocity_controller/tests/test_PID.cpp @@ -0,0 +1,77 @@ +#include +// #include +// #include +#include +// #include +//TODO(henrimha): Add more tests +//TODO(henrimha): fix testing suit +/* +class PID_test : public ::testing::Test { + protected: + double delta = 0.0001; + PID_controller PID; + void SetUp() override { + PID.set_parameters(0, 0, 0, 1); + PID.reset_controller(); + } + void TearDown() override {} +}; + +TEST_F(PID_test, reset_controller) { + PID.set_parameters(0, 1, 0, 100); + PID.calculate_thrust(100); + PID.set_parameters(0, 1, 0, 1); + PID.calculate_thrust(0); + PID.reset_controller(); + SCOPED_TRACE("Scenario: reset"); + EXPECT_NEAR(PID.get_output(), 0, delta); + PID.calculate_thrust(0); + SCOPED_TRACE("Scenario: reset2"); + EXPECT_NEAR(PID.get_output(), 0, delta); +} +TEST_F(PID_test, P) { + PID.set_parameters(1, 0, 0, 1); + EXPECT_NEAR(PID.calculate_thrust(1), 1, delta); + EXPECT_NEAR(PID.calculate_thrust(2), 2, delta); + PID.set_parameters(1.2, 0, 0, 1); + EXPECT_NEAR(PID.calculate_thrust(-2.2), -2.64, delta); + EXPECT_NEAR(PID.calculate_thrust(-1.5), -1.8, delta); +} +TEST_F(PID_test, I) { + PID.set_parameters(0, 1.1, 0, 1); + PID.calculate_thrust(1); + EXPECT_NEAR(PID.get_output(), 0, delta); + PID.calculate_thrust(1); + EXPECT_NEAR(PID.get_output(), 1.1, delta); + PID.calculate_thrust(-1); + EXPECT_NEAR(PID.get_output(), 2.2, delta); + EXPECT_NEAR(PID.calculate_thrust(0), 1.1, delta); + PID.set_output_limits(-101, 101); + PID.reset_controller(); + PID.set_parameters(1, 1, 0, 10); + EXPECT_NEAR(PID.calculate_thrust(1000), 101, delta); + PID.set_parameters(1, 1, 0, 1); + EXPECT_NEAR(PID.calculate_thrust(0), 0, delta); + PID.reset_controller(); + EXPECT_NEAR(PID.calculate_thrust(-10000), -101, delta); + PID.calculate_thrust(-50); + EXPECT_NEAR(PID.calculate_thrust(1), -49, delta); +} +TEST_F(PID_test, D) {} +TEST_F(PID_test, illegal_inputs) { + double temp = PID.get_output(); + PID.set_parameters(1, 1, 0, 0); + + EXPECT_FALSE(PID.calculate_thrust(1)); + EXPECT_NEAR(PID.get_output(), temp, delta); + EXPECT_FALSE(PID.set_output_limits(1, -1)); + PID.set_parameters(1, 1, 0, -1); + + EXPECT_FALSE(PID.calculate_thrust(1)); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +*/ \ No newline at end of file diff --git a/control/velocity_controller/tests/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp new file mode 100644 index 000000000..9791bb299 --- /dev/null +++ b/control/velocity_controller/tests/test_VC.cpp @@ -0,0 +1,72 @@ +#include "tests/test_VC.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "velocity_controller/utilities.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" + +// Denne noden er kun for å teste velocity_controller noden +test_velocity_controller::test_velocity_controller() : Node("test_VC_node") { + this->declare_parameter("topics.guidance.los"); + this->declare_parameter("topics.odom"); + this->topic_guidance = + this->get_parameter("topics.guidance.los").as_string(); + this->topic_odometry = this->get_parameter("topics.odom").as_string(); + rclcpp::QoS pub_QoS(10); + pub_QoS.keep_last(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + publisher_guidance = this->create_publisher( + topic_guidance, pub_QoS); + publisher_state = this->create_publisher( + "/state", pub_QoS); + + rclcpp::QoS sub_QoS(10); + sub_QoS.keep_last(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + subscription_state = this->create_subscription( + topic_odometry, sub_QoS, + std::bind(&test_velocity_controller::odometry_callback, this, std::placeholders::_1)); + timer_ = this->create_wall_timer(std::chrono::milliseconds(200), + std::bind(&test_velocity_controller::send_reference, this)); + clock_ = this->get_clock(); + RCLCPP_INFO(this->get_logger(), "Test_velocity_controller node has been started"); +} + +void test_velocity_controller::send_reference() { + totaltime += 0.2; + //reference_msg.yaw = std::numbers::pi * sin(totaltime * std::numbers::pi / 9); + // reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); + reference_msg.yaw = 0.4; + reference_msg.surge = 0.20; + reference_msg.pitch = -0.4; // reference_msg.yaw=0.0; //Surge, pitch, yaw + publisher_guidance->publish(reference_msg); +} + +void test_velocity_controller::odometry_callback( + const nav_msgs::msg::Odometry::SharedPtr msg_ptr) { + vortex_msgs::msg::LOSGuidance msg; + angle temp = quaternion_to_euler_angle( + msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, + msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); + msg.set__pitch(temp.thetat); + msg.set__yaw(temp.psit); + msg.set__surge(msg_ptr->twist.twist.linear.x); + publisher_state->publish(msg); +} +int main(int argc, char const* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py deleted file mode 100644 index e69de29bb..000000000