diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt new file mode 100644 index 00000000..a6eaa4f1 --- /dev/null +++ b/control/velocity_controller/CMakeLists.txt @@ -0,0 +1,129 @@ +cmake_minimum_required(VERSION 3.8) +project(velocity_controller) + +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 dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(CasADi REQUIRED) +find_package(LAPACK REQUIRED) +find_package(BLAS REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +#find_package(stonefish_ros2 REQUIRED) +#find_package(auv_setup REQUIRED) +#find_package(stonefish_sim REQUIRED) +#find_package(thrust_allocator_auv REQUIRED) + +#target_include_directories(velocity_controller_node +# PUBLIC +# $ +# $ +#) +include_directories( + ${EIGEN3_INCLUDE_DIR} + include +) + +add_executable(velocity_controller_node + src/velocity_controller.cpp + src/PID_setup.cpp + src/LQR_setup.cpp + src/utilities.cpp +) + +add_executable(test_VC_node + src/test_VC.cpp + src/PID_setup.cpp + src/LQR_setup.cpp + src/utilities.cpp +) +add_executable(test_LQR_node + src/LQR_test.cpp + src/LQR_setup.cpp + src/utilities.cpp +) +ament_target_dependencies(velocity_controller_node + rclcpp + std_msgs + vortex_msgs + geometry_msgs + #Eigen3 + CasADi + nav_msgs + vortex_utils + +) + +ament_target_dependencies(test_VC_node + rclcpp + std_msgs + vortex_msgs + geometry_msgs + #Eigen3 + nav_msgs + vortex_utils +) + +ament_target_dependencies(test_LQR_node + rclcpp + #Eigen3 + vortex_msgs + geometry_msgs + std_msgs + nav_msgs + vortex_utils +) + +link_directories(/usr/lib/gcc/x86_64-linux-gnu/11) +set(SLICOT_LIB /usr/lib/x86_64-linux-gnu/libslicot.so) +set(GFORTRAN_LIB /usr/lib/gcc/x86_64-linux-gnu/11/libgfortran.so) + +target_link_libraries(velocity_controller_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} Eigen3::Eigen) +target_link_libraries(test_VC_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} Eigen3::Eigen) +target_link_libraries(test_LQR_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB} Eigen3::Eigen) + + +install(TARGETS + velocity_controller_node + test_VC_node + test_LQR_node + DESTINATION lib/${PROJECT_NAME} +) + + + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + #add_subdirectory(test) +endif() + +ament_package() diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml new file mode 100644 index 00000000..8aea0ed6 --- /dev/null +++ b/control/velocity_controller/config/parameters.yaml @@ -0,0 +1,41 @@ +/**: + ros__parameters: + + topics: + odom_topic: /orca/odom #Odometry + twist_topic: /dvl/twist #Twist + pose_topic: /dvl/pose #Pose + guidance_topic: /guidance/los #Guidance + thrust_topic: /orca/wrench_input #Thrust + softwareoperation_topic: /softwareOperationMode #Software Operation + killswitch_topic: /softwareKillSwitch #Kill Switch + + LQR_params: + q_surge: 75.0 + q_pitch: 175.0 + q_yaw: 175.0 + + r_surge: 0.3 + r_pitch: 0.4 + r_yaw: 0.4 + + i_surge: 0.3 + i_pitch: 0.4 + i_yaw: 0.3 + + i_weight: 0.5 + + dt: 0.1 + + inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.0, 30.0, 0.0, 0.0, -0.6, 0.3, 0.0, 0.0, 30.0, 0.6, 0.3, 0.0, 0.0, 0.0, 0.6, 0.68, 0.0, 0.0, 0.0, -0.6, 0.3, 0.0, 3.32, 0.0, 0.6, 0.3, 0.0, 0.0, 0.0, 3.34] + + + dampening_matrix_low: [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] + 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] + + calculation_rate: 200 #ms integer + publish_rate: 100 #ms + #Clamp parameter + max_force: 1000.0 #should maybe be 99.5 + controller_type: 1 #1 PID 2 LQR + diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp new file mode 100644 index 00000000..cfb4a439 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include +#include "PID_setup.hpp" +#include +#include "velocity_controller/utilities.hpp" + + + +/*class Guidance_values{ + //Dataclass to store guidance values for LQR controller + public: + double surge=0.0; double pitch=0.0; double yaw=0.0; + double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; +}; +*/ +class LQRparameters{ + //Dataclass to store LQR parameters + public: + double q_surge=0.0; double q_pitch=0.0; double q_yaw=0.0; + double r_surge=0.0; double r_pitch=0.0; double r_yaw=0.0; + double i_surge=0.0; double i_pitch=0.0; double i_yaw=0.0; + double i_weight=0.0; double max_force=0.0; +}; + +/*class angle{ + public: + double phit=0.0; + double thetat=0.0; + double psit=0.0; + +};*/ + +struct LQRsolveResult{ + Eigen::Matrix K; + Eigen::Matrix P; + int INFO=0; + LQRsolveResult(Eigen::Matrix K,Eigen::Matrix P, int INFO=0):K(K),P(P),INFO(INFO) {}; +}; +class LQRController{ + + public: + LQRController(LQRparameters params={0,0,0,0,0,0,0,0,0,0,0},Eigen::Matrix3d inertia_matrix=Eigen::Matrix3d::Identity()); + + + void set_params(LQRparameters params); + Eigen::Matrix3d calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); + void set_matrices(Eigen::Matrix3d inertia_matrix); + void update_augmented_matrices(Eigen::Matrix3d coriolis_matrix); + + //angle quaternion_to_euler_angle(double w, double x, double y, double z); + //double ssa(double angle); + + std::tuple saturate (double value, bool windup, double limit); + double anti_windup(double ki, double error, double integral_sum, bool windup); + Eigen::Vector saturate_input(Eigen::Vector u); + + Eigen::Vector update_error(Guidance_data guidance_values, State states); + Eigen::Vector calculate_lqr_u(State states, Guidance_data guidance_values); + + LQRsolveResult solve_k_p(const Eigen::Matrix &A,const Eigen::Matrix &B,const Eigen::Matrix &Q, const Eigen::Matrix &R); + + //Resets controller + void reset_controller(); + + // VariablesEigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix) + const double pi=3.14159265358979323846; + double integral_error_surge; double integral_error_pitch; double integral_error_yaw; + bool surge_windup; bool pitch_windup; bool yaw_windup; + double q_surge; double q_pitch; double q_yaw; + double r_surge; double r_pitch; double r_yaw; + double i_surge; double i_pitch; double i_yaw; + double i_weight; double max_force; + + Eigen::Matrix3d inertia_matrix_inv; + Eigen::Matrix state_weight_matrix; + Eigen::Matrix3d input_weight_matrix; + Eigen::Matrix augmented_system_matrix; + Eigen::Matrix augmented_input_matrix; + + +}; + +//Extra operations +Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix); +std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat); +std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat); +Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix); diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp new file mode 100644 index 00000000..91303d5d --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include +#include "utilities.hpp" + +class PID_controller { + public: + PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output); + PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {}; + void calculate_thrust(double reference, double current_position, double dt); + void reset_controller(); + double output(); + void set_output_limits(double min_output, double max_output); + private: + double k_p; + double k_i; + double k_d; + double integral; + double previous_error; + double previous_position; + double last_output; + double max_output; + double min_output; +}; + diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp new file mode 100644 index 00000000..6d64e671 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/velocity_controller.hpp" +#include +#include +#include "vortex_msgs/msg/los_guidance.hpp" + +class test_VC : public rclcpp::Node{ + public: + test_VC(); + //Callback functions + //void read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg); + void send_guidance(); + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); + //void send_state(); + + //Variables + + //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 + //std::vector thrust_vector; + vortex_msgs::msg::LOSGuidance reference_msg; + + //Topics + //std::string topic_odom; + //std::string topic_thrust; + std::string topic_guidance; + std::string topic_state="/state"; + std::string topic_odometry; + + + //MSGS + //nav_msgs::msg::Odometry odom_msg; +}; + +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); \ 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 00000000..c6627fa1 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -0,0 +1,32 @@ +#pragma once +#include +#include +#include "std_msgs/msg/float64_multi_array.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" + + +class angle{ + public: + 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{ + //Dataclass to store state values for LQR controller + public: + double surge=0.0, pitch=0.0, yaw=0.0, pitch_rate=0.0, yaw_rate=0.0, heave_vel=0, sway_vel=0; + double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; +}; + +class Guidance_data:public State{ + public: + //double surge; double pitch; double yaw; + Guidance_data(vortex_msgs::msg::LOSGuidance msg):State{msg.surge,msg.pitch,msg.yaw}{}; + Guidance_data(double surge, double pitch, double yaw):State{surge, pitch, yaw} {}; + Guidance_data():State{0, 0, 0} {}; + + Guidance_data operator-(const Guidance_data& other) const; + Guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg); +}; diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp new file mode 100644 index 00000000..4b376225 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "velocity_controller/PID_setup.hpp" +#include "LQR_setup.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" + + + +class Velocity_node : public rclcpp::Node{ + public: + Velocity_node(); + //Different initializatin functions + void get_new_parameters(); + + //Timer functions + void publish_thrust(); + void calc_thrust(); + + //Callback functions + void guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr); + void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr); + //void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr); + //void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::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 timer_publish; + + //Subscriber instance + //rclcpp::Subscription::SharedPtr subscriber_twist; + //rclcpp::Subscription::SharedPtr subscriber_pose; + rclcpp::Subscription::SharedPtr subscriber_Odometry; + rclcpp::Subscription::SharedPtr subscriber_guidance; + rclcpp::Subscription::SharedPtr subscriber_killswitch; + + //Variables for topics + + std::string topic_thrust; + std::string topic_guidance; + std::string topic_killswitch; + //std::string topic_twist; + //std::string topic_pose; + std::string topic_odometry; + + //Variables for timers + int calculation_rate; + int publish_rate; + double max_force; + + //Stored wrenches values + vortex_msgs::msg::LOSGuidance reference_in; + Guidance_data guidance_values; + Guidance_data current_state; + geometry_msgs::msg::WrenchStamped thrust_out; + + + int controller_type; //1 PID, 2 LQR + + //PID controllers + PID_controller PID_surge; + PID_controller PID_yaw; + PID_controller PID_pitch; + + //LQR Controller + LQRController lqr_controller; + LQRparameters lqr_parameters; + std::vector inertia_matrix; + + + //Test + rclcpp::Publisher::SharedPtr publisher_reference; + + +}; + + + + diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py new file mode 100644 index 00000000..454a50d9 --- /dev/null +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -0,0 +1,65 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + pkg_share = get_package_share_directory('velocity_controller') + config_path = os.path.join(pkg_share, 'config', 'parameters.yaml') + + stonefish_dir = get_package_share_directory('stonefish_sim') + + stonefish_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') + ), + launch_arguments={'rendering_quality': 'low','rendering':'true'}.items(), + ) + orca_sim = TimerAction( + period=12.0, + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'orca_sim.launch.py') + ) + ) + ] + ) + + + node_name_arg = DeclareLaunchArgument( + 'node_name', default_value='velocity_controller_node', + description='Name of the velocity controller node' + ) + + node_name_arg2 = DeclareLaunchArgument( + 'node_name_1', default_value='test_VC_node', + description='Name of the test VC node' + ) + + velocity_controller_name = LaunchConfiguration('node_name') + test_VC_name = LaunchConfiguration('node_name_1') + + return LaunchDescription([ + stonefish_sim, + orca_sim, + node_name_arg, + node_name_arg2, + Node(package='velocity_controller', + executable='velocity_controller_node', + name=velocity_controller_name, + output='screen', + parameters=[config_path] + #arguments=['--ros-args','--log-level','debug'] + ), + Node(package='velocity_controller', + executable='test_VC_node', + name=test_VC_name, + output='screen', + parameters=[config_path]) + ]) \ No newline at end of file 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 00000000..148a697a --- /dev/null +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +#from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +#from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + pkg_share = get_package_share_directory('velocity_controller') + config_path = os.path.join(pkg_share, 'config', 'parameters.yaml') + + node_name_arg = DeclareLaunchArgument( + 'node_name', default_value='velocity_controller_node', + description='Name of the velocity controller node' + ) + + velocity_controller_name = LaunchConfiguration('node_name') + + + return LaunchDescription([ + node_name_arg, + Node(package='velocity_controller', + executable='velocity_controller_node', + name=velocity_controller_name, + output='screen', + parameters=[config_path]) + ]) \ No newline at end of file diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml new file mode 100644 index 00000000..3f116b99 --- /dev/null +++ b/control/velocity_controller/package.xml @@ -0,0 +1,24 @@ + + + + velocity_controller + 0.0.0 + TODO: Package description + henrik + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + vortex_msgs + geometry_msgs + nav_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp new file mode 100644 index 00000000..8bbd0dbc --- /dev/null +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -0,0 +1,274 @@ +#include "velocity_controller/LQR_setup.hpp" +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +//#include +//#include +//#include +//#include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/utilities.hpp" +#include +#include +#include "vortex/utils/math.hpp" + + +Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); +LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix){ + set_params(params); + set_matrices(inertia_matrix); +}; + + +/*angle LQRController::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}; +};*/ + +/*double LQRController::ssa(double angle){ + return std::fmod(angle+pi, 2*pi)-pi; +};*/ + +//Can be optimized +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 ki, double error, double integral_sum, bool windup){ + if (!windup){ + integral_sum += error * ki; + } + return integral_sum; +} + +Eigen::Matrix3d LQRController::calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel){ + //Inertia matrix values?? + Eigen::Matrix3d result; + result<<0.2,-30*sway_vel*0.01,-30*heave_vel*0.01, + 30 * sway_vel*0.01,0,1.629 * pitchrate, + 30 * heave_vel*0.01,1.769 * yaw_rate,0; + return result; +} + + +void LQRController::set_params(LQRparameters params){ + //set LQR parameters + integral_error_surge= 0.0; integral_error_pitch= 0.0; integral_error_yaw= 0.0; + surge_windup= false; pitch_windup= false; yaw_windup= false; + q_surge= params.q_surge; q_pitch= params.q_pitch; q_yaw= params.q_yaw; + r_surge= params.r_surge; r_pitch= params.r_pitch; r_yaw= params.r_yaw; + i_surge= params.i_surge; i_pitch= params.i_pitch; i_yaw= params.i_yaw; + i_weight= params.i_weight; max_force= params.max_force; + return; + +} +void LQRController::set_matrices(Eigen::Matrix3d inertia_matrix){ + inertia_matrix_inv = inertia_matrix.inverse(); + state_weight_matrix.diagonal()< LQRController::update_error(Guidance_data guidance_values, State states){ + double surge_error = guidance_values.surge - states.surge; + double pitch_error = vortex::utils::math::ssa(guidance_values.pitch - states.pitch); + double yaw_error = vortex::utils::math::ssa(guidance_values.yaw - states.yaw); + + integral_error_surge = anti_windup(i_surge, surge_error, integral_error_surge, surge_windup); + integral_error_pitch = anti_windup(i_pitch, pitch_error, integral_error_pitch, pitch_windup); + integral_error_yaw = anti_windup(i_yaw, yaw_error, integral_error_yaw, yaw_windup); + + Eigen::Vector state_error= {-surge_error, -pitch_error, -yaw_error, 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, max_force); + std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, max_force); + std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); + return {force_x, torque_y, torque_z}; +} +Eigen::Vector LQRController::calculate_lqr_u(State state, Guidance_data guidance_values){ + update_augmented_matrices(calculate_coriolis_matrix(state.pitch_rate,state.yaw_rate,state.sway_vel,state.heave_vel)); + LQRsolveResult result = solve_k_p(augmented_system_matrix,augmented_input_matrix,state_weight_matrix,input_weight_matrix); + if(result.INFO!=0){ + return {9999,9999,9999}; //Need to fix + } + Eigen::Matrix state_error = update_error(guidance_values, state); + Eigen::Vector u= saturate_input(- (result.K*state_error)); + return u; +} +void LQRController::reset_controller(){ + integral_error_surge=0.0; + integral_error_pitch=0.0; + integral_error_yaw=0.0; + + surge_windup=false; + pitch_windup=false; + yaw_windup=false; + return; +} + + +extern "C" { + // Fortran subroutine for solving symplectic Schur decomposition(double precision version) +void sb02mt_( + const char* JOBG, const char* JOBL, const char* FACT, const char* UPLO, + const int* N, const int* M, double* A, const int* LDA, double* B, const int* LDB, + double* Q, const int* LDQ, double* R, const int* LDR, double* L, const int* LDL, + int* IPIV, const int* OUFACT, double* G, const int* LDG, + int* IWORK, double* DWORK, const int* LDWORK, int* INFO +); +void sb02md_( const char* DICO, const char* HINV, const char* UPLO, const char* SCAL, const char* SORT, const int* N, double* A, const int* LDA, double* G, + const int* LDG, double* Q, const int* LDQ, const double* RCOND, double* WR, double* WI, double* S, const int* LDS, double* U, const int* LDU, + int* IWORK, double* DWORK, const int* LDWORK, int* BWORK, const int* INFO + ); +} + + +LQRsolveResult LQRController::solve_k_p(const Eigen::Matrix &A,const Eigen::Matrix &B, const Eigen::Matrix &Q, const Eigen::Matrix &R){ + Eigen::Matrix A_copy=A, Q_copy=Q; + Eigen::Matrix B_copy=B; Eigen::Matrix R_copy=R; + + //First calculate G with sb02mt_ + //calculate G, L is zero, unfactored R, Upper triangle i think + char JOBG='G',JOBL='Z',FACT='N',UPLO='U'; + //Order of matrices A, Q, G and X(P), Order of matrix R and nuber of columns in B and L(is zero) + const int N=6, M=3; + //Dimensions of matrices + int LDA=N, LDB=N, LDQ=N,LDR=M,LDL=N,LDG=N; + std::vector IWORK(8*N),IPIV(N); + //Upper bounds Output but initialized JIC output placeholder + int LDWORK=20*N*N,OUFACT=0,INFO=0; + std::vector DWORK(LDWORK); + Eigen::Matrix L=Eigen::Matrix::Zero(), G=L; + { + double detA = A_copy.determinant(); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling sb02md_: det(A)=%.6g, A(0,0)=%.6g, G(0,0)=%.6g", detA, A_copy(0,0), G(0,0)); + Eigen::EigenSolver> es(A_copy); + for (int i=0;i<6;++i){ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "eigA[%d]=% .6g%+.6gi", i, es.eigenvalues()[i].real(), es.eigenvalues()[i].imag()); + } + } + sb02mt_(&JOBG,&JOBL,&FACT,&UPLO,&N,&M,A_copy.data(),&LDA,B_copy.data(),&LDB,Q_copy.data(),&LDQ,R_copy.data(),&LDR,L.data(),&LDL,IPIV.data(),&OUFACT,G.data(),&LDG,IWORK.data(),DWORK.data(),&LDWORK,&INFO); + Eigen::Matrix K; + if (INFO!=0){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "sb02mt_ returned INFO=%d", INFO); + // Consider throwing or returning a default result. We'll return zeroed K and G for now. + Eigen::Matrix K_zero = Eigen::Matrix::Zero(); + return LQRsolveResult(K_zero, G,INFO); + + } + char DICO='D',HINV='D',SCAL='N',SORT='U'; + std::vector WR(2*N,0),WI(2*N,0),RCOND(2*N,0); + int BWORK[8*N]; + Eigen::Matrix S=Eigen::Matrix::Zero(); + Eigen::MatrixU=Eigen::Matrix::Zero(); + int LDS=2*N,LDU=2*N,INFO1=0; + //A_copy=A;Q_copy=Q; R_copy=R; + { + double detA = A_copy.determinant(); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling sb02md_: det(A)=%.6g, A(0,0)=%.6g, G(0,0)=%.6g", detA, A_copy(0,0), G(0,0)); + Eigen::EigenSolver> es(A_copy); + for (int i=0;i<6;++i){ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "eigA[%d]=% .6g%+.6gi", i, es.eigenvalues()[i].real(), es.eigenvalues()[i].imag()); + } + } + sb02md_(&DICO,&HINV,&UPLO,&SCAL,&SORT,&N,A_copy.data(),&LDA,G.data(),&LDG,Q_copy.data(),&LDQ,RCOND.data(),WR.data(),WI.data(),S.data(),&LDS,U.data(),&LDU,IWORK.data(),DWORK.data(),&LDWORK,BWORK,&INFO1); + if (INFO1!=0){ + //Some Error handling here. Also check that BRB in invertible + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "sb02md_ returned INFO=%d", INFO1); + // Consider throwing or returning a default result. We'll return zeroed K and G for now. + Eigen::Matrix K_zero = Eigen::Matrix::Zero(); + return LQRsolveResult(K_zero, G,INFO1); + } + Eigen::MatrixU11=U.topRows(6); + Eigen::MatrixU21=U.bottomRows(6); + Eigen::MatrixXd X=U21*U11.inverse(); + K=R.inverse()*B.transpose()*X; + + return LQRsolveResult(K,G,INFO1); + +} + + + + +//Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d +Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ + Eigen::Matrix3d mat; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + mat(i, j) = other_matrix[i * 3 + j]; + return mat; +} +std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat){ + std::vector other_matrix(9); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + other_matrix[i * 3 + j] = mat(i, j); + return other_matrix; +} + +std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat){ + std::vector> other_matrix(3, std::vector(3)); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + other_matrix[i][j] = mat(i, j); + return other_matrix; +} + +Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix){ + Eigen::Matrix3d mat; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + mat(i, j) = other_matrix[i][j]; + return mat; +} \ No newline at end of file diff --git a/control/velocity_controller/src/LQR_test.cpp b/control/velocity_controller/src/LQR_test.cpp new file mode 100644 index 00000000..4358f61e --- /dev/null +++ b/control/velocity_controller/src/LQR_test.cpp @@ -0,0 +1,46 @@ +#include "rclcpp/rclcpp.hpp" +#include +#include "velocity_controller/LQR_setup.hpp" + +class test_LQR_node : public rclcpp::Node{ + public: + double q_surge =75.0, q_pitch= 175.0, q_yaw= 175.0, r_surge= 0.3, r_pitch= 0.4, r_yaw= 0.4, i_surge= 0.3, + i_pitch= 0.4, i_yaw= 0.3, i_weight= 0.5, dt= 0.1; + LQRparameters param={q_surge, q_pitch, q_yaw, r_surge, r_pitch, r_yaw, i_surge, i_pitch, i_yaw, i_weight}; + LQRController controller; + test_LQR_node():Node("test_LQR_node"), controller(){ + RCLCPP_INFO(this->get_logger(),"LQR test node started"); + Eigen::Matrix Q=(Eigen::Matrix()<<75,0,0,0,0,0,0,175,0,0,0,0,0,0,175,0,0,0,0,0,0,0.3,0,0,0,0,0,0,0.4,0,0,0,0,0,0,0.3).finished(); + Eigen::Matrix R=(Eigen::Matrix3d()<<0.3,0,0,0,0.4,0,0,0,0.4).finished(); + Eigen::Matrix A=(Eigen::Matrix()<<5,7,23,0,0,0,0,45,21,4,3,4,0,23,1,7,6,5,5,7,6,3,5,7,2,2,3,2,1,0,0,0,8,7,6,5).finished(); + Eigen::Matrix B=(Eigen::Matrix()<<2,0,0,3,0,2,0,3,0,1,2,0,3,4,0,3,5,0).finished(); + /*Eigen::Matrix3d inertia_matrix=(Eigen::Matrix3d()<<30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729).finished(); + Eigen::Matrix3d inertia_matrix_inv=inertia_matrix.inverse(); + Eigen::Matrix3d coriolis_matrix=(Eigen::Matrix3d()<<0.2,-30*2*0.01,-30*2*0.0,30 * 2*0.01,0,1.629 * 2,30 * 2*0.01,1.769 * 2,0).finished(); + Eigen::Matrix3d system_matrix=inertia_matrix.inverse()*coriolis_matrix; + Eigen::Matrix augmented_system_matrix =(Eigen::Matrix()< augmented_input_matrix=(Eigen::Matrix()<< inertia_matrix_inv(0,0),inertia_matrix_inv(0,1),inertia_matrix_inv(0,2),0,0,0, + inertia_matrix_inv(1,0),inertia_matrix_inv(1,1),inertia_matrix_inv(1,2),0,0,0, + inertia_matrix_inv(2,0),inertia_matrix_inv(2,1),inertia_matrix_inv(2,2),0,0,0).finished();*/ + LQRsolveResult result=controller.solve_k_p(A,B,Q,R); + RCLCPP_INFO(this->get_logger(),"LQR Gain K matrix:"); + RCLCPP_INFO(this->get_logger(),"\n%f %f %f %f %f %f\n%f %f %f %f %f %f\n%f %f %f %f %f %f", + result.K(0,0),result.K(0,1),result.K(0,2),result.K(0,3),result.K(0,4),result.K(0,5), + result.K(1,0),result.K(1,1),result.K(1,2),result.K(1,3),result.K(1,4),result.K(1,5), + result.K(2,0),result.K(2,1),result.K(2,2),result.K(2,3),result.K(2,4),result.K(2,5)); + + } +}; + +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/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp new file mode 100644 index 00000000..451d2693 --- /dev/null +++ b/control/velocity_controller/src/PID_setup.cpp @@ -0,0 +1,53 @@ +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/LQR_setup.hpp" +#include "velocity_controller/utilities.hpp" + +PID_controller::PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output):k_p(k_p), k_i(k_i), k_d(k_d), max_output(max_output), min_output(min_output) { + integral = 0.0; + previous_error = 0.0; + previous_position = 0.0; +}; +void PID_controller::calculate_thrust(double reference, double current_position, double dt){ + //Error calculation + double error = reference - current_position; + //P calculation + last_output=k_p*error; + //D calculation + last_output += k_d * (error - previous_error) / dt; + previous_position = current_position; + //I calculation with anti-windup + integral += error * dt; + if (integral > max_output) { + integral -= error * dt; //anti windup + } else if (integral < min_output) { + integral -= error * dt; //anti windup + } + last_output += k_i * integral; + previous_error = error; + //Output calculation with saturation + + if (last_output > max_output){ + last_output = max_output; + } + else if (last_output < min_output){ + last_output = min_output; + } + + return; +}; +void PID_controller::reset_controller(){ + integral = 0.0; + previous_error = 0.0; + previous_position = 0.0; +} + +double PID_controller::output(){ + return last_output; +}; + +void PID_controller::set_output_limits(double min_output, double max_output){ + this->min_output = min_output; + this->max_output = max_output; + return; +}; + diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp new file mode 100644 index 00000000..ef2af22a --- /dev/null +++ b/control/velocity_controller/src/test_VC.cpp @@ -0,0 +1,77 @@ +#include +#include +#include +#include +#include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/test_VC.hpp" +#include +#include +#include "vortex_msgs/msg/los_guidance.hpp" +//#include "velocity_controller/velocity_controller.hpp" +//#include "LQR_setup.hpp" +//Denne noden er kun for å teste velocity_controller noden + +test_VC::test_VC() : Node("test_VC_node") +{ + this->declare_parameter("topics.guidance_topic"); + this->declare_parameter("topics.odom_topic"); + this->topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); + this->topic_odometry=this->get_parameter("topics.odom_topic").as_string(); + publisher_guidance = this->create_publisher(topic_guidance, 10); + publisher_state = this->create_publisher(topic_state,10); + subscription_state = this->create_subscription( + topic_odometry,10, + std::bind(&test_VC::odometry_callback,this,std::placeholders::_1)); + rclcpp::QoS orca_QoS(2); + orca_QoS.keep_last(2).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(200), + std::bind(&test_VC::send_guidance, this)); + clock_ = this->get_clock(); + RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); + reference_msg.surge=0.2;reference_msg.pitch=0.3;reference_msg.yaw=0.3; //Surge, pitch, yaw + +} + +void test_VC::send_guidance() +{ + publisher_guidance->publish(reference_msg); +} + +void test_VC::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; +} + +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; +} + \ No newline at end of file diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp new file mode 100644 index 00000000..3ad23687 --- /dev/null +++ b/control/velocity_controller/src/utilities.cpp @@ -0,0 +1,21 @@ +#include "velocity_controller/utilities.hpp" +#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}; +}; \ No newline at end of file diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp new file mode 100644 index 00000000..1875bcd8 --- /dev/null +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -0,0 +1,263 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "velocity_controller/velocity_controller.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include +#include +#include "std_msgs/msg/bool.hpp" +#include "velocity_controller/PID_setup.hpp" +#include +#include +#include "vortex_msgs/msg/los_guidance.hpp" +#include "vortex/utils/math.hpp" + + +//#include "vortex-msgs/msg" kan legge til nye meldinger nå + +//Lager en klasse velocity node + +//Konstruktør +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(20,1,2), PID_yaw(4,0.5,1), PID_pitch(4,0.5,1), lqr_controller() +{ + //Dytter info til log + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); + + //Parameter from config. + get_new_parameters(); + + + // Publishers + rclcpp::QoS orca_QoS(2); + orca_QoS.keep_last(2).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + + publisher_thrust = create_publisher(topic_thrust, orca_QoS); + publisher_reference = create_publisher("/reference",2); + + //Subscribers + subscriber_Odometry = this->create_subscription( + topic_odometry,10, + std::bind(&Velocity_node::odometry_callback,this,std::placeholders::_1)); + subscriber_guidance = this->create_subscription( + topic_guidance,10, + std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); + subscriber_killswitch = this->create_subscription( + topic_killswitch,10, + std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); + + + + //Timer + + timer_calculation = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::calc_thrust, this)); + timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::publish_thrust, this)); + //Controllers + PID_surge.set_output_limits(-max_force, max_force); + PID_pitch.set_output_limits(-max_force, max_force); + PID_yaw.set_output_limits(-max_force, max_force); + lqr_controller.set_params(lqr_parameters); + lqr_controller.set_matrices(vector_to_matrix3d(inertia_matrix)); + + +} + + + +//Publish/timer functions +void Velocity_node::publish_thrust() +{ + /*thrust_out.wrench.force.x=100; + thrust_out.wrench.force.y=0; + thrust_out.wrench.force.z=0; + thrust_out.wrench.torque.x=0; + thrust_out.wrench.torque.y=0; + thrust_out.wrench.torque.z=0;*/ + publisher_thrust->publish(thrust_out); + //RCLCPP_DEBUG(this->get_logger(),"Publishing thrust: %.3f",thrust_out.wrench.force.x); +} + +//** må forbedre integrasjon og derivasjons beregningene +void Velocity_node::calc_thrust() +{ + switch (controller_type) + { + case 1:{ + //RCLCPP_INFO(this->get_logger(),"PID controller"); + PID_surge.calculate_thrust(guidance_values.surge, current_state.surge,calculation_rate/1000.0); + PID_pitch.calculate_thrust(guidance_values.pitch, current_state.pitch,calculation_rate/1000.0); + PID_yaw.calculate_thrust(guidance_values.yaw, current_state.yaw,calculation_rate/1000.0); + thrust_out.wrench.force.x = PID_surge.output(); + thrust_out.wrench.torque.y = PID_pitch.output(); + thrust_out.wrench.torque.z = PID_yaw.output(); + + break; + } + case 2:{ + //RCLCPP_INFO(this->get_logger(),"LQR controller"); + Eigen::Vector3d u=lqr_controller.calculate_lqr_u(current_state,guidance_values); + if (u==Eigen::Vector3d{9999,9999,9999}){ + controller_type=1; + RCLCPP_ERROR(this->get_logger(),"Switching to PID"); + } + else{ + thrust_out.wrench.force.x=u[0]; + thrust_out.wrench.torque.y=u[1]; + thrust_out.wrench.torque.z=u[2]; + } + break; + } + default:{ + break; + } + } + std_msgs::msg::Float64MultiArray msg; + msg.data={guidance_values.surge,guidance_values.pitch,guidance_values.yaw}; + publisher_reference->publish(msg); + return; +} + + + +//Callback functions +void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ + guidance_values = *msg_ptr; + //RCLCPP_DEBUG(this->get_logger(), "Guidance received: surge=%.3f pitch=%.3f yaw=%.3f", + // guidance_values.surge, guidance_values.pitch, guidance_values.yaw); + return; +} + +void Velocity_node::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ + //RCLCPP_INFO(this->get_logger(),"Recieved odometry"); + 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); + current_state.pitch = temp.thetat; + current_state.yaw = temp.psit; + current_state.surge = msg_ptr->twist.twist.linear.x; + current_state.pitch_rate=msg_ptr->twist.twist.angular.y; + current_state.yaw_rate=msg_ptr->twist.twist.angular.z; + //Need to update angular speed NB!!. + return; +} + +//**Needs to update to shutdown the node +void Velocity_node::killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg_ptr){ + RCLCPP_INFO(this->get_logger(), "Received killswitch: '%d'", msg_ptr->data); + if(msg_ptr->data == true){ + guidance_values = Guidance_data(); + current_state = Guidance_data(); + RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current state set to zero"); + } + return; +} + + +void Velocity_node::get_new_parameters(){ + this->declare_parameter("topics.thrust_topic"); + this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); + this->declare_parameter("topics.guidance_topic"); + this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); + //this->declare_parameter("topics.twist_topic"); + //this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); + //this->declare_parameter("topics.pose_topic"); + //this->topic_pose = this->get_parameter("topics.pose_topic").as_string(); + this->declare_parameter("topics.odom_topic"); + this->topic_odometry = this->get_parameter("topics.odom_topic").as_string(); + this->declare_parameter("topics.killswitch_topic"); + this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); + this->declare_parameter("max_force"); + this->max_force = this->get_parameter("max_force").as_double(); + this->declare_parameter("calculation_rate"); + this->calculation_rate = this->get_parameter("calculation_rate").as_int(); + this->declare_parameter("publish_rate"); + this->publish_rate = this->get_parameter("publish_rate").as_int(); + this->declare_parameter("controller_type"); + this->controller_type=this->get_parameter("controller_type").as_int(); + + + //LQR Parameters + this->declare_parameter("LQR_params.q_surge"); + this->declare_parameter("LQR_params.q_pitch"); + this->declare_parameter("LQR_params.q_yaw"); + this->declare_parameter("LQR_params.r_surge"); + this->declare_parameter("LQR_params.r_pitch"); + this->declare_parameter("LQR_params.r_yaw"); + this->declare_parameter("LQR_params.i_surge"); + this->declare_parameter("LQR_params.i_pitch"); + this->declare_parameter("LQR_params.i_yaw"); + this->declare_parameter("LQR_params.i_weight"); + this->declare_parameter("LQR_params.dt"); + this->declare_parameter>("inertia_matrix"); + + this->lqr_parameters.q_surge=this->get_parameter("LQR_params.q_surge").as_double(); + this->lqr_parameters.q_pitch=this->get_parameter("LQR_params.q_pitch").as_double(); + this->lqr_parameters.q_yaw=this->get_parameter("LQR_params.q_yaw").as_double(); + this->lqr_parameters.r_surge=this->get_parameter("LQR_params.r_surge").as_double(); + this->lqr_parameters.r_pitch=this->get_parameter("LQR_params.r_pitch").as_double(); + this->lqr_parameters.r_yaw=this->get_parameter("LQR_params.r_yaw").as_double(); + this->lqr_parameters.i_surge=this->get_parameter("LQR_params.i_surge").as_double(); + this->lqr_parameters.i_pitch=this->get_parameter("LQR_params.i_pitch").as_double(); + this->lqr_parameters.i_yaw=this->get_parameter("LQR_params.i_yaw").as_double(); + this->lqr_parameters.i_weight=this->get_parameter("LQR_params.i_weight").as_double(); + this->lqr_parameters.max_force=max_force; + this->inertia_matrix=this->get_parameter("inertia_matrix").as_double_array(); + + +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + +//---------------------------------------------------------------------------------------------------------------- +//Operator overloading for geometry_msgs::msg::WrenchStamped +/* +geometry_msgs::msg::WrenchStamped operator-(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) +{ + geometry_msgs::msg::WrenchStamped result; + result.wrench.force.x = a.wrench.force.x - b.wrench.force.x; + result.wrench.force.y = a.wrench.force.y - b.wrench.force.y; + result.wrench.force.z = a.wrench.force.z - b.wrench.force.z; + result.wrench.torque.x = a.wrench.torque.x - b.wrench.torque.x; + result.wrench.torque.y = a.wrench.torque.y - b.wrench.torque.y; + result.wrench.torque.z = a.wrench.torque.z - b.wrench.torque.z; + return result; +} +geometry_msgs::msg::WrenchStamped operator+(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) +{ + geometry_msgs::msg::WrenchStamped result; + result.wrench.force.x = a.wrench.force.x + b.wrench.force.x; + result.wrench.force.y = a.wrench.force.y + b.wrench.force.y; + result.wrench.force.z = a.wrench.force.z + b.wrench.force.z; + result.wrench.torque.x = a.wrench.torque.x + b.wrench.torque.x; + result.wrench.torque.y = a.wrench.torque.y + b.wrench.torque.y; + result.wrench.torque.z = a.wrench.torque.z + b.wrench.torque.z; + return result; +} +//operator overloading for guidance_data +Guidance_data Guidance_data::operator-(const Guidance_data & b) const +{ + Guidance_data result; + result.surge = this->surge - b.surge; + result.pitch = this->pitch - b.pitch; + result.yaw = this->yaw - b.yaw; + return result; +} + +Guidance_data& Guidance_data::operator=(const std_msgs::msg::Float64MultiArray& msg) +{ + if (msg.data.size()>=3){ + surge=msg.data[0]; + pitch=msg.data[1]; + yaw=msg.data[2]; + } + else{ + //throw std::runtime_error("Guidance message too short, needs at least 3 values"); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Guidance message too short, needs at least 3 values"); + } + return *this; +} +*/ +