From ab3bd3a7a2daf786e2e13d8ad71b405545d46fd4 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 24 Sep 2025 13:41:00 +0200 Subject: [PATCH 01/22] added Node, publisher, launch file, config file --- control/velocity_controller/CMakeLists.txt | 44 ++++++++++++++ .../config/parameters.yaml | 6 ++ .../launch/velocity_controller.launch.py | 29 +++++++++ control/velocity_controller/package.xml | 22 +++++++ .../src/velocity_controller.cpp | 59 +++++++++++++++++++ 5 files changed, 160 insertions(+) create mode 100644 control/velocity_controller/CMakeLists.txt create mode 100644 control/velocity_controller/config/parameters.yaml create mode 100644 control/velocity_controller/launch/velocity_controller.launch.py create mode 100644 control/velocity_controller/package.xml create mode 100644 control/velocity_controller/src/velocity_controller.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt new file mode 100644 index 00000000..a905a4d0 --- /dev/null +++ b/control/velocity_controller/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(velocity_controller) + +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) + +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() +endif() + +ament_package() + +add_executable(velocity_controller_node src/velocity_controller.cpp) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) +install(TARGETS velocity_controller_node + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_target_dependencies(velocity_controller_node + rclcpp + std_msgs + vortex_msgs +) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml new file mode 100644 index 00000000..e6c322a0 --- /dev/null +++ b/control/velocity_controller/config/parameters.yaml @@ -0,0 +1,6 @@ +velocity_controller_node: # Name of the node usually goes here. We use wildcard so that we can change the name from the launch file. + ros__parameters: + topic_info_out: "velocity_out_topic" # parameter name: parameter value + topic_w_in: "reference" #reference speed and direction + topic_v_in: "current_velocity" #current speed + #publish_value: "Hello from config!" # parameter name: parameter value 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..713d2d8f --- /dev/null +++ b/control/velocity_controller/package.xml @@ -0,0 +1,22 @@ + + + + velocity_controller + 0.0.0 + TODO: Package description + henrik + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + vortex_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp new file mode 100644 index 00000000..1fc740a8 --- /dev/null +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -0,0 +1,59 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +//#include "vortex-msgs/msg" kan legge til nye meldinger nå + +//Lager en klasse velocity node +class Velocity_node : public rclcpp::Node +{ +public: +//Konstruktør + Velocity_node() : Node("velocity_controller_node") + { + //Dytter info til log + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); + + //Parameter from config. !!Needs to create launch file to prevent writing where to get the parameters from + this->declare_parameter("topic_info_out"); + this->declare_parameter("topic_w_in"); + info_out_topic = this->get_parameter("topic_info_out").as_string(); + reference_topic=this->get_parameter("topic_w_in").as_string(); + + // Lager en publisher som publisher på topic, velocity topic, 10 i "sikkerhet" + publisher_ = create_publisher(info_out_topic, 10); + //Lager en timer som kaller funksjonen timer_callback hvert 500ms + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&Velocity_node::send_velocity, this)); + } + + +//Alle funksjonens variabler +//Publisher og timer instansene + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + std::string info_out_topic; + std::string reference_topic; + + +//Utdata på topic + void send_velocity() + { + auto message = std_msgs::msg::String(); + message.data = "Velocity command"; + //RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + publisher_->publish(message); + } + + + +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + From 23fea72229b0a4cdec8cea9e76390c43432b3d7b Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 24 Sep 2025 14:33:11 +0200 Subject: [PATCH 02/22] added subscriber with topic in config --- .../velocity_controller/config/parameters.yaml | 2 +- .../src/velocity_controller.cpp | 15 ++++++++++++--- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index e6c322a0..7fbf4f03 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -1,6 +1,6 @@ velocity_controller_node: # Name of the node usually goes here. We use wildcard so that we can change the name from the launch file. ros__parameters: topic_info_out: "velocity_out_topic" # parameter name: parameter value - topic_w_in: "reference" #reference speed and direction + topic_ref_in: "reference" #reference speed and direction topic_v_in: "current_velocity" #current speed #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 1fc740a8..efd6cd38 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -14,9 +14,9 @@ class Velocity_node : public rclcpp::Node //Parameter from config. !!Needs to create launch file to prevent writing where to get the parameters from this->declare_parameter("topic_info_out"); - this->declare_parameter("topic_w_in"); + this->declare_parameter("topic_ref_in"); info_out_topic = this->get_parameter("topic_info_out").as_string(); - reference_topic=this->get_parameter("topic_w_in").as_string(); + reference_topic=this->get_parameter("topic_ref_in").as_string(); // Lager en publisher som publisher på topic, velocity topic, 10 i "sikkerhet" publisher_ = create_publisher(info_out_topic, 10); @@ -24,6 +24,10 @@ class Velocity_node : public rclcpp::Node timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&Velocity_node::send_velocity, this)); + + subscriber_ = this->create_subscription( + reference_topic,10, + std::bind(&Velocity_node::recieve_new_reference,this, std::placeholders::_1)); } @@ -31,6 +35,7 @@ class Velocity_node : public rclcpp::Node //Publisher og timer instansene rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr subscriber_; std::string info_out_topic; std::string reference_topic; @@ -45,7 +50,11 @@ class Velocity_node : public rclcpp::Node publisher_->publish(message); } - +//Ny referanse funksjon: + void recieve_new_reference(const std_msgs::msg::String::SharedPtr msg_ptr){ + RCLCPP_INFO(this->get_logger(), "Received reference: '%s'", msg_ptr->data.c_str()); + + } }; From 1d1cb530bb5db09bbbbb9b6c67b4b5d4ea3b193a Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 24 Sep 2025 15:00:46 +0200 Subject: [PATCH 03/22] Added hpp file --- .../include/velocity_controller/velocity_controller.hpp | 0 control/velocity_controller/src/velocity_controller.cpp | 3 ++- 2 files changed, 2 insertions(+), 1 deletion(-) create mode 100644 control/velocity_controller/include/velocity_controller/velocity_controller.hpp 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..e69de29b diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index efd6cd38..90a52254 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -1,5 +1,6 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" +#include "velocity_controller/velocity_controller.hpp" //#include "vortex-msgs/msg" kan legge til nye meldinger nå //Lager en klasse velocity node @@ -12,7 +13,7 @@ class Velocity_node : public rclcpp::Node //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); - //Parameter from config. !!Needs to create launch file to prevent writing where to get the parameters from + //Parameter from config. this->declare_parameter("topic_info_out"); this->declare_parameter("topic_ref_in"); info_out_topic = this->get_parameter("topic_info_out").as_string(); From 60dbb728fe9f5533f80e2ef33cde605d7f4ec775 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 24 Sep 2025 16:41:11 +0200 Subject: [PATCH 04/22] added subscriber to current velocity and pitch maybe --- control/velocity_controller/CMakeLists.txt | 6 + .../config/parameters.yaml | 4 +- .../velocity_controller.hpp | 33 +++++ control/velocity_controller/package.xml | 1 + .../src/velocity_controller.cpp | 118 +++++++++++------- 5 files changed, 115 insertions(+), 47 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index a905a4d0..cf6d27f6 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -27,6 +27,11 @@ ament_package() add_executable(velocity_controller_node src/velocity_controller.cpp) +target_include_directories(velocity_controller_node PUBLIC + $ + $ +) + install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ ) @@ -41,4 +46,5 @@ ament_target_dependencies(velocity_controller_node rclcpp std_msgs vortex_msgs + geometry_msgs ) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 7fbf4f03..feba4875 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -1,6 +1,6 @@ -velocity_controller_node: # Name of the node usually goes here. We use wildcard so that we can change the name from the launch file. +/**: # Name of the node usually goes here. We use wildcard so that we can change the name from the launch file. ros__parameters: topic_info_out: "velocity_out_topic" # parameter name: parameter value topic_ref_in: "reference" #reference speed and direction - topic_v_in: "current_velocity" #current speed + topic_velocity_and_orientation_in: "current_velocity" #current speed #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index e69de29b..1e882249 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include +//#include "vortex-msgs/msg" kan legge til nye meldinger nå + +class Velocity_node : public rclcpp::Node{ + public: + Velocity_node(); + //publiserer fart + void send_velocity(); + //New reference + void recieve_new_reference(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); + //New current velocity and orientation + void recieve_new_velocity_and_orientation(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); + //Alle funksjonens variabler +//Publisher og timer instansene + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr subscriber_; + rclcpp::Subscription::SharedPtr sub_velocity_and_orientation_; + //Variabler + + std::string info_out_topic; + std::string reference_topic; + std::string velocity_and_orientation_topic; + //lagrer referanse og nåværende fart + + geometry_msgs::msg::WrenchStamped reference; + geometry_msgs::msg::WrenchStamped current_velocity_and_orientation; +}; + diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index 713d2d8f..87c5c268 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -12,6 +12,7 @@ rclcpp std_msgs vortex_msgs + geometry_msgs ament_lint_auto ament_lint_common diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 90a52254..209df774 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -1,63 +1,67 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include "velocity_controller/velocity_controller.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" //#include "vortex-msgs/msg" kan legge til nye meldinger nå //Lager en klasse velocity node -class Velocity_node : public rclcpp::Node -{ -public: -//Konstruktør - Velocity_node() : Node("velocity_controller_node") - { - //Dytter info til log - RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); - //Parameter from config. - this->declare_parameter("topic_info_out"); - this->declare_parameter("topic_ref_in"); - info_out_topic = this->get_parameter("topic_info_out").as_string(); - reference_topic=this->get_parameter("topic_ref_in").as_string(); - - // Lager en publisher som publisher på topic, velocity topic, 10 i "sikkerhet" - publisher_ = create_publisher(info_out_topic, 10); - //Lager en timer som kaller funksjonen timer_callback hvert 500ms - timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&Velocity_node::send_velocity, this)); +//Konstruktør +Velocity_node::Velocity_node() : Node("velocity_controller_node") +{ + //Dytter info til log + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); - subscriber_ = this->create_subscription( - reference_topic,10, - std::bind(&Velocity_node::recieve_new_reference,this, std::placeholders::_1)); - } + //Parameter from config. + this->declare_parameter("topic_info_out"); + this->declare_parameter("topic_ref_in"); + this->declare_parameter("topic_velocity_and_orientation_in"); + info_out_topic = this->get_parameter("topic_info_out").as_string(); + reference_topic=this->get_parameter("topic_ref_in").as_string(); + velocity_and_orientation_topic = this->get_parameter("topic_velocity_and_orientation_in").as_string(); + // Lager en publisher som publisher på topic, velocity topic, 10 i "sikkerhet" + publisher_ = create_publisher(info_out_topic, 10); + sub_velocity_and_orientation_ = this->create_subscription( + velocity_and_orientation_topic, 10, + std::bind(&Velocity_node::recieve_new_velocity_and_orientation,this, std::placeholders::_1)); + //Lager en timer som kaller funksjonen timer_callback hvert 500ms + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&Velocity_node::send_velocity, this)); -//Alle funksjonens variabler -//Publisher og timer instansene - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr subscriber_; + subscriber_ = this->create_subscription( + reference_topic,10, + std::bind(&Velocity_node::recieve_new_reference,this, std::placeholders::_1)); +} - std::string info_out_topic; - std::string reference_topic; //Utdata på topic - void send_velocity() - { - auto message = std_msgs::msg::String(); - message.data = "Velocity command"; - //RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - publisher_->publish(message); - } - -//Ny referanse funksjon: - void recieve_new_reference(const std_msgs::msg::String::SharedPtr msg_ptr){ - RCLCPP_INFO(this->get_logger(), "Received reference: '%s'", msg_ptr->data.c_str()); - - } +void Velocity_node::send_velocity() +{ + auto message = geometry_msgs::msg::WrenchStamped(); + message.wrench.force.x = 1.0; + message.wrench.force.y = 0.0; + message.wrench.force.z = 0.0; + message.wrench.torque.x = 0.0; + message.wrench.torque.y = 0.0; + message.wrench.torque.z = 0.0; + publisher_->publish(message); +} -}; +//Ny referanse funksjon: +void Velocity_node::recieve_new_reference(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ + RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); + reference = *msg_ptr; + return; +} +//Ny velocity og orientation funksjon: +void Velocity_node::recieve_new_velocity_and_orientation(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ + RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); + current_velocity_and_orientation = *msg_ptr; + return; +} int main(int argc, char * argv[]) { @@ -67,3 +71,27 @@ int main(int argc, char * argv[]) 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; +} \ No newline at end of file From 252ec54a121ef6b930b43bdc298554a42885e941 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 1 Oct 2025 13:20:43 +0200 Subject: [PATCH 05/22] Changed parameter file, topic and subscriber name --- control/velocity_controller/CMakeLists.txt | 13 +++- .../config/parameters.yaml | 36 ++++++++-- .../velocity_controller.hpp | 39 ++++++----- .../launch/VCnTest.launch.py | 40 +++++++++++ control/velocity_controller/src/test_VC.cpp | 43 ++++++++++++ .../src/velocity_controller.cpp | 66 ++++++++++++------- 6 files changed, 190 insertions(+), 47 deletions(-) create mode 100644 control/velocity_controller/launch/VCnTest.launch.py create mode 100644 control/velocity_controller/src/test_VC.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index cf6d27f6..a0e30f50 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -26,7 +26,7 @@ endif() ament_package() add_executable(velocity_controller_node src/velocity_controller.cpp) - +add_executable(test_VC_node src/test_VC.cpp) target_include_directories(velocity_controller_node PUBLIC $ $ @@ -38,7 +38,10 @@ install(DIRECTORY launch install(TARGETS velocity_controller_node DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY config +install(TARGETS test_VC_node + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/ ) @@ -48,3 +51,9 @@ ament_target_dependencies(velocity_controller_node vortex_msgs geometry_msgs ) +ament_target_dependencies(test_VC_node + rclcpp + std_msgs + vortex_msgs + geometry_msgs +) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index feba4875..c0dc252b 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -1,6 +1,34 @@ -/**: # Name of the node usually goes here. We use wildcard so that we can change the name from the launch file. +/**: ros__parameters: - topic_info_out: "velocity_out_topic" # parameter name: parameter value - topic_ref_in: "reference" #reference speed and direction - topic_velocity_and_orientation_in: "current_velocity" #current speed + + topics: + odom_topic: /orca/odom #Odoemtry + twist_topic: /dvl/twist #Twist + pose_topic: /dvl/pose #Pose + guidance_topic: /guidance/los #Guidance + thrust_topic: /thrust/wrench_input #Thrust + softwareoperation_topic: /softwareOperationMode #Software Operation + killswitch_topic: /softwareKillSwitch #Kill Switch + + LQR_params: + q_surge: 75 + q_pitch: 175 + q_yaw: 175 + + 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.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + + #Clamp parameter + max_force: 99.5 #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 1e882249..60c55a93 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -3,30 +3,37 @@ #include #include #include + //#include "vortex-msgs/msg" kan legge til nye meldinger nå class Velocity_node : public rclcpp::Node{ public: Velocity_node(); - //publiserer fart - void send_velocity(); - //New reference - void recieve_new_reference(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); - //New current velocity and orientation - void recieve_new_velocity_and_orientation(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); - //Alle funksjonens variabler -//Publisher og timer instansene - rclcpp::Publisher::SharedPtr publisher_; + //Timer functions + void publish_thrust(); + //Callback functions + void guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); + void killswitch_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); + void twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); + + //Publisher instance + rclcpp::Publisher::SharedPtr publisher_thrust; + + //Timer instance rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr subscriber_; - rclcpp::Subscription::SharedPtr sub_velocity_and_orientation_; - //Variabler + //Subscriber instance + rclcpp::Subscription::SharedPtr subscriber_twist; + rclcpp::Subscription::SharedPtr subscriber_guidance; + rclcpp::Subscription::SharedPtr subscriber_killswitch; + + //Variables for topics - std::string info_out_topic; - std::string reference_topic; - std::string velocity_and_orientation_topic; - //lagrer referanse og nåværende fart + std::string topic_thrust; + std::string topic_guidance; + std::string topic_killswitch; + std::string topic_twist; + //Stored values geometry_msgs::msg::WrenchStamped reference; geometry_msgs::msg::WrenchStamped current_velocity_and_orientation; }; diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py new file mode 100644 index 00000000..ab2b64fd --- /dev/null +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -0,0 +1,40 @@ +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' + ) + + node_name_arg2 = DeclareLaunchArgument( + 'node_name', default_value='test_VC_node', + description='Name of the test VC node' + ) + + velocity_controller_name = LaunchConfiguration('node_name') + test_VC_name = LaunchConfiguration('node_name') + + return LaunchDescription([ + node_name_arg, + node_name_arg2, + Node(package='velocity_controller', + executable='velocity_controller_node', + name=velocity_controller_name, + output='screen', + parameters=[config_path]), + 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/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp new file mode 100644 index 00000000..bad2dfcb --- /dev/null +++ b/control/velocity_controller/src/test_VC.cpp @@ -0,0 +1,43 @@ +#include +#include +#include +#include +//Denne noden er kun for å teste velocity_controller noden + +class test_VC : public rclcpp::Node{ + public: + test_VC() : Node("test_VC_node") + { + this->declare_parameter("topics.guidance_topic"); + topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); + publisher_ = this->create_publisher(topic_guidance,10); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&test_VC::send_velocity, this)); + clock_ = this->get_clock(); + RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Clock::SharedPtr clock_; + std::string topic_guidance; + void send_velocity() + { + auto message = geometry_msgs::msg::WrenchStamped(); + message.wrench.force.x = std::sin(clock_->now().seconds()); + message.wrench.force.y = 0.0; + message.wrench.force.z = 0.0; + message.wrench.torque.x = 0.0; + message.wrench.torque.y = 0.0; + message.wrench.torque.z = 0.0; + publisher_->publish(message); + } +}; +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/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 209df774..dcfe1efa 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -13,32 +13,39 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); //Parameter from config. - this->declare_parameter("topic_info_out"); - this->declare_parameter("topic_ref_in"); - this->declare_parameter("topic_velocity_and_orientation_in"); - info_out_topic = this->get_parameter("topic_info_out").as_string(); - reference_topic=this->get_parameter("topic_ref_in").as_string(); - velocity_and_orientation_topic = this->get_parameter("topic_velocity_and_orientation_in").as_string(); + this->declare_parameter("topics.thrust_topic"); + this->declare_parameter("topics.guidance_topic"); + this->declare_parameter("topics.twist_topic"); + this->declare_parameter("topics.killswitch_topic"); + this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); + this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); + this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); + this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); - // Lager en publisher som publisher på topic, velocity topic, 10 i "sikkerhet" - publisher_ = create_publisher(info_out_topic, 10); - sub_velocity_and_orientation_ = this->create_subscription( - velocity_and_orientation_topic, 10, - std::bind(&Velocity_node::recieve_new_velocity_and_orientation,this, std::placeholders::_1)); - //Lager en timer som kaller funksjonen timer_callback hvert 500ms - timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&Velocity_node::send_velocity, this)); + // Publishers + publisher_thrust = create_publisher(topic_thrust, 10); + + //Subscribers + subscriber_twist = this->create_subscription( + topic_twist, 10, + std::bind(&Velocity_node::twist_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)); - subscriber_ = this->create_subscription( - reference_topic,10, - std::bind(&Velocity_node::recieve_new_reference,this, std::placeholders::_1)); + //Timer + timer_ = this->create_wall_timer(std::chrono::milliseconds(500),std::bind(&Velocity_node::publish_thrust, this)); + + } -//Utdata på topic -void Velocity_node::send_velocity() +//Publish functions +void Velocity_node::publish_thrust() { auto message = geometry_msgs::msg::WrenchStamped(); message.wrench.force.x = 1.0; @@ -47,22 +54,31 @@ void Velocity_node::send_velocity() message.wrench.torque.x = 0.0; message.wrench.torque.y = 0.0; message.wrench.torque.z = 0.0; - publisher_->publish(message); + publisher_thrust->publish(message); } -//Ny referanse funksjon: -void Velocity_node::recieve_new_reference(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ +//Callback functions +void Velocity_node::guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); reference = *msg_ptr; return; } -//Ny velocity og orientation funksjon: -void Velocity_node::recieve_new_velocity_and_orientation(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ +void Velocity_node::twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); current_velocity_and_orientation = *msg_ptr; return; } +//**Needs to update to shutdown the node +void Velocity_node::killswitch_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ + RCLCPP_INFO(this->get_logger(), "Received killswitch: '%f'", msg_ptr->wrench.force.x); + if(msg_ptr->wrench.force.x == 1.0){ + reference = geometry_msgs::msg::WrenchStamped(); + current_velocity_and_orientation = geometry_msgs::msg::WrenchStamped(); + RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current velocity set to zero"); + } + return; +} int main(int argc, char * argv[]) { rclcpp::init(argc, argv); From 58a87f9769738ad33becf5ec5c3b68f860d1ca2f Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 1 Oct 2025 14:49:36 +0200 Subject: [PATCH 06/22] Made PI regulator, able to plot the thrust --- .../config/parameters.yaml | 2 ++ .../velocity_controller.hpp | 21 +++++++++-- control/velocity_controller/src/test_VC.cpp | 26 +++++++++++++- .../src/velocity_controller.cpp | 36 +++++++++++-------- 4 files changed, 68 insertions(+), 17 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index c0dc252b..dee5d9f2 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -29,6 +29,8 @@ inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + calculation_rate: 10 #ms integer + publish_rate: 10 #ms #Clamp parameter max_force: 99.5 #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 60c55a93..5707b9d9 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -9,8 +9,11 @@ class Velocity_node : public rclcpp::Node{ public: Velocity_node(); + //Timer functions void publish_thrust(); + void calc_thrust(); + //Callback functions void guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); void killswitch_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); @@ -20,7 +23,9 @@ class Velocity_node : public rclcpp::Node{ rclcpp::Publisher::SharedPtr publisher_thrust; //Timer instance - rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr timer_PID; + rclcpp::TimerBase::SharedPtr timer_publish; + //Subscriber instance rclcpp::Subscription::SharedPtr subscriber_twist; rclcpp::Subscription::SharedPtr subscriber_guidance; @@ -33,8 +38,20 @@ class Velocity_node : public rclcpp::Node{ std::string topic_killswitch; std::string topic_twist; - //Stored values + //Variables for timers + int calculation_rate; + int publish_rate; + + //Stored wrenches values geometry_msgs::msg::WrenchStamped reference; geometry_msgs::msg::WrenchStamped current_velocity_and_orientation; + geometry_msgs::msg::WrenchStamped thrust; + + //PID parameters temporary + double k_p = 5.0; + double k_i = 0.5; + double k_d = 0.0; + double integral = 0.0; + double previous_error = 0.0; //improved Riemanns sums }; diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index bad2dfcb..7cda54c9 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -2,6 +2,7 @@ #include #include #include +#include //Denne noden er kun for å teste velocity_controller noden class test_VC : public rclcpp::Node{ @@ -11,21 +12,33 @@ class test_VC : public rclcpp::Node{ this->declare_parameter("topics.guidance_topic"); topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); publisher_ = this->create_publisher(topic_guidance,10); + this->declare_parameter("topics.thrust_topic"); + topic_subscription = this->get_parameter("topics.thrust_topic").as_string(); + subscription_ = this->create_subscription( + topic_subscription, 10, + std::bind(&test_VC::listen, this, std::placeholders::_1)); timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&test_VC::send_velocity, this)); clock_ = this->get_clock(); + thrust_pub = this->create_publisher("thrust_value", 10); + RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); } rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subscription_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; std::string topic_guidance; + std::string topic_subscription; + std::vector thrust_vector; + rclcpp::Publisher::SharedPtr thrust_pub; + void send_velocity() { auto message = geometry_msgs::msg::WrenchStamped(); - message.wrench.force.x = std::sin(clock_->now().seconds()); + message.wrench.force.x = std::sin(clock_->now().seconds()*2*3.14159/10); //sinuskurve med periode 10 sekunder og amplitude 1 message.wrench.force.y = 0.0; message.wrench.force.z = 0.0; message.wrench.torque.x = 0.0; @@ -33,6 +46,17 @@ class test_VC : public rclcpp::Node{ message.wrench.torque.z = 0.0; publisher_->publish(message); } + + void listen(geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { + thrust_vector.push_back(msg->wrench.force.x); + std_msgs::msg::Float64 pub_info; + pub_info.data = thrust_vector.back(); + thrust_pub->publish(pub_info); + //RCLCPP_INFO(this->get_logger(), "Received thrust: '%f'", msg->wrench.force.x); + return; + } + }; int main(int argc, char const *argv[]) { diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index dcfe1efa..d2939767 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -21,7 +21,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); - + this-> // Publishers publisher_thrust = create_publisher(topic_thrust, 10); @@ -37,34 +37,42 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); //Timer - timer_ = this->create_wall_timer(std::chrono::milliseconds(500),std::bind(&Velocity_node::publish_thrust, this)); + this->declare_parameter("calculation_rate"); + this->declare_parameter("publish_rate"); + this->calculation_rate = this->get_parameter("calculation_rate").as_int(); + this->publish_rate = this->get_parameter("publish_rate").as_int(); + timer_PID = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); + timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); - } -//Publish functions +//Publish/timer functions void Velocity_node::publish_thrust() { - auto message = geometry_msgs::msg::WrenchStamped(); - message.wrench.force.x = 1.0; - message.wrench.force.y = 0.0; - message.wrench.force.z = 0.0; - message.wrench.torque.x = 0.0; - message.wrench.torque.y = 0.0; - message.wrench.torque.z = 0.0; - publisher_thrust->publish(message); + publisher_thrust->publish(thrust); +} + +void Velocity_node::calc_thrust() +{ + auto error_x = reference.wrench.force.x - current_velocity_and_orientation.wrench.force.x; + //PID controller here + integral += error_x * (calculation_rate / 1000.0); //integral term + thrust.wrench.force.x = k_p*error_x + k_i*integral; //Placeholder + return; } + + //Callback functions void Velocity_node::guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ - RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); + //RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); reference = *msg_ptr; return; } void Velocity_node::twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ - RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); + //RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); current_velocity_and_orientation = *msg_ptr; return; } From 2760cbeedcf917754112a4adde047b8a8e38ef15 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 7 Oct 2025 22:18:42 +0200 Subject: [PATCH 07/22] Implemented the LQR lib from python --- control/velocity_controller/CMakeLists.txt | 7 +- .../include/velocity_controller/LQR_setup.hpp | 70 +++++++ .../velocity_controller.hpp | 7 +- control/velocity_controller/src/LQR_setup.cpp | 191 ++++++++++++++++++ control/velocity_controller/src/test_VC.cpp | 12 +- .../src/velocity_controller.cpp | 30 ++- 6 files changed, 300 insertions(+), 17 deletions(-) create mode 100644 control/velocity_controller/include/velocity_controller/LQR_setup.hpp create mode 100644 control/velocity_controller/src/LQR_setup.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index a0e30f50..f062f2f8 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -10,6 +10,9 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(drake REQUIRED) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -31,7 +34,9 @@ target_include_directories(velocity_controller_node PUBLIC $ $ ) - +include_directories(${EIGEN3_INCLUDE_DIR}) +include_directories(${drake_INCLUDE_DIRS}) +target_link_libraries(velocity_controller_node ${drake_LIBRARIES}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ ) 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..c13e5444 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include +#include +#include + + + +class State{ + //Dataclass to store state 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 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; +}; +class LQRController{ + + public: + LQRController(LQRparameters params,std::vector inertia_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); + std::vector> calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); + void set_params(LQRparameters params); + void set_matrices(std::vector inertia_matrix); + void update_augmented_matrices(std::vector > coriolis_matrix); + std::vector update_error(Guidance_values guidance_values, State states); + std::vector saturate_input(std::vector u); + std::vector calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_values guidance_values); + void reset_controller(); + + // Variables + 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; + + std::vector> inertia_matrix_inv; + std::vector> state_weight_matrix; + std::vector> input_weight_matrix; + std::vector> augmented_system_matrix; + std::vector> augmented_input_matrix; +}; + diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 5707b9d9..6cf0fcd1 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -41,17 +41,20 @@ class Velocity_node : public rclcpp::Node{ //Variables for timers int calculation_rate; int publish_rate; + double max_force; //Stored wrenches values geometry_msgs::msg::WrenchStamped reference; - geometry_msgs::msg::WrenchStamped current_velocity_and_orientation; + geometry_msgs::msg::WrenchStamped current_twist; geometry_msgs::msg::WrenchStamped thrust; //PID parameters temporary double k_p = 5.0; - double k_i = 0.5; + double k_i = 2.0; double k_d = 0.0; double integral = 0.0; double previous_error = 0.0; //improved Riemanns sums }; + + diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp new file mode 100644 index 00000000..b901b7b6 --- /dev/null +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -0,0 +1,191 @@ +#include "velocity_controller/LQR_setup.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +LQRController::LQRController(LQRparameters params,std::vector 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; +} + +std::vector> LQRController::calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel){ + //Inertia matrix values?? + return {{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}}; +} + + +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(std::vector inertia_matrix){ + Eigen::Matrix3d mat= vector_to_matrix3d(inertia_matrix); + inertia_matrix_inv = matrix3d_to_vector2d(mat.inverse()); + state_weight_matrix = {{q_surge,0,0,0,0,0}, + {0,q_pitch,0,0,0,0}, + {0,0,q_yaw,0,0,0}, + {0,0,0,i_weight,0,0}, + {0,0,0,0,i_weight,0}, + {0,0,0,0,0,i_weight}}; + input_weight_matrix = {{r_surge,0,0}, + {0,r_pitch,0}, + {0,0,r_yaw}}; + + return; +} + + +void LQRController::update_augmented_matrices(std::vector > coriolis_matrix){ + std::vector> system_matrix = matrix3d_to_vector2d(vector2d_to_matrix3d(inertia_matrix_inv) * vector2d_to_matrix3d(coriolis_matrix)); + //input_matrix = inertia_matrix_inv; + augmented_system_matrix = {{system_matrix[0][0],system_matrix[0][1],system_matrix[0][2],0,0,0}, + {system_matrix[1][0],system_matrix[1][1],system_matrix[1][2],0,0,0}, + {system_matrix[2][0],system_matrix[2][1],system_matrix[2][2],0,0,0}, + {-1,0,0,0,0,0}, + {0,-1,0,0,0,0}, + {0,0,-1,0,0,0}}; //Skal det være -1 her? + augmented_input_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}}; + +}; +std::vector LQRController::update_error(Guidance_values guidance_values, State states){ + double surge_error = guidance_values.surge - states.surge; + double pitch_error = ssa(guidance_values.pitch - states.pitch); + double yaw_error = 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); + + std::vector state_error= {-surge_error, -pitch_error, -yaw_error, integral_error_surge, integral_error_pitch, integral_error_yaw}; + return state_error; +} +std::vector LQRController::saturate_input(std::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}; +} +std::vector LQRController::calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_values guidance_values){ + update_augmented_matrices(coriolis_matrix); + auto result = drake::systems::controllers::LinearQuadraticRegulator( + vector2d_to_matrix3d(augmented_system_matrix), + vector2d_to_matrix3d(augmented_input_matrix), + vector2d_to_matrix3d(state_weight_matrix), + vector2d_to_matrix3d(input_weight_matrix)); + std::vector state_error = update_error(guidance_values, states); + std::vector u= saturate_input(matrix3d_to_vector(-result * vector_to_matrix3d(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; +} + + + + + +int main(){ + return 0; +}; + +//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/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 7cda54c9..3c3e153e 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -3,6 +3,7 @@ #include #include #include +//#include "LQR_setup.hpp" //Denne noden er kun for å teste velocity_controller noden class test_VC : public rclcpp::Node{ @@ -24,6 +25,8 @@ class test_VC : public rclcpp::Node{ thrust_pub = this->create_publisher("thrust_value", 10); RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); + + message.wrench.force.x=1; } rclcpp::Publisher::SharedPtr publisher_; @@ -34,16 +37,11 @@ class test_VC : public rclcpp::Node{ std::string topic_subscription; std::vector thrust_vector; rclcpp::Publisher::SharedPtr thrust_pub; + geometry_msgs::msg::WrenchStamped message; void send_velocity() { - auto message = geometry_msgs::msg::WrenchStamped(); message.wrench.force.x = std::sin(clock_->now().seconds()*2*3.14159/10); //sinuskurve med periode 10 sekunder og amplitude 1 - message.wrench.force.y = 0.0; - message.wrench.force.z = 0.0; - message.wrench.torque.x = 0.0; - message.wrench.torque.y = 0.0; - message.wrench.torque.z = 0.0; publisher_->publish(message); } @@ -53,6 +51,7 @@ class test_VC : public rclcpp::Node{ std_msgs::msg::Float64 pub_info; pub_info.data = thrust_vector.back(); thrust_pub->publish(pub_info); + message.wrench.force.x+=0.01*msg->wrench.force.x; //RCLCPP_INFO(this->get_logger(), "Received thrust: '%f'", msg->wrench.force.x); return; } @@ -60,6 +59,7 @@ class test_VC : public rclcpp::Node{ }; int main(int argc, char const *argv[]) { + rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index d2939767..354f1fdd 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -17,11 +17,13 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") this->declare_parameter("topics.guidance_topic"); this->declare_parameter("topics.twist_topic"); this->declare_parameter("topics.killswitch_topic"); + this->declare_parameter("max_force"); + this->max_force = this->get_parameter("max_force").as_double(); this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); - this-> + // Publishers publisher_thrust = create_publisher(topic_thrust, 10); @@ -54,12 +56,24 @@ void Velocity_node::publish_thrust() publisher_thrust->publish(thrust); } +//** må forbedre integrasjon og derivasjons beregningene void Velocity_node::calc_thrust() { - auto error_x = reference.wrench.force.x - current_velocity_and_orientation.wrench.force.x; - //PID controller here - integral += error_x * (calculation_rate / 1000.0); //integral term - thrust.wrench.force.x = k_p*error_x + k_i*integral; //Placeholder + auto error_x = reference.wrench.force.x - current_twist.wrench.force.x; + /*if (!(thrust.wrench.force.x == max_force || thrust.wrench.force.x == -max_force)){ + integral += error_x * (calculation_rate / 1000.0); //anti windup + }*/ + integral += error_x * (calculation_rate / 1000.0); + double derivative = (error_x - previous_error) / (calculation_rate / 1000.0); + previous_error = error_x; + + thrust.wrench.force.x = k_p*error_x + k_i*integral+k_d*derivative; + if (thrust.wrench.force.x > max_force){ + thrust.wrench.force.x = max_force; + } + else if (thrust.wrench.force.x < -max_force){ + thrust.wrench.force.x = -max_force; + } return; } @@ -73,7 +87,7 @@ void Velocity_node::guidance_callback(const geometry_msgs::msg::WrenchStamped::S } void Velocity_node::twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ //RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); - current_velocity_and_orientation = *msg_ptr; + current_twist = *msg_ptr; return; } @@ -82,8 +96,8 @@ void Velocity_node::killswitch_callback(const geometry_msgs::msg::WrenchStamped: RCLCPP_INFO(this->get_logger(), "Received killswitch: '%f'", msg_ptr->wrench.force.x); if(msg_ptr->wrench.force.x == 1.0){ reference = geometry_msgs::msg::WrenchStamped(); - current_velocity_and_orientation = geometry_msgs::msg::WrenchStamped(); - RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current velocity set to zero"); + current_twist = geometry_msgs::msg::WrenchStamped(); + RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current twist set to zero"); } return; } From fc5d99bc5914bf76ccd3312dbd466a3f7a008132 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 8 Oct 2025 12:49:24 +0200 Subject: [PATCH 08/22] added a PID_controller class --- .../include/velocity_controller/PID_setup.hpp | 20 ++++++++++ control/velocity_controller/src/PID_setup.cpp | 39 +++++++++++++++++++ 2 files changed, 59 insertions(+) create mode 100644 control/velocity_controller/include/velocity_controller/PID_setup.hpp create mode 100644 control/velocity_controller/src/PID_setup.cpp 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..59de2484 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include +#include +#include + +class PID_controller { + PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output); + double calculate_thrust(double reference, double current_position, double dt); + void reset_controller(); + private: + double k_p; + double k_i; + double k_d; + double max_output; + double min_output; + double integral; + double previous_error; + double previous_position; +}; \ No newline at end of file diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp new file mode 100644 index 00000000..1901b992 --- /dev/null +++ b/control/velocity_controller/src/PID_setup.cpp @@ -0,0 +1,39 @@ +#include "velocity_controller/PID_setup.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; +}; +double PID_controller::calculate_thrust(double reference, double current_position, double dt){ + //Error calculation + double error = reference - current_position; + //P calculation + double output=k_p*error; + //D calculation + output += k_d * (current_position - previous_position) / 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 + } + output += k_i * integral; + previous_error = error; + //Output calculation with saturation + + if (output > max_output){ + output = max_output; + } + else if (output < min_output){ + output = min_output; + } + return output; +}; +void PID_controller::reset_controller(){ + integral = 0.0; + previous_error = 0.0; + previous_position = 0.0; +} \ No newline at end of file From b962ad5c6476d36a86480cfc2ab6f262c7f51c4f Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 8 Oct 2025 15:53:04 +0200 Subject: [PATCH 09/22] changed datatypes, information flow --- control/velocity_controller/CMakeLists.txt | 3 +- .../config/parameters.yaml | 6 +- .../include/velocity_controller/PID_setup.hpp | 20 +++- .../velocity_controller.hpp | 57 ++++++--- control/velocity_controller/src/LQR_setup.cpp | 2 +- control/velocity_controller/src/PID_setup.cpp | 49 ++++++-- .../src/velocity_controller.cpp | 113 +++++++++++++----- 7 files changed, 181 insertions(+), 69 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index f062f2f8..aafca182 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -28,8 +28,9 @@ endif() ament_package() -add_executable(velocity_controller_node src/velocity_controller.cpp) +add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp) add_executable(test_VC_node src/test_VC.cpp) + target_include_directories(velocity_controller_node PUBLIC $ $ diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index dee5d9f2..5be2714b 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -2,7 +2,7 @@ ros__parameters: topics: - odom_topic: /orca/odom #Odoemtry + odom_topic: /orca/odom #Odometry twist_topic: /dvl/twist #Twist pose_topic: /dvl/pose #Pose guidance_topic: /guidance/los #Guidance @@ -29,8 +29,8 @@ inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] - calculation_rate: 10 #ms integer - publish_rate: 10 #ms + calculation_rate: 200 #ms integer + publish_rate: 200 #ms #Clamp parameter max_force: 99.5 #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index 59de2484..10f42401 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -5,16 +5,28 @@ #include class PID_controller { + public: PID_controller( double k_p, double k_i, double k_d, double max_output, double min_output); - double calculate_thrust(double reference, double current_position, double dt); + 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 max_output; - double min_output; double integral; double previous_error; double previous_position; -}; \ No newline at end of file + double last_output; + double max_output; + double min_output; +}; +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); diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 6cf0fcd1..1d14a029 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -2,10 +2,26 @@ #include #include +#include +#include #include +#include +#include +#include "velocity_controller/PID_setup.hpp" + //#include "vortex-msgs/msg" kan legge til nye meldinger nå +class guidance_data{ + public: + double surge; double pitch; double yaw; + guidance_data(std_msgs::msg::Float64MultiArray msg); + guidance_data():surge(0.0), pitch(0.0), yaw(0.0) {}; + + guidance_data operator-(const guidance_data& other) const; + guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg); +}; + class Velocity_node : public rclcpp::Node{ public: Velocity_node(); @@ -15,21 +31,23 @@ class Velocity_node : public rclcpp::Node{ void calc_thrust(); //Callback functions - void guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); - void killswitch_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); - void twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr); - + void guidance_callback(const std_msgs::msg::Float64MultiArray::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); + //Publisher instance rclcpp::Publisher::SharedPtr publisher_thrust; //Timer instance - rclcpp::TimerBase::SharedPtr timer_PID; + rclcpp::TimerBase::SharedPtr timer_calculation; rclcpp::TimerBase::SharedPtr timer_publish; //Subscriber instance - rclcpp::Subscription::SharedPtr subscriber_twist; - rclcpp::Subscription::SharedPtr subscriber_guidance; - rclcpp::Subscription::SharedPtr subscriber_killswitch; + rclcpp::Subscription::SharedPtr subscriber_twist; + rclcpp::Subscription::SharedPtr subscriber_pose; + rclcpp::Subscription::SharedPtr subscriber_guidance; + rclcpp::Subscription::SharedPtr subscriber_killswitch; //Variables for topics @@ -37,6 +55,7 @@ class Velocity_node : public rclcpp::Node{ std::string topic_guidance; std::string topic_killswitch; std::string topic_twist; + std::string topic_pose; //Variables for timers int calculation_rate; @@ -44,17 +63,19 @@ class Velocity_node : public rclcpp::Node{ double max_force; //Stored wrenches values - geometry_msgs::msg::WrenchStamped reference; - geometry_msgs::msg::WrenchStamped current_twist; - geometry_msgs::msg::WrenchStamped thrust; - - //PID parameters temporary - double k_p = 5.0; - double k_i = 2.0; - double k_d = 0.0; - double integral = 0.0; - double previous_error = 0.0; //improved Riemanns sums + std_msgs::msg::Float64MultiArray reference_in; + guidance_data reference; + guidance_data current_state; + geometry_msgs::msg::WrenchStamped thrust_out; + + //PID controllers + PID_controller PID_surge; + PID_controller PID_yaw; + PID_controller PID_pitch; + + }; + diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index b901b7b6..84d94c60 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -136,7 +136,7 @@ std::vector LQRController::calculate_lqr_u(std::vector state_error = update_error(guidance_values, states); - std::vector u= saturate_input(matrix3d_to_vector(-result * vector_to_matrix3d(state_error))); + std::vector u= saturate_input(matrix3d_to_vector(- result * vector_to_matrix3d(state_error))); return u; } void LQRController::reset_controller(){ diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index 1901b992..74c4fe33 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -5,13 +5,13 @@ PID_controller::PID_controller( double k_p, double k_i, double k_d, double max_o previous_error = 0.0; previous_position = 0.0; }; -double PID_controller::calculate_thrust(double reference, double current_position, double dt){ +void PID_controller::calculate_thrust(double reference, double current_position, double dt){ //Error calculation double error = reference - current_position; //P calculation - double output=k_p*error; + last_output=k_p*error; //D calculation - output += k_d * (current_position - previous_position) / dt; + last_output += k_d * (current_position - previous_position) / dt; previous_position = current_position; //I calculation with anti-windup integral += error * dt; @@ -20,20 +20,49 @@ double PID_controller::calculate_thrust(double reference, double current_positio } else if (integral < min_output) { integral -= error * dt; //anti windup } - output += k_i * integral; + last_output += k_i * integral; previous_error = error; //Output calculation with saturation - if (output > max_output){ - output = max_output; + if (last_output > max_output){ + last_output = max_output; } - else if (output < min_output){ - output = min_output; + else if (last_output < min_output){ + last_output = min_output; } - return output; + return; }; void PID_controller::reset_controller(){ integral = 0.0; previous_error = 0.0; previous_position = 0.0; -} \ No newline at end of file +} + +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; +}; + +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 index 354f1fdd..42886eb8 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -2,12 +2,18 @@ #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 "vortex-msgs/msg" kan legge til nye meldinger nå //Lager en klasse velocity node //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node") +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1,1), PID_yaw(1,1,1), PID_pitch(1,1,1) { //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); @@ -16,6 +22,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") this->declare_parameter("topics.thrust_topic"); this->declare_parameter("topics.guidance_topic"); this->declare_parameter("topics.twist_topic"); + this->declare_parameter("topics.pose_topic"); this->declare_parameter("topics.killswitch_topic"); this->declare_parameter("max_force"); this->max_force = this->get_parameter("max_force").as_double(); @@ -23,18 +30,24 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); - + this->topic_pose = this->get_parameter("topics.pose_topic").as_string(); + // Publishers publisher_thrust = create_publisher(topic_thrust, 10); //Subscribers - subscriber_twist = this->create_subscription( + subscriber_twist = this->create_subscription( topic_twist, 10, std::bind(&Velocity_node::twist_callback,this, std::placeholders::_1)); - subscriber_guidance = this->create_subscription( + + subscriber_pose = this->create_subscription( + topic_pose, 10, + std::bind(&Velocity_node::pose_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( + subscriber_killswitch = this->create_subscription( topic_killswitch,10, std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); @@ -43,9 +56,12 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") this->declare_parameter("publish_rate"); this->calculation_rate = this->get_parameter("calculation_rate").as_int(); this->publish_rate = this->get_parameter("publish_rate").as_int(); - timer_PID = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); + timer_calculation = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); + 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); } @@ -53,51 +69,49 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node") //Publish/timer functions void Velocity_node::publish_thrust() { - publisher_thrust->publish(thrust); + publisher_thrust->publish(thrust_out); } //** må forbedre integrasjon og derivasjons beregningene void Velocity_node::calc_thrust() { - auto error_x = reference.wrench.force.x - current_twist.wrench.force.x; - /*if (!(thrust.wrench.force.x == max_force || thrust.wrench.force.x == -max_force)){ - integral += error_x * (calculation_rate / 1000.0); //anti windup - }*/ - integral += error_x * (calculation_rate / 1000.0); - double derivative = (error_x - previous_error) / (calculation_rate / 1000.0); - previous_error = error_x; - - thrust.wrench.force.x = k_p*error_x + k_i*integral+k_d*derivative; - if (thrust.wrench.force.x > max_force){ - thrust.wrench.force.x = max_force; - } - else if (thrust.wrench.force.x < -max_force){ - thrust.wrench.force.x = -max_force; - } + PID_surge.calculate_thrust(reference.surge, current_state.surge,calculation_rate/1000.0); + PID_pitch.calculate_thrust(reference.pitch, current_state.pitch,calculation_rate/1000.0); + PID_yaw.calculate_thrust(reference.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(); + return; } //Callback functions -void Velocity_node::guidance_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ +void Velocity_node::guidance_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg_ptr){ //RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); reference = *msg_ptr; return; } -void Velocity_node::twist_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ +void Velocity_node::twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr){ //RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); - current_twist = *msg_ptr; + current_state.surge = msg_ptr->twist.twist.linear.x; + return; +} +void Velocity_node::pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr){ + 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; return; } //**Needs to update to shutdown the node -void Velocity_node::killswitch_callback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg_ptr){ - RCLCPP_INFO(this->get_logger(), "Received killswitch: '%f'", msg_ptr->wrench.force.x); - if(msg_ptr->wrench.force.x == 1.0){ - reference = geometry_msgs::msg::WrenchStamped(); - current_twist = geometry_msgs::msg::WrenchStamped(); - RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current twist set to zero"); +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){ + reference = guidance_data(); + current_state = guidance_data(); + RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current state set to zero"); } return; } @@ -132,4 +146,39 @@ geometry_msgs::msg::WrenchStamped operator+(const geometry_msgs::msg::WrenchStam 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; -} \ No newline at end of file +} +//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; +} + +guidance_data::guidance_data(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"); + } + }; \ No newline at end of file From 5e8e77abd077b7a9a472ca088a2ad51ea57ede2d Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 12 Oct 2025 11:20:11 +0200 Subject: [PATCH 10/22] New get_paramters function, changed topics in test_node --- control/velocity_controller/CMakeLists.txt | 134 +++++++++++++----- .../include/velocity_controller/PID_setup.hpp | 12 ++ .../include/velocity_controller/test_VC.hpp | 47 ++++++ .../velocity_controller.hpp | 12 +- control/velocity_controller/src/test_VC.cpp | 128 +++++++++++------ .../src/velocity_controller.cpp | 42 +++--- 6 files changed, 270 insertions(+), 105 deletions(-) create mode 100644 control/velocity_controller/include/velocity_controller/test_VC.hpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index aafca182..424df4ba 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -1,10 +1,15 @@ cmake_minimum_required(VERSION 3.8) project(velocity_controller) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + 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) @@ -13,42 +18,29 @@ find_package(vortex_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(drake REQUIRED) +include_directories( + ${EIGEN3_INCLUDE_DIR} + ${drake_INCLUDE_DIRS} + include +) -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() -endif() - -ament_package() - -add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp) -add_executable(test_VC_node src/test_VC.cpp) +#set(LIB_NAME "${PROJECT_NAME}_component") -target_include_directories(velocity_controller_node PUBLIC - $ - $ -) -include_directories(${EIGEN3_INCLUDE_DIR}) -include_directories(${drake_INCLUDE_DIRS}) -target_link_libraries(velocity_controller_node ${drake_LIBRARIES}) -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME}/ -) -install(TARGETS velocity_controller_node - DESTINATION lib/${PROJECT_NAME} -) -install(TARGETS test_VC_node - DESTINATION lib/${PROJECT_NAME} +#add_library(${LIB_NAME} SHARED +# src/LQR_setup.cpp +# src/PID_setup.cpp +# src/test_VC.cpp +# src/velocity_controller.cpp +#) +add_executable(velocity_controller_node + src/velocity_controller.cpp + src/PID_setup.cpp ) -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME}/ + +add_executable(test_VC_node + src/test_VC.cpp + src/PID_setup.cpp +# src/velocity_controller.cpp ) ament_target_dependencies(velocity_controller_node @@ -56,10 +48,86 @@ ament_target_dependencies(velocity_controller_node std_msgs vortex_msgs geometry_msgs + Eigen3 ) + ament_target_dependencies(test_VC_node rclcpp std_msgs vortex_msgs geometry_msgs + Eigen3 +) + +install(TARGETS + velocity_controller_node + test_VC_node + DESTINATION lib/${PROJECT_NAME} +) + + +#rclcpp_components_register_node( +# ${LIB_NAME} +# PLUGIN "velocity_controller_node" +# EXECUTABLE ${PROJECT_NAME}_node +#) +#ament_export_targets(export_${LIB_NAME}) +#install(TARGETS ${LIB_NAME} +# EXPORT export_${LIB_NAME} +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION bin +#) +install(TARGETS + velocity_controller_node + test_VC_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() + +#add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp) +#add_executable(test_VC_node src/test_VC.cpp src/PID_setup.cpp src/velocity_controller.cpp) + +#target_include_directories(velocity_controller_node PUBLIC +# $ +# $ +#) + +#target_link_libraries(velocity_controller_node ${drake_LIBRARIES}) +#install(DIRECTORY launch +# DESTINATION share/${PROJECT_NAME}/ +#) +#install(TARGETS velocity_controller_node +# DESTINATION lib/${PROJECT_NAME} +#) +#install(TARGETS test_VC_node +# DESTINATION lib/${PROJECT_NAME} +#) +#install(DIRECTORY config +# DESTINATION share/${PROJECT_NAME}/ +#) + + diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp index 10f42401..c9107646 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -2,6 +2,7 @@ #include #include +#include #include class PID_controller { @@ -30,3 +31,14 @@ class angle{ double psit=0.0; }; angle quaternion_to_euler_angle(double w, double x, double y, double z); + +class guidance_data{ + public: + double surge; double pitch; double yaw; + guidance_data(std_msgs::msg::Float64MultiArray msg); + guidance_data(double surge, double pitch, double yaw):surge(surge), pitch(pitch), yaw(yaw) {}; + guidance_data():surge(0), pitch(0), yaw(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/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp new file mode 100644 index 00000000..474f636a --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/velocity_controller.hpp" +#include +#include + +class test_VC : public rclcpp::Node{ + public: + test_VC(); + //Callback functions + void read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg); + void send_guidance(); + void send_state(); + + //Variables + //guidance_data reference; + guidance_data current_state; + //Subscribers and publishers + rclcpp::Publisher::SharedPtr publisher_guidance; + rclcpp::Publisher::SharedPtr publisher_twist; + rclcpp::Publisher::SharedPtr publisher_pose; + rclcpp::Subscription::SharedPtr subscription_thrust; + //Timers + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Clock::SharedPtr clock_; + //Messages + std::vector thrust_vector; + std_msgs::msg::Float64MultiArray reference_msg; + + //Topics + std::string topic_twist; + std::string topic_pose; + std::string topic_thrust; + std::string topic_guidance; + + //MSGS + geometry_msgs::msg::TwistWithCovarianceStamped twist_msg; + geometry_msgs::msg::PoseWithCovarianceStamped pose_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/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index 1d14a029..04422572 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -12,19 +12,13 @@ //#include "vortex-msgs/msg" kan legge til nye meldinger nå -class guidance_data{ - public: - double surge; double pitch; double yaw; - guidance_data(std_msgs::msg::Float64MultiArray msg); - guidance_data():surge(0.0), pitch(0.0), yaw(0.0) {}; - - guidance_data operator-(const guidance_data& other) const; - guidance_data& operator=(const std_msgs::msg::Float64MultiArray& msg); -}; + class Velocity_node : public rclcpp::Node{ public: Velocity_node(); + //Different initializatin functions + void get_new_parameters(); //Timer functions void publish_thrust(); diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 3c3e153e..c6ccfdec 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -2,61 +2,80 @@ #include #include #include -#include +#include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/test_VC.hpp" +#include +#include +//#include "velocity_controller/velocity_controller.hpp" //#include "LQR_setup.hpp" //Denne noden er kun for å teste velocity_controller noden -class test_VC : public rclcpp::Node{ - public: - test_VC() : Node("test_VC_node") - { - this->declare_parameter("topics.guidance_topic"); - topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); - publisher_ = this->create_publisher(topic_guidance,10); - this->declare_parameter("topics.thrust_topic"); - topic_subscription = this->get_parameter("topics.thrust_topic").as_string(); - subscription_ = this->create_subscription( - topic_subscription, 10, - std::bind(&test_VC::listen, this, std::placeholders::_1)); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&test_VC::send_velocity, this)); - clock_ = this->get_clock(); - thrust_pub = this->create_publisher("thrust_value", 10); +test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) +{ + this->declare_parameter("topics.guidance_topic"); + this->declare_parameter("topics.thrust_topic"); + this->declare_parameter("topics.twist_topic"); + this->declare_parameter("topics.pose_topic"); + topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); + topic_twist = this->get_parameter("topics.twist_topic").as_string(); + topic_pose = this->get_parameter("topics.pose_topic").as_string(); + + topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); + publisher_guidance = this->create_publisher(topic_guidance, 10); + publisher_twist = this->create_publisher(topic_twist,10); + publisher_pose = this->create_publisher(topic_pose,10); + + subscription_thrust = this->create_subscription( + topic_thrust, 10, + std::bind(&test_VC::read_thrust, this, std::placeholders::_1)); + 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.data={2.0, 0.0, 0.0}; //Surge, pitch, yaw + +} + +void test_VC::send_guidance() +{ + publisher_guidance->publish(reference_msg); + send_state(); +} + +void test_VC::read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg) +{ + current_state.surge += 0.01 * msg->wrench.force.x; + current_state.pitch += 0.01 * msg->wrench.torque.x; + current_state.yaw += 0.01 * msg->wrench.torque.y; + RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.surge); + RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.pitch); + RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.yaw); + return; +} - RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); +void test_VC::send_state() +{ + + twist_msg.header.stamp = clock_->now(); + twist_msg.header.frame_id = "base_link"; + twist_msg.twist.twist.linear.x = current_state.surge; - message.wrench.force.x=1; - } + pose_msg.header.stamp = clock_->now(); + pose_msg.header.frame_id = "base_link"; + pose_msg.pose.pose.orientation = euler_angle_to_quaternion(0.0, current_state.pitch, current_state.yaw); - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::Subscription::SharedPtr subscription_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Clock::SharedPtr clock_; - std::string topic_guidance; - std::string topic_subscription; - std::vector thrust_vector; - rclcpp::Publisher::SharedPtr thrust_pub; - geometry_msgs::msg::WrenchStamped message; + publisher_twist->publish(twist_msg); + publisher_pose->publish(pose_msg); - void send_velocity() - { - message.wrench.force.x = std::sin(clock_->now().seconds()*2*3.14159/10); //sinuskurve med periode 10 sekunder og amplitude 1 - publisher_->publish(message); - } + //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.surge); + return; + //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.pitch); + //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.yaw); +} - void listen(geometry_msgs::msg::WrenchStamped::SharedPtr msg) - { - thrust_vector.push_back(msg->wrench.force.x); - std_msgs::msg::Float64 pub_info; - pub_info.data = thrust_vector.back(); - thrust_pub->publish(pub_info); - message.wrench.force.x+=0.01*msg->wrench.force.x; - //RCLCPP_INFO(this->get_logger(), "Received thrust: '%f'", msg->wrench.force.x); - return; - } -}; int main(int argc, char const *argv[]) { @@ -65,3 +84,20 @@ int main(int argc, char const *argv[]) 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/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 42886eb8..0f38c89d 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -19,19 +19,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); //Parameter from config. - this->declare_parameter("topics.thrust_topic"); - this->declare_parameter("topics.guidance_topic"); - this->declare_parameter("topics.twist_topic"); - this->declare_parameter("topics.pose_topic"); - this->declare_parameter("topics.killswitch_topic"); - this->declare_parameter("max_force"); - this->max_force = this->get_parameter("max_force").as_double(); - this->topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); - this->topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); - this->topic_twist = this->get_parameter("topics.twist_topic").as_string(); - this->topic_killswitch = this->get_parameter("topics.killswitch_topic").as_string(); - this->topic_pose = this->get_parameter("topics.pose_topic").as_string(); - + get_new_parameters(); // Publishers publisher_thrust = create_publisher(topic_thrust, 10); @@ -52,16 +40,15 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 std::bind(&Velocity_node::killswitch_callback,this, std::placeholders::_1)); //Timer - this->declare_parameter("calculation_rate"); - this->declare_parameter("publish_rate"); - this->calculation_rate = this->get_parameter("calculation_rate").as_int(); - this->publish_rate = this->get_parameter("publish_rate").as_int(); + timer_calculation = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); 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); + this->calculation_rate = this->get_parameter("calculation_rate").as_int(); + this->publish_rate = this->get_parameter("publish_rate").as_int(); } @@ -115,6 +102,27 @@ void Velocity_node::killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg } 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.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(); +} + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); From 40eb88fa0d90d3d40e22d8838e56bbf6dcf4e8cf Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 19 Oct 2025 17:07:37 +0200 Subject: [PATCH 11/22] fixed code so that it likely could compile if I had Drake. Made a Utility file --- control/velocity_controller/CMakeLists.txt | 7 ++ .../config/parameters.yaml | 7 +- .../include/velocity_controller/LQR_setup.hpp | 51 +++++++----- .../include/velocity_controller/PID_setup.hpp | 18 +---- .../include/velocity_controller/test_VC.hpp | 2 +- .../include/velocity_controller/utilities.hpp | 31 +++++++ .../velocity_controller.hpp | 13 ++- control/velocity_controller/src/LQR_setup.cpp | 21 +++-- control/velocity_controller/src/PID_setup.cpp | 20 +---- control/velocity_controller/src/utilities.cpp | 21 +++++ .../src/velocity_controller.cpp | 81 +++++++++++++++---- 11 files changed, 182 insertions(+), 90 deletions(-) create mode 100644 control/velocity_controller/include/velocity_controller/utilities.hpp create mode 100644 control/velocity_controller/src/utilities.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 424df4ba..2f2ea04c 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -24,6 +24,8 @@ include_directories( include ) +link_directories("/opt/drake/lib/") + #set(LIB_NAME "${PROJECT_NAME}_component") #add_library(${LIB_NAME} SHARED @@ -35,11 +37,15 @@ include_directories( 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 # src/velocity_controller.cpp ) @@ -49,6 +55,7 @@ ament_target_dependencies(velocity_controller_node vortex_msgs geometry_msgs Eigen3 + drake ) ament_target_dependencies(test_VC_node diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 5be2714b..bcdd2a15 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -11,9 +11,9 @@ killswitch_topic: /softwareKillSwitch #Kill Switch LQR_params: - q_surge: 75 - q_pitch: 175 - q_yaw: 175 + q_surge: 75.0 + q_pitch: 175.0 + q_yaw: 175.0 r_surge: 0.3 r_pitch: 0.4 @@ -33,4 +33,5 @@ publish_rate: 200 #ms #Clamp parameter max_force: 99.5 + controller_type: 1 #publish_value: "Hello from config!" # parameter name: parameter value diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index c13e5444..4fa1fc02 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -3,23 +3,19 @@ #include #include #include +#include "PID_setup.hpp" +#include +#include "velocity_controller/utilities.hpp" -class State{ - //Dataclass to store state 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 Guidance_values{ +/*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: @@ -29,30 +25,38 @@ class LQRparameters{ double i_weight=0.0; double max_force=0.0; }; -class angle{ +/*class angle{ public: double phit=0.0; double thetat=0.0; double psit=0.0; -}; + +};*/ class LQRController{ public: - LQRController(LQRparameters params,std::vector inertia_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); - std::vector> calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); + LQRController(LQRparameters params={0,0,0,0,0,0,0,0,0,0,0},std::vector inertia_matrix={0,0,0,0,0,0,0,0,0}); + + void set_params(LQRparameters params); + std::vector> calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); void set_matrices(std::vector inertia_matrix); void update_augmented_matrices(std::vector > coriolis_matrix); - std::vector update_error(Guidance_values guidance_values, State states); + + //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); std::vector saturate_input(std::vector u); - std::vector calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_values guidance_values); + + std::vector update_error(Guidance_data guidance_values, State states); + std::vector calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_data guidance_values); + + //Resets controller void reset_controller(); - // Variables + // 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; @@ -66,5 +70,12 @@ class LQRController{ std::vector> input_weight_matrix; std::vector> augmented_system_matrix; std::vector> 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 index c9107646..91303d5d 100644 --- a/control/velocity_controller/include/velocity_controller/PID_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -4,6 +4,7 @@ #include #include #include +#include "utilities.hpp" class PID_controller { public: @@ -24,21 +25,4 @@ class PID_controller { double max_output; double min_output; }; -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 guidance_data{ - public: - double surge; double pitch; double yaw; - guidance_data(std_msgs::msg::Float64MultiArray msg); - guidance_data(double surge, double pitch, double yaw):surge(surge), pitch(pitch), yaw(yaw) {}; - guidance_data():surge(0), pitch(0), yaw(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/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index 474f636a..5e6c8c3d 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -20,7 +20,7 @@ class test_VC : public rclcpp::Node{ //Variables //guidance_data reference; - guidance_data current_state; + Guidance_data current_state; //Subscribers and publishers rclcpp::Publisher::SharedPtr publisher_guidance; rclcpp::Publisher::SharedPtr publisher_twist; 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..82806dad --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -0,0 +1,31 @@ +#pragma once +#include +#include +#include "std_msgs/msg/float64_multi_array.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; double pitch=0.0; double yaw=0.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(std_msgs::msg::Float64MultiArray msg); + 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 index 04422572..d9849df7 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -8,6 +8,7 @@ #include #include #include "velocity_controller/PID_setup.hpp" +#include "LQR_setup.hpp" //#include "vortex-msgs/msg" kan legge til nye meldinger nå @@ -58,15 +59,23 @@ class Velocity_node : public rclcpp::Node{ //Stored wrenches values std_msgs::msg::Float64MultiArray reference_in; - guidance_data reference; - guidance_data current_state; + 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; + }; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 84d94c60..3025d6b5 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -8,6 +8,8 @@ #include #include #include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/utilities.hpp" LQRController::LQRController(LQRparameters params,std::vector inertia_matrix){ @@ -15,7 +17,8 @@ LQRController::LQRController(LQRparameters params,std::vector inertia_ma set_matrices(inertia_matrix); }; -angle LQRController::quaternion_to_euler_angle(double w, double x, double y, double z){ + +/*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); @@ -32,7 +35,7 @@ angle LQRController::quaternion_to_euler_angle(double w, double x, double y, dou double psi = std::atan2(t3, t4); return {phi, theta, psi}; -}; +};*/ double LQRController::ssa(double angle){ return std::fmod(angle+pi, 2*pi)-pi; @@ -109,10 +112,10 @@ void LQRController::update_augmented_matrices(std::vector > {inertia_matrix_inv[2][0],inertia_matrix_inv[2][1],inertia_matrix_inv[2][2],0,0,0}}; }; -std::vector LQRController::update_error(Guidance_values guidance_values, State states){ +std::vector LQRController::update_error(Guidance_data guidance_values, State states){ double surge_error = guidance_values.surge - states.surge; double pitch_error = ssa(guidance_values.pitch - states.pitch); - double yaw_error = ssa(guidance_values.yaw - states.yaw); + double yaw_error = 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); @@ -128,7 +131,7 @@ std::vector LQRController::saturate_input(std::vector u){ std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); return {force_x, torque_y, torque_z}; } -std::vector LQRController::calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_values guidance_values){ +std::vector LQRController::calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_data guidance_values){ update_augmented_matrices(coriolis_matrix); auto result = drake::systems::controllers::LinearQuadraticRegulator( vector2d_to_matrix3d(augmented_system_matrix), @@ -136,7 +139,7 @@ std::vector LQRController::calculate_lqr_u(std::vector state_error = update_error(guidance_values, states); - std::vector u= saturate_input(matrix3d_to_vector(- result * vector_to_matrix3d(state_error))); + std::vector u= saturate_input(matrix3d_to_vector(- (result.K * vector_to_matrix3d(state_error)))); return u; } void LQRController::reset_controller(){ @@ -152,12 +155,6 @@ void LQRController::reset_controller(){ - - -int main(){ - return 0; -}; - //Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ Eigen::Matrix3d mat; diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index 74c4fe33..cf064115 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -1,4 +1,6 @@ #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; @@ -48,21 +50,3 @@ void PID_controller::set_output_limits(double min_output, double max_output){ return; }; -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/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 index 0f38c89d..ae4893c9 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -8,18 +8,21 @@ #include "std_msgs/msg/bool.hpp" #include "velocity_controller/PID_setup.hpp" #include +#include //#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(1,1,1), PID_yaw(1,1,1), PID_pitch(1,1,1) +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1,1), PID_yaw(1,1,1), PID_pitch(1,1,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 publisher_thrust = create_publisher(topic_thrust, 10); @@ -43,12 +46,14 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 timer_calculation = this->create_wall_timer(std::chrono::milliseconds(calculation_rate), std::bind(&Velocity_node::publish_thrust, this)); timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_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); - this->calculation_rate = this->get_parameter("calculation_rate").as_int(); - this->publish_rate = this->get_parameter("publish_rate").as_int(); + lqr_controller.set_params(lqr_parameters); + lqr_controller.set_matrices(inertia_matrix); + + } @@ -62,12 +67,22 @@ void Velocity_node::publish_thrust() //** må forbedre integrasjon og derivasjons beregningene void Velocity_node::calc_thrust() { - PID_surge.calculate_thrust(reference.surge, current_state.surge,calculation_rate/1000.0); - PID_pitch.calculate_thrust(reference.pitch, current_state.pitch,calculation_rate/1000.0); - PID_yaw.calculate_thrust(reference.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(); + switch (controller_type) + { + case 1: + 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: + lqr_controller.update_error(guidance_values,current_state); + default: + break; + } + return; } @@ -77,7 +92,7 @@ void Velocity_node::calc_thrust() //Callback functions void Velocity_node::guidance_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg_ptr){ //RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); - reference = *msg_ptr; + guidance_values = *msg_ptr; return; } void Velocity_node::twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr){ @@ -96,8 +111,8 @@ void Velocity_node::pose_callback(const geometry_msgs::msg::PoseWithCovarianceSt 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){ - reference = guidance_data(); - current_state = guidance_data(); + guidance_values = Guidance_data(); + current_state = Guidance_data(); RCLCPP_INFO(this->get_logger(), "Killswitch activated, reference and current state set to zero"); } return; @@ -121,6 +136,38 @@ void Velocity_node::get_new_parameters(){ 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[]) @@ -156,16 +203,16 @@ geometry_msgs::msg::WrenchStamped operator+(const geometry_msgs::msg::WrenchStam return result; } //operator overloading for guidance_data -guidance_data guidance_data::operator-(const guidance_data & b) const +Guidance_data Guidance_data::operator-(const Guidance_data & b) const { - guidance_data result; + 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) +Guidance_data& Guidance_data::operator=(const std_msgs::msg::Float64MultiArray& msg) { if (msg.data.size()>=3){ surge=msg.data[0]; @@ -179,7 +226,7 @@ guidance_data& guidance_data::operator=(const std_msgs::msg::Float64MultiArray& return *this; } -guidance_data::guidance_data(std_msgs::msg::Float64MultiArray msg){ +Guidance_data::Guidance_data(std_msgs::msg::Float64MultiArray msg){ if (msg.data.size()>=3){ surge=msg.data[0]; pitch=msg.data[1]; From 9141eca39ffbd505c01b408bdfb08c2822b939bb Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 22 Oct 2025 11:59:53 +0200 Subject: [PATCH 12/22] Removed Drake, Changed from std::vector to Eigen in LQR --- control/velocity_controller/CMakeLists.txt | 5 +- .../include/velocity_controller/LQR_setup.hpp | 31 +++++--- control/velocity_controller/src/LQR_setup.cpp | 79 +++++++++---------- .../src/velocity_controller.cpp | 2 +- 4 files changed, 60 insertions(+), 57 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 2f2ea04c..69195bdd 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -9,6 +9,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(DRAKE_DIR /ros2_ws_v/src/drake-build/install/lib/cmake/drake) # find dependencies find_package(ament_cmake REQUIRED) @@ -16,7 +17,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(Eigen3 REQUIRED) -find_package(drake REQUIRED) +#find_package(drake CONFIG REQUIRED) include_directories( ${EIGEN3_INCLUDE_DIR} @@ -55,7 +56,7 @@ ament_target_dependencies(velocity_controller_node vortex_msgs geometry_msgs Eigen3 - drake + #drake ) ament_target_dependencies(test_VC_node diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 4fa1fc02..e3131d56 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -32,26 +32,33 @@ class LQRparameters{ double psit=0.0; };*/ +template +struct LQRsolveResult{ + Eigen::Matrix K; + Eigen::Matrix P; +}; class LQRController{ public: - LQRController(LQRparameters params={0,0,0,0,0,0,0,0,0,0,0},std::vector inertia_matrix={0,0,0,0,0,0,0,0,0}); + 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); - std::vector> calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel); - void set_matrices(std::vector inertia_matrix); - void update_augmented_matrices(std::vector > coriolis_matrix); + 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); - std::vector saturate_input(std::vector u); + Eigen::Vector saturate_input(Eigen::Vector u); - std::vector update_error(Guidance_data guidance_values, State states); - std::vector calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_data guidance_values); + Eigen::Vector update_error(Guidance_data guidance_values, State states); + Eigen::Vector calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values); + template + LQRsolveResult<6,6> solve_k_p(Eigen::Matrix A,Eigen::Matrix B,Eigen::Matrix R, Eigen::Matrix Q); //Resets controller void reset_controller(); @@ -65,11 +72,11 @@ class LQRController{ double i_surge; double i_pitch; double i_yaw; double i_weight; double max_force; - std::vector> inertia_matrix_inv; - std::vector> state_weight_matrix; - std::vector> input_weight_matrix; - std::vector> augmented_system_matrix; - std::vector> augmented_input_matrix; + 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; }; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 3025d6b5..d7f7615d 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -4,15 +4,15 @@ #include #include #include -#include -#include -#include -#include +//#include +//#include +//#include +//#include #include "velocity_controller/PID_setup.hpp" #include "velocity_controller/utilities.hpp" -LQRController::LQRController(LQRparameters params,std::vector inertia_matrix){ +LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix){ set_params(params); set_matrices(inertia_matrix); }; @@ -62,11 +62,13 @@ double LQRController::anti_windup(double ki, double error, double integral_sum, return integral_sum; } -std::vector> LQRController::calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel){ +Eigen::Matrix3d LQRController::calculate_coriolis_matrix(double pitchrate, double yaw_rate, double sway_vel, double heave_vel){ //Inertia matrix values?? - return {{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}}; + 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; } @@ -81,38 +83,29 @@ void LQRController::set_params(LQRparameters params){ return; } -void LQRController::set_matrices(std::vector inertia_matrix){ - Eigen::Matrix3d mat= vector_to_matrix3d(inertia_matrix); - inertia_matrix_inv = matrix3d_to_vector2d(mat.inverse()); - state_weight_matrix = {{q_surge,0,0,0,0,0}, - {0,q_pitch,0,0,0,0}, - {0,0,q_yaw,0,0,0}, - {0,0,0,i_weight,0,0}, - {0,0,0,0,i_weight,0}, - {0,0,0,0,0,i_weight}}; - input_weight_matrix = {{r_surge,0,0}, - {0,r_pitch,0}, - {0,0,r_yaw}}; - +void LQRController::set_matrices(Eigen::Matrix3d inertia_matrix){ + inertia_matrix_inv = inertia_matrix.inverse(); + state_weight_matrix.diagonal() <> coriolis_matrix){ - std::vector> system_matrix = matrix3d_to_vector2d(vector2d_to_matrix3d(inertia_matrix_inv) * vector2d_to_matrix3d(coriolis_matrix)); +void LQRController::update_augmented_matrices(Eigen::Matrix3d coriolis_matrix){ + Eigen::Matrix3d system_matrix = inertia_matrix_inv*coriolis_matrix; //input_matrix = inertia_matrix_inv; - augmented_system_matrix = {{system_matrix[0][0],system_matrix[0][1],system_matrix[0][2],0,0,0}, - {system_matrix[1][0],system_matrix[1][1],system_matrix[1][2],0,0,0}, - {system_matrix[2][0],system_matrix[2][1],system_matrix[2][2],0,0,0}, - {-1,0,0,0,0,0}, - {0,-1,0,0,0,0}, - {0,0,-1,0,0,0}}; //Skal det være -1 her? - augmented_input_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}}; - + augmented_system_matrix < LQRController::update_error(Guidance_data guidance_values, State states){ +Eigen::Vector LQRController::update_error(Guidance_data guidance_values, State states){ double surge_error = guidance_values.surge - states.surge; double pitch_error = ssa(guidance_values.pitch - states.pitch); double yaw_error = ssa(guidance_values.yaw - states.yaw); @@ -121,25 +114,27 @@ std::vector LQRController::update_error(Guidance_data guidance_values, S 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); - std::vector state_error= {-surge_error, -pitch_error, -yaw_error, integral_error_surge, integral_error_pitch, integral_error_yaw}; + Eigen::Vector state_error= {-surge_error, -pitch_error, -yaw_error, integral_error_surge, integral_error_pitch, integral_error_yaw}; return state_error; } -std::vector LQRController::saturate_input(std::vector u){ +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}; } -std::vector LQRController::calculate_lqr_u(std::vector> coriolis_matrix, State states, Guidance_data guidance_values){ +Eigen::Vector LQRController::calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values){ update_augmented_matrices(coriolis_matrix); - auto result = drake::systems::controllers::LinearQuadraticRegulator( + + Eigen::Matrix result=Eigen::Matrix::Identity(); + /*auto result = drake::systems::controllers::LinearQuadraticRegulator( vector2d_to_matrix3d(augmented_system_matrix), vector2d_to_matrix3d(augmented_input_matrix), vector2d_to_matrix3d(state_weight_matrix), - vector2d_to_matrix3d(input_weight_matrix)); - std::vector state_error = update_error(guidance_values, states); - std::vector u= saturate_input(matrix3d_to_vector(- (result.K * vector_to_matrix3d(state_error)))); + vector2d_to_matrix3d(input_weight_matrix));*/ + Eigen::Vector state_error = update_error(guidance_values, states); + Eigen::Vector u= saturate_input(- (result*state_error)); return u; } void LQRController::reset_controller(){ diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index ae4893c9..dcb2c710 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -51,7 +51,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 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(inertia_matrix); + lqr_controller.set_matrices(vector_to_matrix3d(inertia_matrix)); } From 70197875803822c3d6217a952aec1af9099255f7 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 26 Oct 2025 17:18:28 +0100 Subject: [PATCH 13/22] Implemented LQR solver, SLICOT call and some dimension mistakes --- control/velocity_controller/CMakeLists.txt | 53 ++++++++------- .../include/velocity_controller/LQR_setup.hpp | 15 +++-- control/velocity_controller/src/LQR_setup.cpp | 64 +++++++++++++++---- 3 files changed, 89 insertions(+), 43 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 69195bdd..f2429958 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -9,7 +9,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(DRAKE_DIR /ros2_ws_v/src/drake-build/install/lib/cmake/drake) # find dependencies find_package(ament_cmake REQUIRED) @@ -17,15 +16,18 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) find_package(Eigen3 REQUIRED) -#find_package(drake CONFIG REQUIRED) +find_package(CasADi REQUIRED) +find_package(LAPACK REQUIRED) +find_package(BLAS REQUIRED) +find_package(geometry_msgs REQUIRED) + include_directories( ${EIGEN3_INCLUDE_DIR} - ${drake_INCLUDE_DIRS} + include ) -link_directories("/opt/drake/lib/") #set(LIB_NAME "${PROJECT_NAME}_component") @@ -47,7 +49,6 @@ add_executable(test_VC_node src/PID_setup.cpp src/LQR_setup.cpp src/utilities.cpp -# src/velocity_controller.cpp ) ament_target_dependencies(velocity_controller_node @@ -56,7 +57,8 @@ ament_target_dependencies(velocity_controller_node vortex_msgs geometry_msgs Eigen3 - #drake + CasADi + ) ament_target_dependencies(test_VC_node @@ -67,30 +69,22 @@ ament_target_dependencies(test_VC_node Eigen3 ) -install(TARGETS - velocity_controller_node - test_VC_node - DESTINATION lib/${PROJECT_NAME} -) +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}) +target_link_libraries(test_VC_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) -#rclcpp_components_register_node( -# ${LIB_NAME} -# PLUGIN "velocity_controller_node" -# EXECUTABLE ${PROJECT_NAME}_node -#) -#ament_export_targets(export_${LIB_NAME}) -#install(TARGETS ${LIB_NAME} -# EXPORT export_${LIB_NAME} -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION bin -#) install(TARGETS velocity_controller_node test_VC_node DESTINATION lib/${PROJECT_NAME} ) + + + install( DIRECTORY include/ DESTINATION include @@ -138,4 +132,15 @@ ament_package() # DESTINATION share/${PROJECT_NAME}/ #) - +#rclcpp_components_register_node( +# ${LIB_NAME} +# PLUGIN "velocity_controller_node" +# EXECUTABLE ${PROJECT_NAME}_node +#) +#ament_export_targets(export_${LIB_NAME}) +#install(TARGETS ${LIB_NAME} +# EXPORT export_${LIB_NAME} +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION bin +#) diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index e3131d56..5a4bb2a9 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -32,10 +32,11 @@ class LQRparameters{ double psit=0.0; };*/ -template + struct LQRsolveResult{ - Eigen::Matrix K; - Eigen::Matrix P; + Eigen::Matrix K; + Eigen::Matrix P; + LQRsolveResult(Eigen::Matrix K,Eigen::Matrix P):K(K),P(P){}; }; class LQRController{ @@ -53,12 +54,12 @@ class LQRController{ 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 saturate_input(Eigen::Vector u); Eigen::Vector update_error(Guidance_data guidance_values, State states); Eigen::Vector calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values); - template - LQRsolveResult<6,6> solve_k_p(Eigen::Matrix A,Eigen::Matrix B,Eigen::Matrix R, Eigen::Matrix Q); + + LQRsolveResult solve_k_p(Eigen::Matrix A,Eigen::Matrix B,Eigen::Matrix Q, Eigen::Matrix R); //Resets controller void reset_controller(); @@ -76,7 +77,7 @@ class LQRController{ Eigen::Matrix state_weight_matrix; Eigen::Matrix3d input_weight_matrix; Eigen::Matrix augmented_system_matrix; - Eigen::Matrix augmented_input_matrix; + Eigen::Matrix augmented_input_matrix; }; diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index d7f7615d..dcfb8e51 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -1,5 +1,5 @@ #include "velocity_controller/LQR_setup.hpp" -#include +#include "rclcpp/rclcpp.hpp" #include #include #include @@ -10,7 +10,8 @@ //#include #include "velocity_controller/PID_setup.hpp" #include "velocity_controller/utilities.hpp" - +#include +#include LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix){ set_params(params); @@ -117,7 +118,7 @@ Eigen::Vector LQRController::update_error(Guidance_data guidance_value 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){ +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); @@ -126,15 +127,9 @@ Eigen::Vector LQRController::saturate_input(Eigen::Vector u) } Eigen::Vector LQRController::calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values){ update_augmented_matrices(coriolis_matrix); - - Eigen::Matrix result=Eigen::Matrix::Identity(); - /*auto result = drake::systems::controllers::LinearQuadraticRegulator( - vector2d_to_matrix3d(augmented_system_matrix), - vector2d_to_matrix3d(augmented_input_matrix), - vector2d_to_matrix3d(state_weight_matrix), - vector2d_to_matrix3d(input_weight_matrix));*/ - Eigen::Vector state_error = update_error(guidance_values, states); - Eigen::Vector u= saturate_input(- (result*state_error)); + LQRsolveResult result = solve_k_p(augmented_system_matrix,augmented_input_matrix,state_weight_matrix,input_weight_matrix); + Eigen::Matrix state_error = update_error(guidance_values, states); + Eigen::Vector u= saturate_input(- (result.K*state_error)); return u; } void LQRController::reset_controller(){ @@ -149,6 +144,51 @@ void LQRController::reset_controller(){ } +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 +); +} + + +LQRsolveResult LQRController::solve_k_p(Eigen::Matrix A,Eigen::Matrix B, Eigen::Matrix Q,Eigen::Matrix R){ + //First calculate G with sb02mt_ + char JOBG='G'; //calculate G + char JOBL='Z'; //L is zero + char FACT='N'; //unfactored R + char UPLO='U'; //Upper triangle i think + const int N=6; //Order of matrices A, Q, G and X(P) + const int M=3; //Order of matrix R and nuber of columns in B and L(is zero) + int LDA=N, LDB=M, LDQ=N,LDR=M,LDL=N,LDG=N; + + std::vector IWORK(N); + int LDWORK=10*N*N; //Upper bounds + std::vector DWORK(LDWORK); + std::vector IPIV(N); + int OUFACT=0; //Output but initialized JIC + Eigen::Matrix L=Eigen::Matrix::Zero(); + Eigen::Matrix G; + int INFO; + sb02mt_(&JOBG,&JOBL,&FACT,&UPLO,&N,&M,A.data(),&LDA,B.data(),&LDB,Q.data(),&LDQ,R.data(),&LDR,L.data(),&LDL,IPIV.data(),&OUFACT,G.data(),&LDG,IWORK.data(),DWORK.data(),&LDWORK,&INFO); + + if (INFO!=0){ + //Some Error handling here. Also check that BRB in invertible + } + Eigen::Matrix K; + Eigen::Matrix BRB = R+B.transpose()*G*B; + K=BRB.inverse()*B.transpose()*G*A; + + return LQRsolveResult(K,G); + +} + + + //Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ From 3afca9745957f5e964361e645e7714e54fc46c44 Mon Sep 17 00:00:00 2001 From: Henrik Date: Sun, 2 Nov 2025 16:18:36 +0100 Subject: [PATCH 14/22] Fixed LQR_solve_k_p --- control/velocity_controller/CMakeLists.txt | 69 +++++----------- .../include/velocity_controller/LQR_setup.hpp | 7 +- .../include/velocity_controller/test_VC.hpp | 9 +-- .../include/velocity_controller/utilities.hpp | 5 +- .../velocity_controller.hpp | 26 +++--- control/velocity_controller/package.xml | 1 + control/velocity_controller/src/LQR_setup.cpp | 78 ++++++++++++------ control/velocity_controller/src/LQR_test.cpp | 45 +++++++++++ control/velocity_controller/src/test_VC.cpp | 42 +++++----- .../src/velocity_controller.cpp | 79 ++++++++++--------- 10 files changed, 205 insertions(+), 156 deletions(-) create mode 100644 control/velocity_controller/src/LQR_test.cpp diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index f2429958..228f188b 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -1,9 +1,7 @@ cmake_minimum_required(VERSION 3.8) project(velocity_controller) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) -endif() +set(CMAKE_CXX_STANDARD 20) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -20,23 +18,14 @@ find_package(CasADi REQUIRED) find_package(LAPACK REQUIRED) find_package(BLAS REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) include_directories( ${EIGEN3_INCLUDE_DIR} - include ) - -#set(LIB_NAME "${PROJECT_NAME}_component") - -#add_library(${LIB_NAME} SHARED -# src/LQR_setup.cpp -# src/PID_setup.cpp -# src/test_VC.cpp -# src/velocity_controller.cpp -#) add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp @@ -50,7 +39,11 @@ add_executable(test_VC_node 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 @@ -58,6 +51,7 @@ ament_target_dependencies(velocity_controller_node geometry_msgs Eigen3 CasADi + nav_msgs ) @@ -67,6 +61,16 @@ ament_target_dependencies(test_VC_node vortex_msgs geometry_msgs Eigen3 + nav_msgs +) + +ament_target_dependencies(test_LQR_node + rclcpp + Eigen3 + vortex_msgs + geometry_msgs + std_msgs + nav_msgs ) link_directories(/usr/lib/gcc/x86_64-linux-gnu/11) @@ -75,11 +79,13 @@ 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}) target_link_libraries(test_VC_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) +target_link_libraries(test_LQR_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) install(TARGETS velocity_controller_node test_VC_node + test_LQR_node DESTINATION lib/${PROJECT_NAME} ) @@ -109,38 +115,3 @@ if(BUILD_TESTING) endif() ament_package() - -#add_executable(velocity_controller_node src/velocity_controller.cpp src/PID_setup.cpp) -#add_executable(test_VC_node src/test_VC.cpp src/PID_setup.cpp src/velocity_controller.cpp) - -#target_include_directories(velocity_controller_node PUBLIC -# $ -# $ -#) - -#target_link_libraries(velocity_controller_node ${drake_LIBRARIES}) -#install(DIRECTORY launch -# DESTINATION share/${PROJECT_NAME}/ -#) -#install(TARGETS velocity_controller_node -# DESTINATION lib/${PROJECT_NAME} -#) -#install(TARGETS test_VC_node -# DESTINATION lib/${PROJECT_NAME} -#) -#install(DIRECTORY config -# DESTINATION share/${PROJECT_NAME}/ -#) - -#rclcpp_components_register_node( -# ${LIB_NAME} -# PLUGIN "velocity_controller_node" -# EXECUTABLE ${PROJECT_NAME}_node -#) -#ament_export_targets(export_${LIB_NAME}) -#install(TARGETS ${LIB_NAME} -# EXPORT export_${LIB_NAME} -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION bin -#) diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 5a4bb2a9..9b39f474 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -36,7 +36,8 @@ class LQRparameters{ struct LQRsolveResult{ Eigen::Matrix K; Eigen::Matrix P; - LQRsolveResult(Eigen::Matrix K,Eigen::Matrix P):K(K),P(P){}; + int INFO=0; + LQRsolveResult(Eigen::Matrix K,Eigen::Matrix P, int INFO=0):K(K),P(P),INFO(INFO) {}; }; class LQRController{ @@ -57,9 +58,9 @@ class LQRController{ Eigen::Vector saturate_input(Eigen::Vector u); Eigen::Vector update_error(Guidance_data guidance_values, State states); - Eigen::Vector calculate_lqr_u(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values); + Eigen::Vector calculate_lqr_u(State states, Guidance_data guidance_values); - LQRsolveResult solve_k_p(Eigen::Matrix A,Eigen::Matrix B,Eigen::Matrix Q, Eigen::Matrix R); + 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(); diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index 5e6c8c3d..c8974d09 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -23,8 +23,7 @@ class test_VC : public rclcpp::Node{ Guidance_data current_state; //Subscribers and publishers rclcpp::Publisher::SharedPtr publisher_guidance; - rclcpp::Publisher::SharedPtr publisher_twist; - rclcpp::Publisher::SharedPtr publisher_pose; + rclcpp::Publisher::SharedPtr publisher_odom; rclcpp::Subscription::SharedPtr subscription_thrust; //Timers rclcpp::TimerBase::SharedPtr timer_; @@ -34,14 +33,12 @@ class test_VC : public rclcpp::Node{ std_msgs::msg::Float64MultiArray reference_msg; //Topics - std::string topic_twist; - std::string topic_pose; + std::string topic_odom; std::string topic_thrust; std::string topic_guidance; //MSGS - geometry_msgs::msg::TwistWithCovarianceStamped twist_msg; - geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; + 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 index 82806dad..c6627fa1 100644 --- a/control/velocity_controller/include/velocity_controller/utilities.hpp +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -2,6 +2,7 @@ #include #include #include "std_msgs/msg/float64_multi_array.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" class angle{ @@ -15,14 +16,14 @@ 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; double pitch=0.0; double yaw=0.0; + 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(std_msgs::msg::Float64MultiArray msg); + 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} {}; diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index d9849df7..e559297e 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -9,9 +9,8 @@ #include #include "velocity_controller/PID_setup.hpp" #include "LQR_setup.hpp" - - -//#include "vortex-msgs/msg" kan legge til nye meldinger nå +#include "nav_msgs/msg/odometry.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" @@ -26,10 +25,11 @@ class Velocity_node : public rclcpp::Node{ void calc_thrust(); //Callback functions - void guidance_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg_ptr); + 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 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; @@ -39,9 +39,10 @@ class Velocity_node : public rclcpp::Node{ rclcpp::TimerBase::SharedPtr timer_publish; //Subscriber instance - rclcpp::Subscription::SharedPtr subscriber_twist; - rclcpp::Subscription::SharedPtr subscriber_pose; - rclcpp::Subscription::SharedPtr subscriber_guidance; + //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 @@ -49,8 +50,9 @@ class Velocity_node : public rclcpp::Node{ std::string topic_thrust; std::string topic_guidance; std::string topic_killswitch; - std::string topic_twist; - std::string topic_pose; + //std::string topic_twist; + //std::string topic_pose; + std::string topic_odometry; //Variables for timers int calculation_rate; @@ -58,7 +60,7 @@ class Velocity_node : public rclcpp::Node{ double max_force; //Stored wrenches values - std_msgs::msg::Float64MultiArray reference_in; + vortex_msgs::msg::LOSGuidance reference_in; Guidance_data guidance_values; Guidance_data current_state; geometry_msgs::msg::WrenchStamped thrust_out; diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml index 87c5c268..3f116b99 100644 --- a/control/velocity_controller/package.xml +++ b/control/velocity_controller/package.xml @@ -13,6 +13,7 @@ std_msgs vortex_msgs geometry_msgs + nav_msgs ament_lint_auto ament_lint_common diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index dcfb8e51..441f1781 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -125,10 +125,13 @@ Eigen::Vector LQRController::saturate_input(Eigen::Vector u) 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(Eigen::Matrix3d coriolis_matrix, State states, Guidance_data guidance_values){ - update_augmented_matrices(coriolis_matrix); +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); - Eigen::Matrix state_error = update_error(guidance_values, states); + 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; } @@ -153,37 +156,60 @@ void sb02mt_( 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(Eigen::Matrix A,Eigen::Matrix B, Eigen::Matrix Q,Eigen::Matrix R){ +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_ - char JOBG='G'; //calculate G - char JOBL='Z'; //L is zero - char FACT='N'; //unfactored R - char UPLO='U'; //Upper triangle i think - const int N=6; //Order of matrices A, Q, G and X(P) - const int M=3; //Order of matrix R and nuber of columns in B and L(is zero) - int LDA=N, LDB=M, LDQ=N,LDR=M,LDL=N,LDG=N; - - std::vector IWORK(N); - int LDWORK=10*N*N; //Upper bounds + //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); - std::vector IPIV(N); - int OUFACT=0; //Output but initialized JIC - Eigen::Matrix L=Eigen::Matrix::Zero(); - Eigen::Matrix G; - int INFO; - sb02mt_(&JOBG,&JOBL,&FACT,&UPLO,&N,&M,A.data(),&LDA,B.data(),&LDB,Q.data(),&LDQ,R.data(),&LDR,L.data(),&LDL,IPIV.data(),&OUFACT,G.data(),&LDG,IWORK.data(),DWORK.data(),&LDWORK,&INFO); - + Eigen::Matrix L=Eigen::Matrix::Zero(), G=L; + 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){ //Some Error handling here. Also check that BRB in invertible + 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; + 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::Matrix K; - Eigen::Matrix BRB = R+B.transpose()*G*B; - K=BRB.inverse()*B.transpose()*G*A; - - return LQRsolveResult(K,G); + 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); } diff --git a/control/velocity_controller/src/LQR_test.cpp b/control/velocity_controller/src/LQR_test.cpp new file mode 100644 index 00000000..108c2354 --- /dev/null +++ b/control/velocity_controller/src/LQR_test.cpp @@ -0,0 +1,45 @@ +#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/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index c6ccfdec..56717470 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -15,16 +15,12 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) { this->declare_parameter("topics.guidance_topic"); this->declare_parameter("topics.thrust_topic"); - this->declare_parameter("topics.twist_topic"); - this->declare_parameter("topics.pose_topic"); + this->declare_parameter("topics.odom_topic"); topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); - topic_twist = this->get_parameter("topics.twist_topic").as_string(); - topic_pose = this->get_parameter("topics.pose_topic").as_string(); - + topic_odom = this->get_parameter("topics.odom_topic").as_string(); topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); publisher_guidance = this->create_publisher(topic_guidance, 10); - publisher_twist = this->create_publisher(topic_twist,10); - publisher_pose = this->create_publisher(topic_pose,10); + publisher_odom = this->create_publisher(topic_odom,10); subscription_thrust = this->create_subscription( topic_thrust, 10, @@ -46,28 +42,34 @@ void test_VC::send_guidance() void test_VC::read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg) { - current_state.surge += 0.01 * msg->wrench.force.x; + /*current_state.surge += 0.01 * msg->wrench.force.x; current_state.pitch += 0.01 * msg->wrench.torque.x; - current_state.yaw += 0.01 * msg->wrench.torque.y; - RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.surge); - RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.pitch); - RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.yaw); + current_state.yaw += 0.01 * msg->wrench.torque.y;*/ + //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.surge); + //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.pitch); + //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.yaw); return; } void test_VC::send_state() { - twist_msg.header.stamp = clock_->now(); - twist_msg.header.frame_id = "base_link"; - twist_msg.twist.twist.linear.x = current_state.surge; + odom_msg.header.stamp = clock_->now(); + odom_msg.header.frame_id = "base_link"; + odom_msg.twist.twist.linear.x = current_state.surge; + odom_msg.pose.pose.orientation = euler_angle_to_quaternion(0.0, current_state.pitch, current_state.yaw); + odom_msg.twist.twist.linear.y=1; + odom_msg.twist.twist.linear.z=1; + odom_msg.twist.twist.angular.x=1; + odom_msg.twist.twist.angular.y=1; + odom_msg.twist.twist.angular.z=1; + odom_msg.twist.twist.linear.y=1; + odom_msg.twist.twist.linear.z=1; + + - pose_msg.header.stamp = clock_->now(); - pose_msg.header.frame_id = "base_link"; - pose_msg.pose.pose.orientation = euler_angle_to_quaternion(0.0, current_state.pitch, current_state.yaw); - publisher_twist->publish(twist_msg); - publisher_pose->publish(pose_msg); + publisher_odom->publish(odom_msg); //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.surge); return; diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index dcb2c710..80473191 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -9,6 +9,7 @@ #include "velocity_controller/PID_setup.hpp" #include #include +#include "vortex_msgs/msg/los_guidance.hpp" //#include "vortex-msgs/msg" kan legge til nye meldinger nå //Lager en klasse velocity node @@ -26,16 +27,11 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 // Publishers publisher_thrust = create_publisher(topic_thrust, 10); - //Subscribers - subscriber_twist = this->create_subscription( - topic_twist, 10, - std::bind(&Velocity_node::twist_callback,this, std::placeholders::_1)); - - subscriber_pose = this->create_subscription( - topic_pose, 10, - std::bind(&Velocity_node::pose_callback,this, std::placeholders::_1)); - - subscriber_guidance = this->create_subscription( + //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( @@ -69,7 +65,8 @@ void Velocity_node::calc_thrust() { switch (controller_type) { - case 1: + 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); @@ -77,33 +74,46 @@ void Velocity_node::calc_thrust() thrust_out.wrench.torque.y = PID_pitch.output(); thrust_out.wrench.torque.z = PID_yaw.output(); break; - case 2: - lqr_controller.update_error(guidance_values,current_state); - default: + } + 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; + } + 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; + } + } - + publisher_thrust->publish(thrust_out); return; } //Callback functions -void Velocity_node::guidance_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg_ptr){ - //RCLCPP_INFO(this->get_logger(), "Received reference: '%f'", msg_ptr->wrench.force.x); +void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ guidance_values = *msg_ptr; return; } -void Velocity_node::twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg_ptr){ - //RCLCPP_INFO(this->get_logger(), "Received velocity and orientation: '%f'", msg_ptr->wrench.force.x); - current_state.surge = msg_ptr->twist.twist.linear.x; - return; -} -void Velocity_node::pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr){ + +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; } @@ -124,10 +134,12 @@ void Velocity_node::get_new_parameters(){ 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.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"); @@ -180,6 +192,7 @@ int main(int argc, char * argv[]) //---------------------------------------------------------------------------------------------------------------- //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; @@ -225,15 +238,5 @@ Guidance_data& Guidance_data::operator=(const std_msgs::msg::Float64MultiArray& } return *this; } +*/ -Guidance_data::Guidance_data(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"); - } - }; \ No newline at end of file From 2f6c253dbc9b4268e414ef17ad89443e1ca800e9 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 5 Nov 2025 11:11:04 +0100 Subject: [PATCH 15/22] Changed LQR test file, it works --- control/velocity_controller/src/LQR_test.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/control/velocity_controller/src/LQR_test.cpp b/control/velocity_controller/src/LQR_test.cpp index 108c2354..4358f61e 100644 --- a/control/velocity_controller/src/LQR_test.cpp +++ b/control/velocity_controller/src/LQR_test.cpp @@ -33,6 +33,7 @@ class test_LQR_node : public rclcpp::Node{ 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)); + } }; From 9ec7fd48b8123f4a9db539e0204ff1741084fcb1 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 5 Nov 2025 11:11:50 +0100 Subject: [PATCH 16/22] Changed launch file, fixed QoS problem and changed ouput topic --- .../config/parameters.yaml | 8 ++--- .../launch/VCnTest.launch.py | 33 ++++++++++++++++--- control/velocity_controller/src/test_VC.cpp | 8 +++-- .../src/velocity_controller.cpp | 9 +++-- 4 files changed, 44 insertions(+), 14 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index bcdd2a15..7f08156e 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -6,7 +6,7 @@ twist_topic: /dvl/twist #Twist pose_topic: /dvl/pose #Pose guidance_topic: /guidance/los #Guidance - thrust_topic: /thrust/wrench_input #Thrust + thrust_topic: /orca/wrench_input #Thrust softwareoperation_topic: /softwareOperationMode #Software Operation killswitch_topic: /softwareKillSwitch #Kill Switch @@ -30,8 +30,8 @@ inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] calculation_rate: 200 #ms integer - publish_rate: 200 #ms + publish_rate: 1000 #ms #Clamp parameter max_force: 99.5 - controller_type: 1 - #publish_value: "Hello from config!" # parameter name: parameter value + controller_type: 1 #1 PID 2 LQR + diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index ab2b64fd..bc53f7da 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -1,9 +1,10 @@ from launch import LaunchDescription from launch_ros.actions import Node import os -#from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription from launch.actions import DeclareLaunchArgument -#from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory from launch.substitutions import LaunchConfiguration @@ -11,20 +12,42 @@ 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={'scenario': 'tacc','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', default_value='test_VC_node', + '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') + test_VC_name = LaunchConfiguration('node_name_1') return LaunchDescription([ + stonefish_sim, + orca_sim, node_name_arg, node_name_arg2, Node(package='velocity_controller', @@ -36,5 +59,5 @@ def generate_launch_description(): executable='test_VC_node', name=test_VC_name, output='screen', - parameters=[config_path]) + parameters=[config_path]) ]) \ No newline at end of file diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 56717470..617cbd34 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -22,8 +22,11 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) publisher_guidance = this->create_publisher(topic_guidance, 10); publisher_odom = this->create_publisher(topic_odom,10); + rclcpp::QoS orca_QoS(2); + orca_QoS.keep_last(2).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + subscription_thrust = this->create_subscription( - topic_thrust, 10, + topic_thrust, orca_QoS, std::bind(&test_VC::read_thrust, this, std::placeholders::_1)); timer_ = this->create_wall_timer( std::chrono::milliseconds(200), @@ -37,7 +40,7 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) void test_VC::send_guidance() { publisher_guidance->publish(reference_msg); - send_state(); + //send_state(); } void test_VC::read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg) @@ -48,6 +51,7 @@ void test_VC::read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg) //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.surge); //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.pitch); //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.yaw); + (void) msg; return; } diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 80473191..9974a77b 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -15,7 +15,7 @@ //Lager en klasse velocity node //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1,1), PID_yaw(1,1,1), PID_pitch(1,1,1), lqr_controller() +Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(10,1,1), PID_yaw(10,1,1), PID_pitch(10,1,1), lqr_controller() { //Dytter info til log RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); @@ -25,7 +25,10 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(1,1 // Publishers - publisher_thrust = create_publisher(topic_thrust, 10); + 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); //Subscribers subscriber_Odometry = this->create_subscription( @@ -66,7 +69,7 @@ void Velocity_node::calc_thrust() switch (controller_type) { case 1:{ - RCLCPP_INFO(this->get_logger(),"PID controller"); + //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); From 314a60b9f7759db9ee576263dfcbf265bd1e79c8 Mon Sep 17 00:00:00 2001 From: Henrik Date: Wed, 12 Nov 2025 14:55:35 +0100 Subject: [PATCH 17/22] All changes until christmas --- .../config/parameters.yaml | 6 ++-- .../include/velocity_controller/test_VC.hpp | 5 +-- .../velocity_controller.hpp | 4 +++ .../launch/VCnTest.launch.py | 6 ++-- control/velocity_controller/src/LQR_setup.cpp | 34 +++++++++++++++---- control/velocity_controller/src/PID_setup.cpp | 5 +-- control/velocity_controller/src/test_VC.cpp | 5 +-- .../src/velocity_controller.cpp | 25 +++++++++++--- 8 files changed, 68 insertions(+), 22 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 7f08156e..9f396c83 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -30,8 +30,8 @@ inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] calculation_rate: 200 #ms integer - publish_rate: 1000 #ms + publish_rate: 100 #ms #Clamp parameter - max_force: 99.5 - controller_type: 1 #1 PID 2 LQR + max_force: 1000.0 #should maybe be 99.5 + controller_type: 2 #1 PID 2 LQR diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index c8974d09..c8b4fb85 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -9,6 +9,7 @@ #include "velocity_controller/velocity_controller.hpp" #include #include +#include "vortex_msgs/msg/los_guidance.hpp" class test_VC : public rclcpp::Node{ public: @@ -22,7 +23,7 @@ class test_VC : public rclcpp::Node{ //guidance_data reference; Guidance_data current_state; //Subscribers and publishers - rclcpp::Publisher::SharedPtr publisher_guidance; + rclcpp::Publisher::SharedPtr publisher_guidance; rclcpp::Publisher::SharedPtr publisher_odom; rclcpp::Subscription::SharedPtr subscription_thrust; //Timers @@ -30,7 +31,7 @@ class test_VC : public rclcpp::Node{ rclcpp::Clock::SharedPtr clock_; //Messages std::vector thrust_vector; - std_msgs::msg::Float64MultiArray reference_msg; + vortex_msgs::msg::LOSGuidance reference_msg; //Topics std::string topic_odom; diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp index e559297e..4b376225 100644 --- a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -78,6 +78,10 @@ class Velocity_node : public rclcpp::Node{ 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 index bc53f7da..6e2e20ef 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -18,7 +18,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), - launch_arguments={'scenario': 'tacc','rendering_quality': 'low','rendering':'true'}.items(), + launch_arguments={'rendering_quality': 'low','rendering':'false'}.items(), ) orca_sim = TimerAction( period=12.0, @@ -54,7 +54,9 @@ def generate_launch_description(): executable='velocity_controller_node', name=velocity_controller_name, output='screen', - parameters=[config_path]), + parameters=[config_path] + #arguments=['--ros-args','--log-level','debug'] + ), Node(package='velocity_controller', executable='test_VC_node', name=test_VC_name, diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 441f1781..8d77b51b 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -4,6 +4,7 @@ #include #include #include +#include //#include //#include //#include @@ -12,7 +13,7 @@ #include "velocity_controller/utilities.hpp" #include #include - +Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix){ set_params(params); set_matrices(inertia_matrix); @@ -86,24 +87,30 @@ void LQRController::set_params(LQRparameters params){ } 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){ @@ -179,10 +186,17 @@ LQRsolveResult LQRController::solve_k_p(const Eigen::Matrix &A,const 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){ - //Some Error handling here. Also check that BRB in invertible 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(); @@ -195,7 +209,15 @@ LQRsolveResult LQRController::solve_k_p(const Eigen::Matrix &A,const 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; + //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 diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp index cf064115..451d2693 100644 --- a/control/velocity_controller/src/PID_setup.cpp +++ b/control/velocity_controller/src/PID_setup.cpp @@ -13,7 +13,7 @@ void PID_controller::calculate_thrust(double reference, double current_position, //P calculation last_output=k_p*error; //D calculation - last_output += k_d * (current_position - previous_position) / dt; + last_output += k_d * (error - previous_error) / dt; previous_position = current_position; //I calculation with anti-windup integral += error * dt; @@ -32,8 +32,9 @@ void PID_controller::calculate_thrust(double reference, double current_position, else if (last_output < min_output){ last_output = min_output; } + return; -}; +}; void PID_controller::reset_controller(){ integral = 0.0; previous_error = 0.0; diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 617cbd34..5145c4f5 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -7,6 +7,7 @@ #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 @@ -19,7 +20,7 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); topic_odom = this->get_parameter("topics.odom_topic").as_string(); topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); - publisher_guidance = this->create_publisher(topic_guidance, 10); + publisher_guidance = this->create_publisher(topic_guidance, 10); publisher_odom = this->create_publisher(topic_odom,10); rclcpp::QoS orca_QoS(2); @@ -33,7 +34,7 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) std::bind(&test_VC::send_guidance, this)); clock_ = this->get_clock(); RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); - reference_msg.data={2.0, 0.0, 0.0}; //Surge, pitch, yaw + reference_msg.surge=0.2;reference_msg.pitch=0.3;reference_msg.yaw=0.0; //Surge, pitch, yaw } diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 9974a77b..94c4fdfe 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -15,7 +15,7 @@ //Lager en klasse velocity node //Konstruktør -Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(10,1,1), PID_yaw(10,1,1), PID_pitch(10,1,1), lqr_controller() +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."); @@ -29,6 +29,7 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(10, 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( @@ -41,10 +42,12 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(10, 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::publish_thrust, this)); - timer_publish = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); + 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); @@ -60,7 +63,14 @@ Velocity_node::Velocity_node() : Node("velocity_controller_node"), PID_surge(10, //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 @@ -76,6 +86,7 @@ void Velocity_node::calc_thrust() 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:{ @@ -83,6 +94,7 @@ void Velocity_node::calc_thrust() 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]; @@ -95,8 +107,9 @@ void Velocity_node::calc_thrust() break; } } - - publisher_thrust->publish(thrust_out); + std_msgs::msg::Float64MultiArray msg; + msg.data={guidance_values.surge,guidance_values.pitch,guidance_values.yaw}; + publisher_reference->publish(msg); return; } @@ -105,6 +118,8 @@ void Velocity_node::calc_thrust() //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; } From 76da1d94748ea41bbbed264cb5bebe2f9ec7881e Mon Sep 17 00:00:00 2001 From: Henrik Date: Mon, 5 Jan 2026 12:20:14 +0100 Subject: [PATCH 18/22] Added support for vortex utils and changed to using their ssa --- control/velocity_controller/CMakeLists.txt | 20 ++++++++++++------- .../include/velocity_controller/LQR_setup.hpp | 2 +- control/velocity_controller/src/LQR_setup.cpp | 11 ++++++---- .../src/velocity_controller.cpp | 3 +++ 4 files changed, 24 insertions(+), 12 deletions(-) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 228f188b..4f072393 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -13,6 +13,7 @@ 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) @@ -20,7 +21,11 @@ find_package(BLAS REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) - +#target_include_directories(velocity_controller_node +# PUBLIC +# $ +# $ +#) include_directories( ${EIGEN3_INCLUDE_DIR} include @@ -49,9 +54,10 @@ ament_target_dependencies(velocity_controller_node std_msgs vortex_msgs geometry_msgs - Eigen3 + #Eigen3 CasADi nav_msgs + vortex_utils ) @@ -60,13 +66,13 @@ ament_target_dependencies(test_VC_node std_msgs vortex_msgs geometry_msgs - Eigen3 + #Eigen3 nav_msgs ) ament_target_dependencies(test_LQR_node rclcpp - Eigen3 + #Eigen3 vortex_msgs geometry_msgs std_msgs @@ -77,9 +83,9 @@ 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}) -target_link_libraries(test_VC_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) -target_link_libraries(test_LQR_node ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES} ${SLICOT_LIB} ${GFORTRAN_LIB}) +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 diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp index 9b39f474..cfb4a439 100644 --- a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -51,7 +51,7 @@ class LQRController{ 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); + //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); diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp index 8d77b51b..8bbd0dbc 100644 --- a/control/velocity_controller/src/LQR_setup.cpp +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -13,6 +13,9 @@ #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); @@ -39,9 +42,9 @@ LQRController::LQRController(LQRparameters params,Eigen::Matrix3d inertia_matrix return {phi, theta, psi}; };*/ -double LQRController::ssa(double angle){ +/*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){ @@ -115,8 +118,8 @@ void LQRController::update_augmented_matrices(Eigen::Matrix3d coriolis_matrix){ }; Eigen::Vector LQRController::update_error(Guidance_data guidance_values, State states){ double surge_error = guidance_values.surge - states.surge; - double pitch_error = ssa(guidance_values.pitch - states.pitch); - double yaw_error = ssa(guidance_values.yaw - states.yaw); + 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); diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp index 94c4fdfe..1875bcd8 100644 --- a/control/velocity_controller/src/velocity_controller.cpp +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -10,6 +10,9 @@ #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 From 5ce96efe159b9e1c76d97c86d849f2a81cec3342 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 6 Jan 2026 12:32:55 +0100 Subject: [PATCH 19/22] New parameters for water resistance --- control/velocity_controller/config/parameters.yaml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/control/velocity_controller/config/parameters.yaml b/control/velocity_controller/config/parameters.yaml index 9f396c83..8aea0ed6 100644 --- a/control/velocity_controller/config/parameters.yaml +++ b/control/velocity_controller/config/parameters.yaml @@ -27,11 +27,15 @@ dt: 0.1 - inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + 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: 2 #1 PID 2 LQR + controller_type: 1 #1 PID 2 LQR From 1305ea7c3da88bb45bb97cb626193c675b789527 Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 6 Jan 2026 12:34:20 +0100 Subject: [PATCH 20/22] removed unneccessary features, added euler angle publisher --- .../include/velocity_controller/test_VC.hpp | 23 ++++--- .../launch/VCnTest.launch.py | 2 +- control/velocity_controller/src/test_VC.cpp | 67 +++++-------------- 3 files changed, 31 insertions(+), 61 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index c8b4fb85..18a81f91 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -15,31 +15,34 @@ class test_VC : public rclcpp::Node{ public: test_VC(); //Callback functions - void read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg); + //void read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg); void send_guidance(); - void send_state(); + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); + //void send_state(); //Variables - //guidance_data reference; - Guidance_data current_state; + //Subscribers and publishers rclcpp::Publisher::SharedPtr publisher_guidance; - rclcpp::Publisher::SharedPtr publisher_odom; - rclcpp::Subscription::SharedPtr subscription_thrust; + rclcpp::Publisher::SharedPtr publisher_state; + rclcpp::Subscription::SharedPtr subscription_state; //Timers rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; //Messages - std::vector thrust_vector; + //std::vector thrust_vector; vortex_msgs::msg::LOSGuidance reference_msg; //Topics - std::string topic_odom; - std::string topic_thrust; + //std::string topic_odom; + //std::string topic_thrust; std::string topic_guidance; + std::string topic_state; + std::string topic_odometry; + //MSGS - nav_msgs::msg::Odometry odom_msg; + //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/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py index 6e2e20ef..454a50d9 100644 --- a/control/velocity_controller/launch/VCnTest.launch.py +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -18,7 +18,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), - launch_arguments={'rendering_quality': 'low','rendering':'false'}.items(), + launch_arguments={'rendering_quality': 'low','rendering':'true'}.items(), ) orca_sim = TimerAction( period=12.0, diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index 5145c4f5..b9ff1ccd 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -12,23 +12,21 @@ //#include "LQR_setup.hpp" //Denne noden er kun for å teste velocity_controller noden -test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) +test_VC::test_VC() : Node("test_VC_node") { this->declare_parameter("topics.guidance_topic"); - this->declare_parameter("topics.thrust_topic"); - this->declare_parameter("topics.odom_topic"); - topic_thrust = this->get_parameter("topics.thrust_topic").as_string(); - topic_odom = this->get_parameter("topics.odom_topic").as_string(); - topic_guidance = this->get_parameter("topics.guidance_topic").as_string(); + this->declare_parameter("topics.odometry_topic"); + this->topic_guidance=this->get_parameter("topics.guidance_topic").as_string(); + this->topic_odometry=this->get_parameter("topics.odometry_topic").as_string(); + topic_state="state"; publisher_guidance = this->create_publisher(topic_guidance, 10); - publisher_odom = this->create_publisher(topic_odom,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); - subscription_thrust = this->create_subscription( - topic_thrust, orca_QoS, - std::bind(&test_VC::read_thrust, this, std::placeholders::_1)); timer_ = this->create_wall_timer( std::chrono::milliseconds(200), std::bind(&test_VC::send_guidance, this)); @@ -41,48 +39,16 @@ test_VC::test_VC() : Node("test_VC_node"), current_state(0,2,2) void test_VC::send_guidance() { publisher_guidance->publish(reference_msg); - //send_state(); -} - -void test_VC::read_thrust(geometry_msgs::msg::WrenchStamped::SharedPtr msg) -{ - /*current_state.surge += 0.01 * msg->wrench.force.x; - current_state.pitch += 0.01 * msg->wrench.torque.x; - current_state.yaw += 0.01 * msg->wrench.torque.y;*/ - //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.surge); - //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.pitch); - //RCLCPP_INFO(this->get_logger(),"info: '%f'", current_state.yaw); - (void) msg; - return; } -void test_VC::send_state() -{ - - odom_msg.header.stamp = clock_->now(); - odom_msg.header.frame_id = "base_link"; - odom_msg.twist.twist.linear.x = current_state.surge; - odom_msg.pose.pose.orientation = euler_angle_to_quaternion(0.0, current_state.pitch, current_state.yaw); - odom_msg.twist.twist.linear.y=1; - odom_msg.twist.twist.linear.z=1; - odom_msg.twist.twist.angular.x=1; - odom_msg.twist.twist.angular.y=1; - odom_msg.twist.twist.angular.z=1; - odom_msg.twist.twist.linear.y=1; - odom_msg.twist.twist.linear.z=1; - +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.phit); + publisher_state->publish(msg); - - - publisher_odom->publish(odom_msg); - - //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.surge); - return; - //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.pitch); - //RCLCPP_INFO(this->get_logger(), "Published state: '%f'", current_state.yaw); } - - int main(int argc, char const *argv[]) { @@ -107,4 +73,5 @@ geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pit q.z = cr * cp * sy - sr * sp * cy; return q; -} \ No newline at end of file +} + \ No newline at end of file From f5d698a0e332d4cba2cf9ebf6da34607c50a8adb Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 6 Jan 2026 12:34:45 +0100 Subject: [PATCH 21/22] fixed dumb bugs --- control/velocity_controller/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt index 4f072393..a6eaa4f1 100644 --- a/control/velocity_controller/CMakeLists.txt +++ b/control/velocity_controller/CMakeLists.txt @@ -20,6 +20,10 @@ 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 @@ -68,6 +72,7 @@ ament_target_dependencies(test_VC_node geometry_msgs #Eigen3 nav_msgs + vortex_utils ) ament_target_dependencies(test_LQR_node @@ -77,6 +82,7 @@ ament_target_dependencies(test_LQR_node geometry_msgs std_msgs nav_msgs + vortex_utils ) link_directories(/usr/lib/gcc/x86_64-linux-gnu/11) From b7805eaca1721c8464e7fb5cf85c54869535399b Mon Sep 17 00:00:00 2001 From: Henrik Date: Tue, 6 Jan 2026 13:40:42 +0100 Subject: [PATCH 22/22] Fixed bugs so that it works --- .../include/velocity_controller/test_VC.hpp | 2 +- control/velocity_controller/src/test_VC.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/control/velocity_controller/include/velocity_controller/test_VC.hpp b/control/velocity_controller/include/velocity_controller/test_VC.hpp index 18a81f91..6d64e671 100644 --- a/control/velocity_controller/include/velocity_controller/test_VC.hpp +++ b/control/velocity_controller/include/velocity_controller/test_VC.hpp @@ -37,7 +37,7 @@ class test_VC : public rclcpp::Node{ //std::string topic_odom; //std::string topic_thrust; std::string topic_guidance; - std::string topic_state; + std::string topic_state="/state"; std::string topic_odometry; diff --git a/control/velocity_controller/src/test_VC.cpp b/control/velocity_controller/src/test_VC.cpp index b9ff1ccd..ef2af22a 100644 --- a/control/velocity_controller/src/test_VC.cpp +++ b/control/velocity_controller/src/test_VC.cpp @@ -15,10 +15,9 @@ test_VC::test_VC() : Node("test_VC_node") { this->declare_parameter("topics.guidance_topic"); - this->declare_parameter("topics.odometry_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.odometry_topic").as_string(); - topic_state="state"; + 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( @@ -32,7 +31,7 @@ test_VC::test_VC() : Node("test_VC_node") 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.0; //Surge, pitch, yaw + reference_msg.surge=0.2;reference_msg.pitch=0.3;reference_msg.yaw=0.3; //Surge, pitch, yaw } @@ -45,7 +44,8 @@ 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.phit); + msg.set__yaw(temp.psit); + msg.set__surge(msg_ptr->twist.twist.linear.x); publisher_state->publish(msg); }