From 2fb9d916e804f2275905e41cbd49010993198868 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 19 Oct 2025 15:56:52 +0200 Subject: [PATCH 01/87] nye versjonen av guidance --- .../CMakeLists.txt | 61 ++++++ .../action/LOSGuidance.action | 10 + .../config/guidanceParamsa.yaml | 10 + .../guidance_velocity_controller/guidance.hpp | 80 +++++++ .../guidance_ros.hpp | 73 +++++++ .../launch/launch_Guidance.py | 32 +++ .../guidance_velocity_controller/package.xml | 34 +++ .../src/guidance.cpp | 89 ++++++++ .../src/guidance_node.cpp | 14 ++ .../src/guidance_ros.cpp | 200 ++++++++++++++++++ 10 files changed, 603 insertions(+) create mode 100644 guidance/guidance_velocity_controller/CMakeLists.txt create mode 100644 guidance/guidance_velocity_controller/action/LOSGuidance.action create mode 100755 guidance/guidance_velocity_controller/config/guidanceParamsa.yaml create mode 100755 guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp create mode 100755 guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp create mode 100755 guidance/guidance_velocity_controller/launch/launch_Guidance.py create mode 100644 guidance/guidance_velocity_controller/package.xml create mode 100755 guidance/guidance_velocity_controller/src/guidance.cpp create mode 100755 guidance/guidance_velocity_controller/src/guidance_node.cpp create mode 100755 guidance/guidance_velocity_controller/src/guidance_ros.cpp diff --git a/guidance/guidance_velocity_controller/CMakeLists.txt b/guidance/guidance_velocity_controller/CMakeLists.txt new file mode 100644 index 000000000..7945b90be --- /dev/null +++ b/guidance/guidance_velocity_controller/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(guidance_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_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +add_executable(guidance_node + src/guidance_node.cpp + src/guidance_ros.cpp + src/guidance.cpp +) + +ament_target_dependencies(guidance_node + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + Eigen3 + tf2 + tf2_geometry_msgs + spdlog + fmt +) + +target_link_libraries(guidance_node fmt::fmt) + +install(TARGETS + guidance_node + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/action/LOSGuidance.action b/guidance/guidance_velocity_controller/action/LOSGuidance.action new file mode 100644 index 000000000..49c487775 --- /dev/null +++ b/guidance/guidance_velocity_controller/action/LOSGuidance.action @@ -0,0 +1,10 @@ +# Goal: +geometry_msgs/PointStamped goal + +--- +# Result: +bool success + +--- +# Feedback: +std_msgs/Float64MultiArray feedback diff --git a/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml b/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml new file mode 100755 index 000000000..5a333a316 --- /dev/null +++ b/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + lookahead_distance_h = 1.5; + lookahead_distance_v = 0.6; + gamma_h = 0.05; + gamma_v = 0.1; + time_step = 0.01; + u_desired = 0.3; + u_min = 0.1; + d_scale = 1.09; \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp new file mode 100755 index 000000000..26ca12642 --- /dev/null +++ b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp @@ -0,0 +1,80 @@ +#pragma once +#include +#include +#include +#include + +extern std::vector g_waypoints; + +namespace LOS { + +struct Point { + double x; + double y; + double z; + + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } +}; + +struct CrossTrackError { + double x_e; + double y_e; + double z_e; + + + inline static CrossTrackError from_vector(const Eigen::Vector3d& v) { + return CrossTrackError{v.x(), v.y(), v.z()}; + } +}; + +} + +class LOSGuidance { +public: + LOSGuidance( + double lookahead_distance_h_, + double lookahead_distance_v_, + double gamma_h_, + double gamma_v_, + double time_step_, + double u_desired_, + double u_min_, + double d_scale_); + ~LOSGuidance() = default; + + void update_angles(const LOS::Point& new_point, const LOS::Point& prev_point); + void cross_track_error(const LOS::Point& current_position, const LOS::Point& prev_point); + double desired_surge_speed(const LOS::Point& destination , const LOS::Point& current_position) ; + double desired_heading(); + double desired_pitch(); + void update_adaptive_estimates(); + + Eigen::Vector3d get_outputs() const; + LOS::CrossTrackError& cross_track(); + +private: + + double lookahead_distance_h; + double lookahead_distance_v; + double gamma_h; + double gamma_v; + double time_step; + double u_desired; + double u_min; + double d_scale; + + Eigen::Matrix3d rotation_y_; + Eigen::Matrix3d rotation_z_; + + LOS::CrossTrackError cte; + + double pi_h_; + double pi_v_; + + double beta_c_hat_ = 0.0; + double alpha_c_hat_ = 0.0; +}; diff --git a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp new file mode 100755 index 000000000..0c5a66b97 --- /dev/null +++ b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp @@ -0,0 +1,73 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +#include +#include +#include + +class GuidanceNode : public rclcpp::Node { +public: + explicit GuidanceNode(); + +private: + // init + void establish_communications(); + void setup_parameters(); + + // io + void waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg); + void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void publish_setpoints(); + vortex_msgs::msg::LOSGuidance fill_los_reference(); + void fill_los_waypoints(const geometry_msgs::msg::PointStamped & g_waypoints); + + // actions + using GoalHandle = rclcpp_action::ServerGoalHandle; + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); + void handle_accepted(const std::shared_ptr goal_handle); + void execute_guidance(const std::shared_ptr goal_handle); + + // pubs/subs + rclcpp::Publisher::SharedPtr refrence_out; + rclcpp::Subscription::SharedPtr waypoint_in; + rclcpp::Subscription::SharedPtr pose_in; + rclcpp_action::Server::SharedPtr guidance_action_server; + + // guidance + std::unique_ptr adaptive_los_guidance_; + + // state + LOS::Point current_position_{}; + LOS::Point last_waypoint_{}; + LOS::Point target_waypoint_{}; + + std::mutex mutex_; + rclcpp_action::GoalUUID preempted_goal_id_{}; + std::shared_ptr goal_handle_{}; + + // timing + std::chrono::milliseconds time_step{0}; + + // outputs + double yaw_d_; + double pitch_d_; + double u_d_ ; +}; diff --git a/guidance/guidance_velocity_controller/launch/launch_Guidance.py b/guidance/guidance_velocity_controller/launch/launch_Guidance.py new file mode 100755 index 000000000..b17fd0344 --- /dev/null +++ b/guidance/guidance_velocity_controller/launch/launch_Guidance.py @@ -0,0 +1,32 @@ +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +adapt_params = path.join( + get_package_share_directory("guidance_velocity_controller"), + "config", + "guidanceParamsa.yaml", +) +auv_params = path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + "orca.yaml", +) + + +def generate_launch_description(): + guidance_node = Node( + package="guidance_velocity_controller", + executable="guidance_node", + name="guidance_node", + namespace="orca", + parameters=[ + auv_params, + adapt_params, + ], + output="screen", + ) + return LaunchDescription([guidance_node]) \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/package.xml b/guidance/guidance_velocity_controller/package.xml new file mode 100644 index 000000000..a8b1876a0 --- /dev/null +++ b/guidance/guidance_velocity_controller/package.xml @@ -0,0 +1,34 @@ + + + + guidance_velocity_controller + 0.0.1 + LOS guidance + action server for the velocity controller. + anbitadhi + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + nav_msgs + tf2 + tf2_geometry_msgs + vortex_msgs + + rosidl_default_generators + rosidl_default_runtime + builtin_interfaces + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + rosidl_interface_packages + + + diff --git a/guidance/guidance_velocity_controller/src/guidance.cpp b/guidance/guidance_velocity_controller/src/guidance.cpp new file mode 100755 index 000000000..c0a94dd66 --- /dev/null +++ b/guidance/guidance_velocity_controller/src/guidance.cpp @@ -0,0 +1,89 @@ +#include + +LOSGuidance::LOSGuidance( + double lookahead_distance_h_, + double lookahead_distance_v_, + double gamma_h_, + double gamma_v_, + double time_step_, + double u_desired_, + double u_min_, + double d_scale_) + : lookahead_distance_h(lookahead_distance_h_), + lookahead_distance_v(lookahead_distance_v_), + gamma_h(gamma_h_), + gamma_v(gamma_v_), + time_step(time_step_), + u_desired(u_desired_), + u_min(u_min_), + d_scale(d_scale_) { + rotation_y_ = Eigen::Matrix3d::Identity(); + rotation_z_ = Eigen::Matrix3d::Identity(); +}; + +void LOSGuidance::update_angles(const LOS::Point& new_point, const LOS::Point& prev_point) { + const LOS::Point difference = new_point - prev_point; + + pi_h_ = atan2(difference.y, difference.x); + pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()).toRotationMatrix(); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + +}; + +void LOSGuidance::cross_track_error( + const LOS::Point& current_position, const LOS::Point& prev_point) { + const LOS::Point difference = current_position - prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); + + const Eigen::Vector3d cross_track_error = rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + + //save the cross track error + cte = LOS::CrossTrackError::from_vector(cross_track_error); +} + +LOS::CrossTrackError& LOSGuidance::cross_track() { + return cte; +}; + +void LOSGuidance::update_adaptive_estimates() { + double beta_c_hat_dot = + gamma_h * + (lookahead_distance_h / + sqrt(lookahead_distance_h* lookahead_distance_h + + cte.y_e * cte.y_e)) * + cte.y_e; + double alpha_c_hat_dot = + gamma_v * + (lookahead_distance_v / + sqrt(lookahead_distance_v* lookahead_distance_v + + cte.z_e * cte.z_e))* + cte.z_e; + + beta_c_hat_ += beta_c_hat_dot * time_step; + alpha_c_hat_ += alpha_c_hat_dot * time_step; +} + +double LOSGuidance::desired_surge_speed(const LOS::Point& destination , const LOS::Point& current_position) { + double dx = destination.x - current_position.x; + double dy = destination.y - current_position.y; + double dz = destination.z - current_position.z; + double distance = std::sqrt(dx*dx + dy*dy + dz*dz); + + double d = u_min + (u_desired - u_min) * std::tanh(distance/d_scale); + + return d; +} + +double LOSGuidance::desired_heading() { + return pi_h_ - beta_c_hat_ - atan(cte.y_e / lookahead_distance_h); +} + +double LOSGuidance::desired_pitch() { + return pi_v_ + alpha_c_hat_ + atan(cte.z_e / lookahead_distance_v); +} + +Eigen::Vector3d LOSGuidance::get_outputs() const { + return Eigen::Vector3d::Zero(); +} diff --git a/guidance/guidance_velocity_controller/src/guidance_node.cpp b/guidance/guidance_velocity_controller/src/guidance_node.cpp new file mode 100755 index 000000000..399eafb92 --- /dev/null +++ b/guidance/guidance_velocity_controller/src/guidance_node.cpp @@ -0,0 +1,14 @@ +#include +#include + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor exec; + exec.add_node(node); + exec.spin(); + rclcpp::shutdown(); + return 0; +} + diff --git a/guidance/guidance_velocity_controller/src/guidance_ros.cpp b/guidance/guidance_velocity_controller/src/guidance_ros.cpp new file mode 100755 index 000000000..287040439 --- /dev/null +++ b/guidance/guidance_velocity_controller/src/guidance_ros.cpp @@ -0,0 +1,200 @@ +#include + +GuidanceNode::GuidanceNode() : Node("guidance_node") { + establish_communications(); + setup_parameters(); + spdlog::info("LOS guidance node initialized"); +} + +void GuidanceNode::establish_communications() { + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.guidance.los"); + this->declare_parameter("topics.waypoint"); + this->declare_parameter("action_servers.los"); + + const std::string pose_topic = this->get_parameter("topics.pose").as_string(); + const std::string guidance_topic = this->get_parameter("topics.guidance.los").as_string(); + const std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); + const std::string actions_server = this->get_parameter("action_servers.los").as_string(); + + refrence_out = this->create_publisher(guidance_topic, 10); + + waypoint_in = this->create_subscription( + waypoint_topic, 10, + std::bind(&GuidanceNode::waypoint_callback, this, std::placeholders::_1)); + + pose_in = this->create_subscription( + pose_topic, 10, + std::bind(&GuidanceNode::pose_callback, this, std::placeholders::_1)); + + guidance_action_server = rclcpp_action::create_server( + this, + actions_server, + std::bind(&GuidanceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GuidanceNode::handle_cancel, this, std::placeholders::_1), + std::bind(&GuidanceNode::handle_accepted, this, std::placeholders::_1)); +} + +void GuidanceNode::setup_parameters() { + this->declare_parameter("lookahead_distance_h"); + this->declare_parameter("lookahead_distance_v"); + this->declare_parameter("gamma_h"); + this->declare_parameter("gamma_v"); + this->declare_parameter("time_step"); // seconds + this->declare_parameter("u_desired"); // m/s + this->declare_parameter("u_min"); + this->declare_parameter("d_scale"); + + const double lookahead_distance_h_ = this->get_parameter("lookahead_distance_h").as_double(); + const double lookahead_distance_v_ = this->get_parameter("lookahead_distance_v").as_double(); + const double gamma_h_ = this->get_parameter("gamma_h").as_double(); + const double gamma_v_ = this->get_parameter("gamma_v").as_double(); + const double time_step_s = this->get_parameter("time_step").as_double(); + const double u_desired_param = this->get_parameter("u_desired").as_double(); + const double u_min_ = this->get_parameter("u_min").as_double(); + const double d_scale_ = this->get_parameter("d_scale").as_double(); + + // Construct guidance object + adaptive_los_guidance_ = std::make_unique( + lookahead_distance_h_, + lookahead_distance_v_, + gamma_h_, + gamma_v_, + time_step_s, + u_desired_param, + u_min_, + d_scale_); + + // Convert seconds -> milliseconds for loop timing + time_step = std::chrono::milliseconds(static_cast(std::round(time_step_s * 1000.0))); +} + + +void GuidanceNode::fill_los_waypoints(const geometry_msgs::msg::PointStamped & msg) { + last_waypoint_.x = current_position_.x; + last_waypoint_.y = current_position_.y; + last_waypoint_.z = current_position_.z; + + target_waypoint_.x = msg.point.x; + target_waypoint_.y = msg.point.y; + target_waypoint_.z = msg.point.z; +} + +void GuidanceNode::waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr g_waypoints) { + fill_los_waypoints(*g_waypoints); + // NOTE: update_angles(prev, next) + adaptive_los_guidance_->update_angles(last_waypoint_, target_waypoint_); + spdlog::info("Waypoint received"); +} + +void GuidanceNode::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + current_position_.x = msg->pose.position.x; + current_position_.y = msg->pose.position.y; + current_position_.z = msg->pose.position.z; +} + +rclcpp_action::GoalResponse GuidanceNode::handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr goal) { + (void)goal; + { + std::lock_guard lock(mutex_); + if (goal_handle_ && goal_handle_->is_active()) { + spdlog::info("Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); + } + } + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GuidanceNode::handle_cancel(const std::shared_ptr goal_handle) { + spdlog::info("Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GuidanceNode::handle_accepted( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + execute_guidance(goal_handle); +} + +vortex_msgs::msg::LOSGuidance GuidanceNode::fill_los_reference() { + vortex_msgs::msg::LOSGuidance reference_msg; + reference_msg.pitch = pitch_d_; + reference_msg.yaw = yaw_d_; + reference_msg.surge = u_d_; + return reference_msg; +} + +void GuidanceNode::execute_guidance( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + { + std::lock_guard lock(mutex_); + goal_handle_ = goal_handle; + } + + const geometry_msgs::msg::PointStamped goal_to_go = goal_handle->get_goal()->goal; + fill_los_waypoints(goal_to_go); + // correct member + argument order: (prev, next) + adaptive_los_guidance_->update_angles(last_waypoint_, target_waypoint_); + + const double loop_hz = + (time_step.count() > 0) ? (1000.0 / static_cast(time_step.count())) : 10.0; + rclcpp::Rate loop_rate(loop_hz); + + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + while (rclcpp::ok()) { + // Preemption / cancel checks + { + std::lock_guard lock(mutex_); + if (goal_handle_ != goal_handle) { + spdlog::info("New goal accepted, preempting current goal"); + result->success = false; + goal_handle->canceled(result); + return; + } + if (goal_handle->is_canceling()) { + spdlog::info("Goal canceled"); + result->success = false; + goal_handle->canceled(result); + return; + } + } + + // Guidance update + adaptive_los_guidance_->cross_track_error(last_waypoint_, current_position_); + yaw_d_ = adaptive_los_guidance_->desired_heading(); + pitch_d_ = adaptive_los_guidance_->desired_pitch(); + u_d_ = adaptive_los_guidance_->desired_surge_speed(target_waypoint_, current_position_); + adaptive_los_guidance_->update_adaptive_estimates(); + + // Publish feedback + reference + const vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); + feedback->feedback = reference_msg; + goal_handle->publish_feedback(feedback); + refrence_out->publish(reference_msg); + + // Goal check using explicit distance calc + const double dx = current_position_.x - target_waypoint_.x; + const double dy = current_position_.y - target_waypoint_.y; + const double dz = current_position_.z - target_waypoint_.z; + const double dist = std::sqrt(dx * dx + dy * dy + dz * dz); + if (dist <= 0.5) { + result->success = true; + goal_handle->succeed(result); + refrence_out->publish(fill_los_reference()); + spdlog::info("Goal reached"); + return; + } + + loop_rate.sleep(); + } +} + From 064adef0ab5fbfdcbc4af38df6f85c3d71b607ca Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 26 Oct 2025 13:52:33 +0100 Subject: [PATCH 02/87] los test --- .github/workflows/simulator-test.yml | 2 +- tests/simulator_tests/los_test/simulator_test.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index d226aa02e..6ff6d50bf 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -14,7 +14,7 @@ jobs: matrix: test_script: - "tests/simulator_tests/waypoint_navigation/simulator_test.sh" - #- "tests/simulator_tests/los_test/simulator_test.sh" + - "tests/simulator_tests/los_test/simulator_test.sh" uses: vortexntnu/vortex-ci/.github/workflows/reusable-ros2-simulator-test.yml@main with: vcs_repos_file: "tests/dependencies.repos" diff --git a/tests/simulator_tests/los_test/simulator_test.sh b/tests/simulator_tests/los_test/simulator_test.sh index 97da70538..0c75818db 100755 --- a/tests/simulator_tests/los_test/simulator_test.sh +++ b/tests/simulator_tests/los_test/simulator_test.sh @@ -60,7 +60,7 @@ echo "Waiting for pose data..." timeout 10s ros2 topic echo /orca/pose --once echo "Got pose data" -setsid ros2 launch los_guidance los_guidance.launch.py & +setsid ros2 launch guidance_velocity_controller launch_Guidance.py & LOS_PID=$! setsid ros2 launch velocity_controller_lqr velocity_controller_lqr.launch.py & From e6ae1452c6f0592062ef2e018be1e2d9bdff6f9d Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 28 Oct 2025 20:23:33 +0100 Subject: [PATCH 03/87] Resolve stash conflicts: keep my YAML; keep main's joystick node --- .../config/guidanceParamsa.yaml | 16 ++++++++-------- .../los_guidance/config/guidance_params.yaml | 14 ++++++-------- 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml b/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml index 5a333a316..0225d102a 100755 --- a/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml +++ b/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml @@ -1,10 +1,10 @@ /**: ros__parameters: - lookahead_distance_h = 1.5; - lookahead_distance_v = 0.6; - gamma_h = 0.05; - gamma_v = 0.1; - time_step = 0.01; - u_desired = 0.3; - u_min = 0.1; - d_scale = 1.09; \ No newline at end of file + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + gamma_h: 0.05 + gamma_v: 0.1 + time_step: 0.01 + u_desired: 0.3 + u_min: 0.1 + d_scale: 1.09 \ No newline at end of file diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index a580cf57c..6ab3a2298 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,10 +1,8 @@ /**: ros__parameters: - los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 - goal_reached_tol: 1.0 + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + gamma_h: 0.05 + gamma_v: 0.1 + time_step: 0.01 + u_desired: 0.3 From 4d222674cd61e03b00e879d2fdc4544b902a010c Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 28 Oct 2025 20:31:44 +0100 Subject: [PATCH 04/87] fixing my error --- guidance/los_guidance/config/guidance_params.yaml | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 6ab3a2298..ebee79903 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,8 +1,10 @@ /**: ros__parameters: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 + los: + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + gamma_h: 0.05 + gamma_v: 0.1 + time_step: 0.01 + u_desired: 0.3 + goal_reached_tol: 1.0 \ No newline at end of file From 3f3e6718695a11501b25a1ad74bf32b09a1633a0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 29 Oct 2025 13:51:14 +0100 Subject: [PATCH 05/87] maths behind proportional LOS --- .../include/los_guidance/los_guidance.hpp | 130 +++++++++++------- guidance/los_guidance/src/los_guidance.cpp | 39 ++++++ 2 files changed, 120 insertions(+), 49 deletions(-) diff --git a/guidance/los_guidance/include/los_guidance/los_guidance.hpp b/guidance/los_guidance/include/los_guidance/los_guidance.hpp index 5f04ca3cc..c642a2072 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance.hpp @@ -2,76 +2,108 @@ #define LOS_GUIDANCE_HPP #include +#include +#include namespace vortex::guidance { -namespace LOS { + namespace LOS { -struct Point { - double x{}; - double y{}; - double z{}; + struct Point { + double x{}; + double y{}; + double z{}; - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } -}; + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + }; -struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; + struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } -}; + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } + }; -struct Params { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; -}; + struct Params { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; + }; -} // namespace LOS + } // namespace LOS /** * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 * in "Fossen 2024 Lecture on 2D and 3D path-following control". */ -class AdaptiveLOSGuidance { - public: - AdaptiveLOSGuidance(const LOS::Params& params); - ~AdaptiveLOSGuidance() = default; - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); + class AdaptiveLOSGuidance { + public: + AdaptiveLOSGuidance(const LOS::Params& params); + ~AdaptiveLOSGuidance() = default; - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); - double calculate_psi_d(const double& y_e) const; + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const; - double calculate_theta_d(const double& z_e) const; + double calculate_psi_d(const double& y_e) const; - void update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error); + double calculate_theta_d(const double& z_e) const; - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; -}; + void update_adaptive_estimates( + const LOS::CrossTrackError& crosstrack_error); + + private: + LOS::Params params_; + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; + }; + +// ---------------- Proportional LOS Guidance Implementation ---------------- + +/* + * Proportional Line-of-Sight (LOS) guidance algorithm based on page 356 + * form "The handbook of marine craft hydrodynamic and motion controll". +*/ + class ProportionalLOSGuidance { + public: + ProportionalLOSGuidance(const LOS::Params& params); + ~ProportionalLOSGuidance() = default; + + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); + + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const; + + double calculate_psi_d(const double& y_e) const; + double calculate_theta_d(const double& z_e) const; + + private: + LOS::Params params_; + Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; + Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; + double pi_h_{}; + double pi_v_{}; + }; } // namespace vortex::guidance -#endif // LOS_GUIDANCE_HPP +#endif // LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/src/los_guidance.cpp b/guidance/los_guidance/src/los_guidance.cpp index 3171d5442..07ee1d35f 100644 --- a/guidance/los_guidance/src/los_guidance.cpp +++ b/guidance/los_guidance/src/los_guidance.cpp @@ -56,4 +56,43 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; } +// ---------------- Proportional LOS Guidance Implementation ---------------- + +ProportionalLOSGuidance::ProportionalLOSGuidance(const LOS::Params& params) + : params_(params) {} + +void ProportionalLOSGuidance::update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point) { + const LOS::Point difference = next_point - prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, + std::sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +LOS::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const { + const Eigen::Vector3d diff_vec = (current_position - prev_point).as_vector(); + + const Eigen::Vector3d e_perp = + rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; + + return LOS::CrossTrackError::from_vector(e_perp); +} + +double ProportionalLOSGuidance::calculate_psi_d(const double& y_e) const { + const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); + return pi_h_ - std::atan(k_p_h * y_e); +} + +double ProportionalLOSGuidance::calculate_theta_d(const double& z_e) const { + const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); + return pi_v_ + std::atan(k_p_v * z_e); +} + } // namespace vortex::guidance From 2688e27ed1bcab7c0e0499dbc904a4cdf826fca7 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 2 Nov 2025 11:55:05 +0100 Subject: [PATCH 06/87] refactor: change los structure --- guidance/los_guidance/CMakeLists.txt | 2 +- .../include/los_guidance/los_guidance.hpp | 109 ------------------ .../include/los_guidance/los_guidance_ros.hpp | 3 +- guidance/los_guidance/src/los_guidance.cpp | 98 ---------------- .../los_guidance/test/test_los_guidance.cpp | 2 +- 5 files changed, 3 insertions(+), 211 deletions(-) delete mode 100644 guidance/los_guidance/include/los_guidance/los_guidance.hpp delete mode 100644 guidance/los_guidance/src/los_guidance.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index b7bd8d746..9bde13d8f 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -23,7 +23,7 @@ include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED - src/los_guidance.cpp + src/lib/adaptive_los.cpp src/los_guidance_ros.cpp ) diff --git a/guidance/los_guidance/include/los_guidance/los_guidance.hpp b/guidance/los_guidance/include/los_guidance/los_guidance.hpp deleted file mode 100644 index c642a2072..000000000 --- a/guidance/los_guidance/include/los_guidance/los_guidance.hpp +++ /dev/null @@ -1,109 +0,0 @@ -#ifndef LOS_GUIDANCE_HPP -#define LOS_GUIDANCE_HPP - -#include -#include -#include - -namespace vortex::guidance { - - namespace LOS { - - struct Point { - double x{}; - double y{}; - double z{}; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } - }; - - struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } - }; - - struct Params { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; - }; - - } // namespace LOS - -/** - * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 - * in "Fossen 2024 Lecture on 2D and 3D path-following control". - */ - - class AdaptiveLOSGuidance { - public: - AdaptiveLOSGuidance(const LOS::Params& params); - ~AdaptiveLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - - double calculate_psi_d(const double& y_e) const; - - double calculate_theta_d(const double& z_e) const; - - void update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error); - - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; - }; - -// ---------------- Proportional LOS Guidance Implementation ---------------- - -/* - * Proportional Line-of-Sight (LOS) guidance algorithm based on page 356 - * form "The handbook of marine craft hydrodynamic and motion controll". -*/ - class ProportionalLOSGuidance { - public: - ProportionalLOSGuidance(const LOS::Params& params); - ~ProportionalLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - - double calculate_psi_d(const double& y_e) const; - double calculate_theta_d(const double& z_e) const; - - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; - Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; - double pi_h_{}; - double pi_v_{}; - }; - -} // namespace vortex::guidance - -#endif // LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 94e3073ee..c12838ba4 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -4,13 +4,12 @@ #include #include #include -#include +#include "los_guidance/lib/adaptive_los.hpp" #include #include #include #include #include -#include "los_guidance.hpp" namespace vortex::guidance { diff --git a/guidance/los_guidance/src/los_guidance.cpp b/guidance/los_guidance/src/los_guidance.cpp deleted file mode 100644 index 07ee1d35f..000000000 --- a/guidance/los_guidance/src/los_guidance.cpp +++ /dev/null @@ -1,98 +0,0 @@ -#include "los_guidance/los_guidance.hpp" - -namespace vortex::guidance { - -AdaptiveLOSGuidance::AdaptiveLOSGuidance(const LOS::Params& params) - : params_(params) {} - -void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point) { - const LOS::Point difference = next_point - prev_point; - - pi_h_ = atan2(difference.y, difference.x); - pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const { - const LOS::Point difference = current_position - prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); - - const Eigen::Vector3d cross_track_error = - rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - - return LOS::CrossTrackError::from_vector(cross_track_error); -} - -double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) const { - return pi_h_ - beta_c_hat_ - atan(y_e / params_.lookahead_distance_h); -} - -double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) const { - return pi_v_ + alpha_c_hat_ + atan(z_e / params_.lookahead_distance_v); -} - -void AdaptiveLOSGuidance::update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error) { - double beta_c_hat_dot = - params_.gamma_h * - (params_.lookahead_distance_h / - sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + - crosstrack_error.y_e * crosstrack_error.y_e)) * - crosstrack_error.y_e; - double alpha_c_hat_dot = - params_.gamma_v * - (params_.lookahead_distance_v / - sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + - crosstrack_error.z_e * crosstrack_error.z_e)) * - crosstrack_error.z_e; - - beta_c_hat_ += beta_c_hat_dot * params_.time_step; - alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; -} - -// ---------------- Proportional LOS Guidance Implementation ---------------- - -ProportionalLOSGuidance::ProportionalLOSGuidance(const LOS::Params& params) - : params_(params) {} - -void ProportionalLOSGuidance::update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point) { - const LOS::Point difference = next_point - prev_point; - - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, - std::sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -LOS::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const { - const Eigen::Vector3d diff_vec = (current_position - prev_point).as_vector(); - - const Eigen::Vector3d e_perp = - rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; - - return LOS::CrossTrackError::from_vector(e_perp); -} - -double ProportionalLOSGuidance::calculate_psi_d(const double& y_e) const { - const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); - return pi_h_ - std::atan(k_p_h * y_e); -} - -double ProportionalLOSGuidance::calculate_theta_d(const double& z_e) const { - const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); - return pi_v_ + std::atan(k_p_v * z_e); -} - -} // namespace vortex::guidance diff --git a/guidance/los_guidance/test/test_los_guidance.cpp b/guidance/los_guidance/test/test_los_guidance.cpp index 78b3d4fe1..84ebe7bff 100644 --- a/guidance/los_guidance/test/test_los_guidance.cpp +++ b/guidance/los_guidance/test/test_los_guidance.cpp @@ -1,6 +1,6 @@ #include -#include "los_guidance/los_guidance.hpp" +#include "los_guidance/lib/adaptive_los.hpp" namespace vortex::guidance { From 798542a4b338ad029b62023cd4fc2a53981fff5c Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 2 Nov 2025 11:57:45 +0100 Subject: [PATCH 07/87] refactor: remove unused los package --- .../CMakeLists.txt | 61 ------ .../action/LOSGuidance.action | 10 - .../config/guidanceParamsa.yaml | 10 - .../guidance_velocity_controller/guidance.hpp | 80 ------- .../guidance_ros.hpp | 73 ------- .../launch/launch_Guidance.py | 32 --- .../guidance_velocity_controller/package.xml | 34 --- .../src/guidance.cpp | 89 -------- .../src/guidance_node.cpp | 14 -- .../src/guidance_ros.cpp | 200 ------------------ 10 files changed, 603 deletions(-) delete mode 100644 guidance/guidance_velocity_controller/CMakeLists.txt delete mode 100644 guidance/guidance_velocity_controller/action/LOSGuidance.action delete mode 100755 guidance/guidance_velocity_controller/config/guidanceParamsa.yaml delete mode 100755 guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp delete mode 100755 guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp delete mode 100755 guidance/guidance_velocity_controller/launch/launch_Guidance.py delete mode 100644 guidance/guidance_velocity_controller/package.xml delete mode 100755 guidance/guidance_velocity_controller/src/guidance.cpp delete mode 100755 guidance/guidance_velocity_controller/src/guidance_node.cpp delete mode 100755 guidance/guidance_velocity_controller/src/guidance_ros.cpp diff --git a/guidance/guidance_velocity_controller/CMakeLists.txt b/guidance/guidance_velocity_controller/CMakeLists.txt deleted file mode 100644 index 7945b90be..000000000 --- a/guidance/guidance_velocity_controller/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(guidance_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_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(vortex_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(spdlog REQUIRED) -find_package(fmt REQUIRED) - -include_directories(include) - -add_executable(guidance_node - src/guidance_node.cpp - src/guidance_ros.cpp - src/guidance.cpp -) - -ament_target_dependencies(guidance_node - rclcpp - rclcpp_action - geometry_msgs - vortex_msgs - Eigen3 - tf2 - tf2_geometry_msgs - spdlog - fmt -) - -target_link_libraries(guidance_node fmt::fmt) - -install(TARGETS - guidance_node - DESTINATION lib/${PROJECT_NAME} -) - -install( - DIRECTORY include/ - DESTINATION include -) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME}/ -) - -ament_package() \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/action/LOSGuidance.action b/guidance/guidance_velocity_controller/action/LOSGuidance.action deleted file mode 100644 index 49c487775..000000000 --- a/guidance/guidance_velocity_controller/action/LOSGuidance.action +++ /dev/null @@ -1,10 +0,0 @@ -# Goal: -geometry_msgs/PointStamped goal - ---- -# Result: -bool success - ---- -# Feedback: -std_msgs/Float64MultiArray feedback diff --git a/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml b/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml deleted file mode 100755 index 0225d102a..000000000 --- a/guidance/guidance_velocity_controller/config/guidanceParamsa.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 - u_min: 0.1 - d_scale: 1.09 \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp deleted file mode 100755 index 26ca12642..000000000 --- a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance.hpp +++ /dev/null @@ -1,80 +0,0 @@ -#pragma once -#include -#include -#include -#include - -extern std::vector g_waypoints; - -namespace LOS { - -struct Point { - double x; - double y; - double z; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } -}; - -struct CrossTrackError { - double x_e; - double y_e; - double z_e; - - - inline static CrossTrackError from_vector(const Eigen::Vector3d& v) { - return CrossTrackError{v.x(), v.y(), v.z()}; - } -}; - -} - -class LOSGuidance { -public: - LOSGuidance( - double lookahead_distance_h_, - double lookahead_distance_v_, - double gamma_h_, - double gamma_v_, - double time_step_, - double u_desired_, - double u_min_, - double d_scale_); - ~LOSGuidance() = default; - - void update_angles(const LOS::Point& new_point, const LOS::Point& prev_point); - void cross_track_error(const LOS::Point& current_position, const LOS::Point& prev_point); - double desired_surge_speed(const LOS::Point& destination , const LOS::Point& current_position) ; - double desired_heading(); - double desired_pitch(); - void update_adaptive_estimates(); - - Eigen::Vector3d get_outputs() const; - LOS::CrossTrackError& cross_track(); - -private: - - double lookahead_distance_h; - double lookahead_distance_v; - double gamma_h; - double gamma_v; - double time_step; - double u_desired; - double u_min; - double d_scale; - - Eigen::Matrix3d rotation_y_; - Eigen::Matrix3d rotation_z_; - - LOS::CrossTrackError cte; - - double pi_h_; - double pi_v_; - - double beta_c_hat_ = 0.0; - double alpha_c_hat_ = 0.0; -}; diff --git a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp b/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp deleted file mode 100755 index 0c5a66b97..000000000 --- a/guidance/guidance_velocity_controller/include/guidance_velocity_controller/guidance_ros.hpp +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include - -#include -#include -#include - -class GuidanceNode : public rclcpp::Node { -public: - explicit GuidanceNode(); - -private: - // init - void establish_communications(); - void setup_parameters(); - - // io - void waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg); - void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void publish_setpoints(); - vortex_msgs::msg::LOSGuidance fill_los_reference(); - void fill_los_waypoints(const geometry_msgs::msg::PointStamped & g_waypoints); - - // actions - using GoalHandle = rclcpp_action::ServerGoalHandle; - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID &, - std::shared_ptr goal); - rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); - void handle_accepted(const std::shared_ptr goal_handle); - void execute_guidance(const std::shared_ptr goal_handle); - - // pubs/subs - rclcpp::Publisher::SharedPtr refrence_out; - rclcpp::Subscription::SharedPtr waypoint_in; - rclcpp::Subscription::SharedPtr pose_in; - rclcpp_action::Server::SharedPtr guidance_action_server; - - // guidance - std::unique_ptr adaptive_los_guidance_; - - // state - LOS::Point current_position_{}; - LOS::Point last_waypoint_{}; - LOS::Point target_waypoint_{}; - - std::mutex mutex_; - rclcpp_action::GoalUUID preempted_goal_id_{}; - std::shared_ptr goal_handle_{}; - - // timing - std::chrono::milliseconds time_step{0}; - - // outputs - double yaw_d_; - double pitch_d_; - double u_d_ ; -}; diff --git a/guidance/guidance_velocity_controller/launch/launch_Guidance.py b/guidance/guidance_velocity_controller/launch/launch_Guidance.py deleted file mode 100755 index b17fd0344..000000000 --- a/guidance/guidance_velocity_controller/launch/launch_Guidance.py +++ /dev/null @@ -1,32 +0,0 @@ -from os import path - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -adapt_params = path.join( - get_package_share_directory("guidance_velocity_controller"), - "config", - "guidanceParamsa.yaml", -) -auv_params = path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - "orca.yaml", -) - - -def generate_launch_description(): - guidance_node = Node( - package="guidance_velocity_controller", - executable="guidance_node", - name="guidance_node", - namespace="orca", - parameters=[ - auv_params, - adapt_params, - ], - output="screen", - ) - return LaunchDescription([guidance_node]) \ No newline at end of file diff --git a/guidance/guidance_velocity_controller/package.xml b/guidance/guidance_velocity_controller/package.xml deleted file mode 100644 index a8b1876a0..000000000 --- a/guidance/guidance_velocity_controller/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - guidance_velocity_controller - 0.0.1 - LOS guidance + action server for the velocity controller. - anbitadhi - Apache-2.0 - - ament_cmake - - rclcpp - rclcpp_action - geometry_msgs - nav_msgs - tf2 - tf2_geometry_msgs - vortex_msgs - - rosidl_default_generators - rosidl_default_runtime - builtin_interfaces - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - rosidl_interface_packages - - - diff --git a/guidance/guidance_velocity_controller/src/guidance.cpp b/guidance/guidance_velocity_controller/src/guidance.cpp deleted file mode 100755 index c0a94dd66..000000000 --- a/guidance/guidance_velocity_controller/src/guidance.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include - -LOSGuidance::LOSGuidance( - double lookahead_distance_h_, - double lookahead_distance_v_, - double gamma_h_, - double gamma_v_, - double time_step_, - double u_desired_, - double u_min_, - double d_scale_) - : lookahead_distance_h(lookahead_distance_h_), - lookahead_distance_v(lookahead_distance_v_), - gamma_h(gamma_h_), - gamma_v(gamma_v_), - time_step(time_step_), - u_desired(u_desired_), - u_min(u_min_), - d_scale(d_scale_) { - rotation_y_ = Eigen::Matrix3d::Identity(); - rotation_z_ = Eigen::Matrix3d::Identity(); -}; - -void LOSGuidance::update_angles(const LOS::Point& new_point, const LOS::Point& prev_point) { - const LOS::Point difference = new_point - prev_point; - - pi_h_ = atan2(difference.y, difference.x); - pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()).toRotationMatrix(); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()).toRotationMatrix(); - -}; - -void LOSGuidance::cross_track_error( - const LOS::Point& current_position, const LOS::Point& prev_point) { - const LOS::Point difference = current_position - prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); - - const Eigen::Vector3d cross_track_error = rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - - //save the cross track error - cte = LOS::CrossTrackError::from_vector(cross_track_error); -} - -LOS::CrossTrackError& LOSGuidance::cross_track() { - return cte; -}; - -void LOSGuidance::update_adaptive_estimates() { - double beta_c_hat_dot = - gamma_h * - (lookahead_distance_h / - sqrt(lookahead_distance_h* lookahead_distance_h + - cte.y_e * cte.y_e)) * - cte.y_e; - double alpha_c_hat_dot = - gamma_v * - (lookahead_distance_v / - sqrt(lookahead_distance_v* lookahead_distance_v + - cte.z_e * cte.z_e))* - cte.z_e; - - beta_c_hat_ += beta_c_hat_dot * time_step; - alpha_c_hat_ += alpha_c_hat_dot * time_step; -} - -double LOSGuidance::desired_surge_speed(const LOS::Point& destination , const LOS::Point& current_position) { - double dx = destination.x - current_position.x; - double dy = destination.y - current_position.y; - double dz = destination.z - current_position.z; - double distance = std::sqrt(dx*dx + dy*dy + dz*dz); - - double d = u_min + (u_desired - u_min) * std::tanh(distance/d_scale); - - return d; -} - -double LOSGuidance::desired_heading() { - return pi_h_ - beta_c_hat_ - atan(cte.y_e / lookahead_distance_h); -} - -double LOSGuidance::desired_pitch() { - return pi_v_ + alpha_c_hat_ + atan(cte.z_e / lookahead_distance_v); -} - -Eigen::Vector3d LOSGuidance::get_outputs() const { - return Eigen::Vector3d::Zero(); -} diff --git a/guidance/guidance_velocity_controller/src/guidance_node.cpp b/guidance/guidance_velocity_controller/src/guidance_node.cpp deleted file mode 100755 index 399eafb92..000000000 --- a/guidance/guidance_velocity_controller/src/guidance_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include -#include - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor exec; - exec.add_node(node); - exec.spin(); - rclcpp::shutdown(); - return 0; -} - diff --git a/guidance/guidance_velocity_controller/src/guidance_ros.cpp b/guidance/guidance_velocity_controller/src/guidance_ros.cpp deleted file mode 100755 index 287040439..000000000 --- a/guidance/guidance_velocity_controller/src/guidance_ros.cpp +++ /dev/null @@ -1,200 +0,0 @@ -#include - -GuidanceNode::GuidanceNode() : Node("guidance_node") { - establish_communications(); - setup_parameters(); - spdlog::info("LOS guidance node initialized"); -} - -void GuidanceNode::establish_communications() { - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.guidance.los"); - this->declare_parameter("topics.waypoint"); - this->declare_parameter("action_servers.los"); - - const std::string pose_topic = this->get_parameter("topics.pose").as_string(); - const std::string guidance_topic = this->get_parameter("topics.guidance.los").as_string(); - const std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); - const std::string actions_server = this->get_parameter("action_servers.los").as_string(); - - refrence_out = this->create_publisher(guidance_topic, 10); - - waypoint_in = this->create_subscription( - waypoint_topic, 10, - std::bind(&GuidanceNode::waypoint_callback, this, std::placeholders::_1)); - - pose_in = this->create_subscription( - pose_topic, 10, - std::bind(&GuidanceNode::pose_callback, this, std::placeholders::_1)); - - guidance_action_server = rclcpp_action::create_server( - this, - actions_server, - std::bind(&GuidanceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&GuidanceNode::handle_cancel, this, std::placeholders::_1), - std::bind(&GuidanceNode::handle_accepted, this, std::placeholders::_1)); -} - -void GuidanceNode::setup_parameters() { - this->declare_parameter("lookahead_distance_h"); - this->declare_parameter("lookahead_distance_v"); - this->declare_parameter("gamma_h"); - this->declare_parameter("gamma_v"); - this->declare_parameter("time_step"); // seconds - this->declare_parameter("u_desired"); // m/s - this->declare_parameter("u_min"); - this->declare_parameter("d_scale"); - - const double lookahead_distance_h_ = this->get_parameter("lookahead_distance_h").as_double(); - const double lookahead_distance_v_ = this->get_parameter("lookahead_distance_v").as_double(); - const double gamma_h_ = this->get_parameter("gamma_h").as_double(); - const double gamma_v_ = this->get_parameter("gamma_v").as_double(); - const double time_step_s = this->get_parameter("time_step").as_double(); - const double u_desired_param = this->get_parameter("u_desired").as_double(); - const double u_min_ = this->get_parameter("u_min").as_double(); - const double d_scale_ = this->get_parameter("d_scale").as_double(); - - // Construct guidance object - adaptive_los_guidance_ = std::make_unique( - lookahead_distance_h_, - lookahead_distance_v_, - gamma_h_, - gamma_v_, - time_step_s, - u_desired_param, - u_min_, - d_scale_); - - // Convert seconds -> milliseconds for loop timing - time_step = std::chrono::milliseconds(static_cast(std::round(time_step_s * 1000.0))); -} - - -void GuidanceNode::fill_los_waypoints(const geometry_msgs::msg::PointStamped & msg) { - last_waypoint_.x = current_position_.x; - last_waypoint_.y = current_position_.y; - last_waypoint_.z = current_position_.z; - - target_waypoint_.x = msg.point.x; - target_waypoint_.y = msg.point.y; - target_waypoint_.z = msg.point.z; -} - -void GuidanceNode::waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr g_waypoints) { - fill_los_waypoints(*g_waypoints); - // NOTE: update_angles(prev, next) - adaptive_los_guidance_->update_angles(last_waypoint_, target_waypoint_); - spdlog::info("Waypoint received"); -} - -void GuidanceNode::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - current_position_.x = msg->pose.position.x; - current_position_.y = msg->pose.position.y; - current_position_.z = msg->pose.position.z; -} - -rclcpp_action::GoalResponse GuidanceNode::handle_goal( - const rclcpp_action::GoalUUID &, - std::shared_ptr goal) { - (void)goal; - { - std::lock_guard lock(mutex_); - if (goal_handle_ && goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } - } - spdlog::info("Accepted goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse GuidanceNode::handle_cancel(const std::shared_ptr goal_handle) { - spdlog::info("Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; -} - -void GuidanceNode::handle_accepted( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - execute_guidance(goal_handle); -} - -vortex_msgs::msg::LOSGuidance GuidanceNode::fill_los_reference() { - vortex_msgs::msg::LOSGuidance reference_msg; - reference_msg.pitch = pitch_d_; - reference_msg.yaw = yaw_d_; - reference_msg.surge = u_d_; - return reference_msg; -} - -void GuidanceNode::execute_guidance( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - { - std::lock_guard lock(mutex_); - goal_handle_ = goal_handle; - } - - const geometry_msgs::msg::PointStamped goal_to_go = goal_handle->get_goal()->goal; - fill_los_waypoints(goal_to_go); - // correct member + argument order: (prev, next) - adaptive_los_guidance_->update_angles(last_waypoint_, target_waypoint_); - - const double loop_hz = - (time_step.count() > 0) ? (1000.0 / static_cast(time_step.count())) : 10.0; - rclcpp::Rate loop_rate(loop_hz); - - auto feedback = std::make_shared(); - auto result = std::make_shared(); - - while (rclcpp::ok()) { - // Preemption / cancel checks - { - std::lock_guard lock(mutex_); - if (goal_handle_ != goal_handle) { - spdlog::info("New goal accepted, preempting current goal"); - result->success = false; - goal_handle->canceled(result); - return; - } - if (goal_handle->is_canceling()) { - spdlog::info("Goal canceled"); - result->success = false; - goal_handle->canceled(result); - return; - } - } - - // Guidance update - adaptive_los_guidance_->cross_track_error(last_waypoint_, current_position_); - yaw_d_ = adaptive_los_guidance_->desired_heading(); - pitch_d_ = adaptive_los_guidance_->desired_pitch(); - u_d_ = adaptive_los_guidance_->desired_surge_speed(target_waypoint_, current_position_); - adaptive_los_guidance_->update_adaptive_estimates(); - - // Publish feedback + reference - const vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); - feedback->feedback = reference_msg; - goal_handle->publish_feedback(feedback); - refrence_out->publish(reference_msg); - - // Goal check using explicit distance calc - const double dx = current_position_.x - target_waypoint_.x; - const double dy = current_position_.y - target_waypoint_.y; - const double dz = current_position_.z - target_waypoint_.z; - const double dist = std::sqrt(dx * dx + dy * dy + dz * dz); - if (dist <= 0.5) { - result->success = true; - goal_handle->succeed(result); - refrence_out->publish(fill_los_reference()); - spdlog::info("Goal reached"); - return; - } - - loop_rate.sleep(); - } -} - From 3bb892627bd89d847961fe2da87048f7a998d616 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 2 Nov 2025 12:36:03 +0100 Subject: [PATCH 08/87] fix: fix ignore lib --- .gitignore | 1 - .../include/los_guidance/lib/adaptive_los.hpp | 135 ++++++++++++++++++ .../include/los_guidance/lib/types.hpp | 56 ++++++++ .../los_guidance/src/lib/adaptive_los.cpp | 98 +++++++++++++ 4 files changed, 289 insertions(+), 1 deletion(-) create mode 100644 guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp create mode 100755 guidance/los_guidance/include/los_guidance/lib/types.hpp create mode 100644 guidance/los_guidance/src/lib/adaptive_los.cpp diff --git a/.gitignore b/.gitignore index f86dbd841..2a085ac8d 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,6 @@ install/ log/ build/ bin/ -lib/ msg_gen/ srv_gen/ msg/*Action.msg diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp new file mode 100644 index 000000000..d7a50fec4 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -0,0 +1,135 @@ +#ifndef LOS_GUIDANCE_HPP +#define LOS_GUIDANCE_HPP + +#include +#include +#include + +namespace vortex::guidance { + + namespace LOS { + + struct Point { + double x{}; + double y{}; + double z{}; + + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + }; + + struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; + + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } + }; + + struct Params { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; + }; + + } // namespace LOS + +/** + * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 + * in "Fossen 2024 Lecture on 2D and 3D path-following control". + */ + + class AdaptiveLOSGuidance { + public: + AdaptiveLOSGuidance(const LOS::Params& params); + ~AdaptiveLOSGuidance() = default; + + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); + + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const; + + double calculate_psi_d(const double& y_e) const; + + double calculate_theta_d(const double& z_e) const; + + void update_adaptive_estimates( + const LOS::CrossTrackError& crosstrack_error); + + private: + LOS::Params params_; + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; + }; + +// ---------------- Proportional LOS Guidance Implementation ---------------- + +/* + * Proportional Line-of-Sight (LOS) guidance algorithm based on page 356 + * form "The handbook of marine craft hydrodynamic and motion controll". +*/ + class ProportionalLOSGuidance { + public: + ProportionalLOSGuidance(const LOS::Params& params); + ~ProportionalLOSGuidance() = default; + + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); + + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const; + + double calculate_psi_d(const double& y_e) const; + double calculate_theta_d(const double& z_e) const; + + private: + LOS::Params params_; + Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; + Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; + double pi_h_{}; + double pi_v_{}; + }; + +// ---------------- Integer LOS Guidance Implementation ---------------- + +/* + * Integer Line-of-Sight (LOS) guidance algorithm based on page 356 + * form "The handbook of marine craft hydrodynamic and motion controll". +*/ + + class IntergalLOSGuidance { + public: + IntergalLOSGuidance(const LOS::Params& params); + ~IntergalLOSGuidance() = default; + void update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point); + LOS::CrossTrackError calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const; + double calculate_psi_d(const double& y_e) const; + double calculate_theta_d(const double& z_e) const; + private: + LOS::Params params_; + Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; + Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; + }; +} // namespace vortex::guidance +#endif // LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp new file mode 100755 index 000000000..6c7643a6c --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -0,0 +1,56 @@ +#ifndef types_hpp +#define types_hpp + +#include +#include +#include + +namespace vortex::guidance::lod::types{ + + namespace LOS { + + struct Point { + double x{}; + double y{}; + double z{}; + + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + }; + + struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; + + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } + }; + + struct Output { + double psi_d{}; + double theta_d{}; + }; + + struct Inputs{ + Point prev_point{}; + Point next_point{}; + Point current_position{}; + }; + + enum class Active_LOSMethod { + PROPORTIONAL, + INTEGRAL, + ADAPTIVE + }; + + + } // namespace LOS + +} // namespace vortex::guidance::los::types + +#endif \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp new file mode 100644 index 000000000..7e6b6a650 --- /dev/null +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -0,0 +1,98 @@ +#include "los_guidance/lib/adaptive_los.hpp" + +namespace vortex::guidance { + +AdaptiveLOSGuidance::AdaptiveLOSGuidance(const LOS::Params& params) + : params_(params) {} + +void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point) { + const LOS::Point difference = next_point - prev_point; + + pi_h_ = atan2(difference.y, difference.x); + pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const { + const LOS::Point difference = current_position - prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); + + const Eigen::Vector3d cross_track_error = + rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + + return LOS::CrossTrackError::from_vector(cross_track_error); +} + +double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) const { + return pi_h_ - beta_c_hat_ - atan(y_e / params_.lookahead_distance_h); +} + +double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) const { + return pi_v_ + alpha_c_hat_ + atan(z_e / params_.lookahead_distance_v); +} + +void AdaptiveLOSGuidance::update_adaptive_estimates( + const LOS::CrossTrackError& crosstrack_error) { + double beta_c_hat_dot = + params_.gamma_h * + (params_.lookahead_distance_h / + sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + + crosstrack_error.y_e * crosstrack_error.y_e)) * + crosstrack_error.y_e; + double alpha_c_hat_dot = + params_.gamma_v * + (params_.lookahead_distance_v / + sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + + crosstrack_error.z_e * crosstrack_error.z_e)) * + crosstrack_error.z_e; + + beta_c_hat_ += beta_c_hat_dot * params_.time_step; + alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; +} + +// ---------------- Proportional LOS Guidance Implementation ---------------- + +ProportionalLOSGuidance::ProportionalLOSGuidance(const LOS::Params& params) + : params_(params) {} + +void ProportionalLOSGuidance::update_angles(const LOS::Point& prev_point, + const LOS::Point& next_point) { + const LOS::Point difference = next_point - prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, + std::sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +LOS::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( + const LOS::Point& prev_point, + const LOS::Point& current_position) const { + const Eigen::Vector3d diff_vec = (current_position - prev_point).as_vector(); + + const Eigen::Vector3d e_perp = + rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; + + return LOS::CrossTrackError::from_vector(e_perp); +} + +double ProportionalLOSGuidance::calculate_psi_d(const double& y_e) const { + const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); + return pi_h_ - std::atan(k_p_h * y_e); +} + +double ProportionalLOSGuidance::calculate_theta_d(const double& z_e) const { + const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); + return pi_v_ + std::atan(k_p_v * z_e); +} + +} // namespace vortex::guidance From b7f63a90cced3d38fdd21097af5dc97bca7f97a2 Mon Sep 17 00:00:00 2001 From: Anbit Adhikari Date: Wed, 5 Nov 2025 13:13:27 +0100 Subject: [PATCH 09/87] Test: add test files for ALos and Plos --- guidance/los_guidance/CMakeLists.txt | 2 + .../include/los_guidance/lib/adaptive_los.hpp | 148 +++----------- .../include/los_guidance/lib/integral_los.hpp | 32 +++ .../los_guidance/lib/proportional_los.hpp | 38 ++++ .../include/los_guidance/lib/types.hpp | 88 ++++----- .../los_guidance/src/lib/adaptive_los.cpp | 122 ++++-------- .../los_guidance/src/lib/integral_los.cpp | 1 + .../los_guidance/src/lib/proportional_los.cpp | 38 ++++ guidance/los_guidance/test/CMakeLists.txt | 5 +- .../los_guidance/test/adaptive_los_test.cpp | 179 +++++++++++++++++ .../los_guidance/test/integral_los_test.cpp | 0 .../test/proportional_los_test.cpp | 113 +++++++++++ .../los_guidance/test/test_los_guidance.cpp | 182 ------------------ 13 files changed, 519 insertions(+), 429 deletions(-) create mode 100755 guidance/los_guidance/include/los_guidance/lib/integral_los.hpp create mode 100755 guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp create mode 100755 guidance/los_guidance/src/lib/integral_los.cpp create mode 100755 guidance/los_guidance/src/lib/proportional_los.cpp create mode 100644 guidance/los_guidance/test/adaptive_los_test.cpp create mode 100755 guidance/los_guidance/test/integral_los_test.cpp create mode 100755 guidance/los_guidance/test/proportional_los_test.cpp delete mode 100644 guidance/los_guidance/test/test_los_guidance.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 9bde13d8f..34996951e 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -23,6 +23,8 @@ include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED + src/lib/proportional_los.cpp + src/lib/integral_los.cpp src/lib/adaptive_los.cpp src/los_guidance_ros.cpp ) diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index d7a50fec4..ee655ccb8 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -1,135 +1,49 @@ -#ifndef LOS_GUIDANCE_HPP -#define LOS_GUIDANCE_HPP +#ifndef ADAPTIVE_LOS_GUIDANCE_HPP +#define ADAPTIVE_LOS_GUIDANCE_HPP #include #include +#include "los_guidance/lib/types.hpp" #include -namespace vortex::guidance { - - namespace LOS { - - struct Point { - double x{}; - double y{}; - double z{}; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } - }; - - struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } - }; - - struct Params { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; - }; - - } // namespace LOS - /** * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 * in "Fossen 2024 Lecture on 2D and 3D path-following control". */ - class AdaptiveLOSGuidance { - public: - AdaptiveLOSGuidance(const LOS::Params& params); - ~AdaptiveLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - - double calculate_psi_d(const double& y_e) const; - - double calculate_theta_d(const double& z_e) const; +namespace vortex::guidance::los { - void update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error); + struct AdaptiveLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; + }; - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; - }; - -// ---------------- Proportional LOS Guidance Implementation ---------------- - -/* - * Proportional Line-of-Sight (LOS) guidance algorithm based on page 356 - * form "The handbook of marine craft hydrodynamic and motion controll". -*/ - class ProportionalLOSGuidance { - public: - ProportionalLOSGuidance(const LOS::Params& params); - ~ProportionalLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); + class AdaptiveLOSGuidance { + public: + AdaptiveLOSGuidance(const AdaptiveLosParams& params); + ~AdaptiveLOSGuidance() = default; - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; + types::Output calculate_outputs(const types::Inputs& inputs); - double calculate_psi_d(const double& y_e) const; - double calculate_theta_d(const double& z_e) const; + private: + void update_angles(const types::Inputs& inputs); + const types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs); + void update_adaptive_estimates(const types::CrossTrackError& cross_track_error); - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; - Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; - double pi_h_{}; - double pi_v_{}; - }; + AdaptiveLosParams m_params{}; + //usikker om disse skal vaere med i den nye strukturen? + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; -// ---------------- Integer LOS Guidance Implementation ---------------- -/* - * Integer Line-of-Sight (LOS) guidance algorithm based on page 356 - * form "The handbook of marine craft hydrodynamic and motion controll". -*/ + }; // namespace vortex::guidance::los - class IntergalLOSGuidance { - public: - IntergalLOSGuidance(const LOS::Params& params); - ~IntergalLOSGuidance() = default; - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - double calculate_psi_d(const double& y_e) const; - double calculate_theta_d(const double& z_e) const; - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_{Eigen::Matrix3d::Zero()}; - Eigen::Matrix3d rotation_z_{Eigen::Matrix3d::Zero()}; - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; - }; -} // namespace vortex::guidance -#endif // LOS_GUIDANCE_HPP \ No newline at end of file +} +#endif // LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp new file mode 100755 index 000000000..29a750e98 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -0,0 +1,32 @@ +#ifndef INTEGRAL_LOS_GUIDANCE_HPP +#define INTEGRAL_LOS_GUIDANCE_HPP + +#include +#include +#include "los_guidance/lib/types.hpp" +#include + +namespace vortex::guidance::los { + + struct IntergalLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; + }; + + class IntergalLOSGuidance { + public: + IntergalLOSGuidance(const IntergalLosParams& params); + ~IntergalLOSGuidance() = default; + + private: + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error(const tyoes::Inputs& inputs); + + IntergalLosParams m_params{}; + }; +} + +#endif // INTEGRAL_LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp new file mode 100755 index 000000000..730646758 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -0,0 +1,38 @@ +#ifndef PROPORTIONAL_LOS_GUIDANCE_HPP +#define PROPORTIONAL_LOS_GUIDANCE_HPP + +#include +#include +#include "los_guidance/lib/types.hpp" +#include + +namespace vortex::guidance::los { + struct ProportionalLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double k_p_h{}; + double k_p_v{}; + }; + + class ProportionalLOSGuidance { + public: + ProportionalLOSGuidance(const ProportionalLosParams& params); + ~ProportionalLOSGuidance() = default; + + tyes::Output calculate_outputs(const types::Inputs& inputs); + + private: + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs) const; + + ProportionalLosParams m_params{}; + //again i dont know if i should have them here or just in the functions + double pi_h_{0.0}; + double pi_v_{0.0}; + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; + }; + +} + +#endif // PROPORTIONAL_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index 6c7643a6c..c0b53fcf0 100755 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -1,55 +1,49 @@ -#ifndef types_hpp -#define types_hpp +#ifndef TYPES_HPP +#define TYPES_HPP #include #include #include -namespace vortex::guidance::lod::types{ - - namespace LOS { - - struct Point { - double x{}; - double y{}; - double z{}; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } - }; - - struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } - }; - - struct Output { - double psi_d{}; - double theta_d{}; - }; - - struct Inputs{ - Point prev_point{}; - Point next_point{}; - Point current_position{}; - }; - - enum class Active_LOSMethod { - PROPORTIONAL, - INTEGRAL, - ADAPTIVE - }; - - - } // namespace LOS +namespace vortex::guidance::los::types{ + struct Point { + double x{}; + double y{}; + double z{}; + + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + }; + + struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; + + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } + }; + + struct Output { + double psi_d{}; + double theta_d{}; + }; + + struct Inputs{ + Point prev_point{}; + Point next_point{}; + Point current_position{}; + }; + + enum class Active_LOSMethod { + PROPORTIONAL, + INTEGRAL, + ADAPTIVE + }; } // namespace vortex::guidance::los::types diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 7e6b6a650..86c9763f6 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -1,98 +1,56 @@ #include "los_guidance/lib/adaptive_los.hpp" -namespace vortex::guidance { +namespace vortex::guidance::los { + + AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params): m_params{params} {} -AdaptiveLOSGuidance::AdaptiveLOSGuidance(const LOS::Params& params) - : params_(params) {} + void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { + const double dx = inputs.next_point.x - inputs.prev_point.x; + const double dy = inputs.next_point.y - inputs.prev_point.y; + const double dz = inputs.next_point.z - inputs.prev_point.z; -void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point) { - const LOS::Point difference = next_point - prev_point; + pi_h_ = std::atan2(dy, dx); + pi_v_ = std::atan2(-dz, std::sqrt(dx*dx + dy*dy)); - pi_h_ = atan2(difference.y, difference.x); - pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + - difference.y * difference.y)); + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + } - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} + types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) const { -LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const { - const LOS::Point difference = current_position - prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); + const types::Point difference = inputs.current_position - inputs.prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); - const Eigen::Vector3d cross_track_error = - rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + const Eigen::Vector3d cross_track_error = + rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - return LOS::CrossTrackError::from_vector(cross_track_error); -} + return types::CrossTrackError::from_vector(cross_track_error); + } -double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) const { - return pi_h_ - beta_c_hat_ - atan(y_e / params_.lookahead_distance_h); -} + void AdaptiveLOSGuidance::update_adaptive_estimates(const types::CrossTrackError& e) { + const double denom_h = std::sqrt( + m_params.lookahead_distance_h * m_params.lookahead_distance_h + e.y_e * e.y_e); + const double denom_v = std::sqrt( + m_params.lookahead_distance_v * m_params.lookahead_distance_v + e.z_e * e.z_e); -double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) const { - return pi_v_ + alpha_c_hat_ + atan(z_e / params_.lookahead_distance_v); -} + const double beta_dot = m_params.gamma_h * (m_params.lookahead_distance_h / denom_h) * e.y_e; + const double alpha_dot = m_params.gamma_v * (m_params.lookahead_distance_v / denom_v) * e.z_e; -void AdaptiveLOSGuidance::update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error) { - double beta_c_hat_dot = - params_.gamma_h * - (params_.lookahead_distance_h / - sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + - crosstrack_error.y_e * crosstrack_error.y_e)) * - crosstrack_error.y_e; - double alpha_c_hat_dot = - params_.gamma_v * - (params_.lookahead_distance_v / - sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + - crosstrack_error.z_e * crosstrack_error.z_e)) * - crosstrack_error.z_e; + beta_c_hat_ += beta_dot * m_params.time_step; + alpha_c_hat_ += alpha_dot * m_params.time_step; + } + + types::Output AdaptiveLOSGuidance::calculate_outputs(const types::Inputs& inputs) { - beta_c_hat_ += beta_c_hat_dot * params_.time_step; - alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; -} + update_angles(inputs) + const types::CrossTrackError e = calculate_crosstrack_error(inputs); + update_adaptive_estimates(e); -// ---------------- Proportional LOS Guidance Implementation ---------------- - -ProportionalLOSGuidance::ProportionalLOSGuidance(const LOS::Params& params) - : params_(params) {} - -void ProportionalLOSGuidance::update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point) { - const LOS::Point difference = next_point - prev_point; - - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, - std::sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -LOS::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const { - const Eigen::Vector3d diff_vec = (current_position - prev_point).as_vector(); - - const Eigen::Vector3d e_perp = - rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; - - return LOS::CrossTrackError::from_vector(e_perp); -} - -double ProportionalLOSGuidance::calculate_psi_d(const double& y_e) const { - const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); - return pi_h_ - std::atan(k_p_h * y_e); -} - -double ProportionalLOSGuidance::calculate_theta_d(const double& z_e) const { - const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); - return pi_v_ + std::atan(k_p_v * z_e); -} + const double psi_d = pi_h_ - beta_c_hat_ - std::atan(e.y_e / params_.lookahead_distance_h); + const double theta_d = pi_v_ + alpha_c_hat_ + std::atan(e.z_e / params_.lookahead_distance_v); + + return types::Output{psi_d, theta_d}; + } + } // namespace vortex::guidance diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp new file mode 100755 index 000000000..847f0d408 --- /dev/null +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -0,0 +1 @@ + #include "los_guidance/lib/integral_los.hpp" \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp new file mode 100755 index 000000000..e94cf57e0 --- /dev/null +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -0,0 +1,38 @@ + #include "los_guidance/lib/proportional_los.hpp" + +namespace vortex::guidance::los { + + ProportionalLOSGuidance::ProportionalLOSGuidance(const ProportionalLosParams& params) : m_params{params} {} + + void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + } + + types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) const { + + const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); + const Eigen::Vector3d e_perp = rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; + + return types::CrossTrackError::from_vector(e_perp); + } + + types::Output ProportionalLOSGuidance::calculate_outputs(const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError e = calculate_crosstrack_error(inputs); + + const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); + const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); + + const double psi_d = pi_h_ - std::atan(k_p_h * e.y_e); + const double theta_d = pi_v_ + std::atan(k_p_v * e.z_e); + + return types::Output{psi_d, theta_d}; + } + +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index 69c0dfa52..f2b81bdc2 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -6,7 +6,10 @@ include(GoogleTest) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) add_executable( ${TEST_BINARY_NAME} - test_los_guidance.cpp + adaptive_los_test.cpp + proportional_los_test.cpp + integral_los_test.cpp + ) target_link_libraries( diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp new file mode 100644 index 000000000..a0195a79a --- /dev/null +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -0,0 +1,179 @@ +#include +#include "los_guidance/lib/adaptive_los.hpp" + +namespace vortex::guidance::los{ //new namespace added los + + class AdaptiveLosTest : public ::testing::Test { + protected: + AdaptiveLosTest() : los_{get_params()} {} + + AdaptiveLosParams get_params() { + AdaptiveLosParams p; + p.lookahead_distance_h = 1.0; + p.lookahead_distance_v = 1.0; + p.gamma_h = 1.0; + p.gamma_v = 1.0; + p.time_step = 0.01; + return p; + } + + AdaptiveLOSGuidance los_; + const double tol = 1e-9; + }; + /* + TEST_F(AdaptiveLosTest, T01_test_cross_track_error_on_track){ + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); + } + + TEST_F(AdaptiveLosTests, T02_test_cross_track_error_right_off_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); + } + + TEST_F(AdaptiveLosTests, T03_test_cross_track_error_left_off_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, -0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); + } + + TEST_F(AdaptiveLosTests, T04_test_cross_track_error_under_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.5, tol); + } + + TEST_F(AdaptiveLosTests, T05_test_cross_track_error_over_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, -0.5, tol); + } + */ + + // Test commanded angles when drone is to the right of the track + TEST_F(AdaptiveLosTests, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is to the left of the track + TEST_F(AdaptiveLosTests, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is under the track + TEST_F(AdaptiveLosTests, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); + } + + // Test commanded angles when drone is over the track + TEST_F(AdaptiveLosTests, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + + // Test commanded angles when drone is over and to the right of the track + + TEST_F(AdaptiveLosTests, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp new file mode 100755 index 000000000..e69de29bb diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp new file mode 100755 index 000000000..de040614c --- /dev/null +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -0,0 +1,113 @@ +#include +#include "los_guidance/lib/adaptive_los.hpp" + +namespace vortex::guidance::los{ + + class ProportionalLosTest : public ::testing::Test { + protected: + ProportionalLosTest() : Plos_{get_params()} {} + + ProportionalLosTest get_params() { + ProportionalLosParams params; + params.lookahead_distance_h = 10.0; + params.lookahead_distance_v = 10.0; + params.k_p_h = 0.667; // needs tuning + params.k_p_v = 0.582; // needs tuning + params.time_step = 0.01; + return params; + } + + ProportionalLOSGuidance Plos_; + const double tol = 1e-9; + }; + + // Test commanded angles when drone is to the right of the track + TEST_F(ProportionalLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Output O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is to the left of the track + TEST_F(ProportionalLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Output O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is under the track + TEST_F(ProportionalLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Output O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); + } + + // Test commanded angles when drone is over the track + TEST_F(ProportionalLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Output O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + + // Test commanded angles when drone is over and to the right of the track + + TEST_F(ProportionalLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Output O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + +} // namespace vortex::guidance + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/guidance/los_guidance/test/test_los_guidance.cpp b/guidance/los_guidance/test/test_los_guidance.cpp deleted file mode 100644 index 84ebe7bff..000000000 --- a/guidance/los_guidance/test/test_los_guidance.cpp +++ /dev/null @@ -1,182 +0,0 @@ -#include - -#include "los_guidance/lib/adaptive_los.hpp" - -namespace vortex::guidance { - -class LOSTests : public ::testing::Test { - protected: - LOSTests() : los_guidance_{get_los_params()} {} - - LOS::Params get_los_params() { - LOS::Params params; - params.lookahead_distance_h = 1.0; - params.lookahead_distance_v = 1.0; - params.gamma_h = 1.0; - params.gamma_v = 1.0; - params.time_step = 0.01; - return params; - } - - AdaptiveLOSGuidance los_guidance_; - const double tol = 1e-9; -}; - -TEST_F(LOSTests, T01_test_cross_track_error_on_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T02_test_cross_track_error_right_off_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T03_test_cross_track_error_left_off_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, -0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, -0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T04_test_cross_track_error_under_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.5, tol); -} - -TEST_F(LOSTests, T05_test_cross_track_error_over_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, -0.5, tol); -} - -// Test commanded angles when drone is to the right of the track -TEST_F(LOSTests, T06_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(psi_d, 0.0); - EXPECT_GT(psi_d, -1.57); - // Pitch cmd should be zero - EXPECT_NEAR(theta_d, 0.0, tol); -} - -// Test commanded angles when drone is to the left of the track -TEST_F(LOSTests, T07_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, -0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(psi_d, 0.0); - EXPECT_LT(psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(theta_d, 0.0, tol); -} - -// Test commanded angles when drone is under the track -TEST_F(LOSTests, T08_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be 0 - EXPECT_NEAR(psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(theta_d, 0.0); - EXPECT_LT(theta_d, 1.57); -} - -// Test commanded angles when drone is over the track -TEST_F(LOSTests, T09_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be 0 - EXPECT_NEAR(psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(theta_d, 0.0); - EXPECT_GT(theta_d, -1.57); -} - -// Test commanded angles when drone is over and to the right of the track -TEST_F(LOSTests, T10_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(psi_d, 0.0); - EXPECT_GT(psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(theta_d, 0.0); - EXPECT_GT(theta_d, -1.57); -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} From ee303e7e5f7b7ea86c01ca2a3a3a3b462afde39f Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 9 Nov 2025 17:55:05 +0100 Subject: [PATCH 10/87] complete the restructuring of ros node --- guidance/los_guidance/CMakeLists.txt | 12 +- .../los_guidance/config/guidance_params.yaml | 35 +- .../include/los_guidance/lib/adaptive_los.hpp | 4 +- .../include/los_guidance/lib/integral_los.hpp | 32 +- .../los_guidance/lib/proportional_los.hpp | 5 +- .../include/los_guidance/lib/types.hpp | 14 +- .../include/los_guidance/los_guidance_ros.hpp | 168 +++---- .../launch/los_guidance.launch.py | 5 +- .../los_guidance/src/lib/adaptive_los.cpp | 17 +- .../los_guidance/src/lib/integral_los.cpp | 42 +- .../los_guidance/src/lib/proportional_los.cpp | 14 +- .../los_guidance/src/los_guidance_node.cpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 434 ++++++++++-------- guidance/los_guidance/test/CMakeLists.txt | 10 +- .../los_guidance/test/adaptive_los_test.cpp | 30 +- .../los_guidance/test/integral_los_test.cpp | 110 +++++ .../test/proportional_los_test.cpp | 23 +- guidance/los_guidance/test/test_main.cpp | 8 + 18 files changed, 609 insertions(+), 356 deletions(-) create mode 100644 guidance/los_guidance/test/test_main.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 34996951e..b9f48e915 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -18,12 +18,14 @@ find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) +find_package(yaml-cpp REQUIRED) + include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED - src/lib/proportional_los.cpp + src/lib/proportional_los.cpp src/lib/integral_los.cpp src/lib/adaptive_los.cpp src/los_guidance_ros.cpp @@ -44,7 +46,10 @@ add_executable(los_guidance_node src/los_guidance_node.cpp ) -target_link_libraries(los_guidance_node ${LIB_NAME}) +target_link_libraries(los_guidance_node + ${LIB_NAME} + yaml-cpp +) install(TARGETS ${LIB_NAME} @@ -61,7 +66,7 @@ install(TARGETS install( DIRECTORY include/ - DESTINATION include + DESTINATION include/ ) install(DIRECTORY @@ -71,6 +76,7 @@ install(DIRECTORY ) if(BUILD_TESTING) + include(CTest) add_subdirectory(test) endif() diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index ebee79903..c65637117 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,10 +1,25 @@ -/**: - ros__parameters: - los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 - goal_reached_tol: 1.0 \ No newline at end of file +adaptive_los: + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + gamma_h: 0.05 + gamma_v: 0.1 + +prop_los: + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + k_p_h: 0.5 + k_p_v: 0.5 + +integer_los: + lookahead_distance_h: 1.5 + lookahead_distance_v: 0.6 + k_p_h: 0.5 + k_p_v: 0.5 + K_i_h: 0.1 + K_i_v: 0.1 + +common: + los_method: "ADAPTIVE" + u_desired: 0.3 + goal_reached_tol: 1.0 + \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index ee655ccb8..9282fc8b2 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -5,7 +5,7 @@ #include #include "los_guidance/lib/types.hpp" #include - + /** * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 * in "Fossen 2024 Lecture on 2D and 3D path-following control". @@ -26,7 +26,7 @@ namespace vortex::guidance::los { AdaptiveLOSGuidance(const AdaptiveLosParams& params); ~AdaptiveLOSGuidance() = default; - types::Output calculate_outputs(const types::Inputs& inputs); + types::Outputs calculate_outputs(const types::Inputs& inputs); private: void update_angles(const types::Inputs& inputs); diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index 29a750e98..3fbf122df 100755 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -4,28 +4,40 @@ #include #include #include "los_guidance/lib/types.hpp" -#include - +#include + namespace vortex::guidance::los { - struct IntergalLosParams { + struct IntegralLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; + double k_p_h{}; + double k_p_v{}; + double k_i_h{}; + double k_i_v{}; double time_step{}; }; - class IntergalLOSGuidance { + class IntegralLOSGuidance { public: - IntergalLOSGuidance(const IntergalLosParams& params); - ~IntergalLOSGuidance() = default; + IntegralLOSGuidance(const IntegralLosParams& params); + ~IntegralLOSGuidance() = default; + + types::Outputs calculate_outputs(const types::Inputs& inputs); private: void update_angles(const types::Inputs& inputs); - types::CrossTrackError calculate_crosstrack_error(const tyoes::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs); + + IntegralLosParams m_params{}; - IntergalLosParams m_params{}; + double int_h{}; + double int_v{}; + //again i dont know if i should have them here or just in the functions + double pi_h_{}; + double pi_v_{}; + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index 730646758..fad30eecc 100755 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -5,13 +5,14 @@ #include #include "los_guidance/lib/types.hpp" #include - + namespace vortex::guidance::los { struct ProportionalLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; double k_p_h{}; double k_p_v{}; + double time_step{}; }; class ProportionalLOSGuidance { @@ -19,7 +20,7 @@ namespace vortex::guidance::los { ProportionalLOSGuidance(const ProportionalLosParams& params); ~ProportionalLOSGuidance() = default; - tyes::Output calculate_outputs(const types::Inputs& inputs); + types::Outputs calculate_outputs(const types::Inputs& inputs); private: void update_angles(const types::Inputs& inputs); diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index c0b53fcf0..fd2340bf2 100755 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -3,9 +3,12 @@ #include #include +#include +#include #include namespace vortex::guidance::los::types{ + struct Point { double x{}; double y{}; @@ -16,6 +19,13 @@ namespace vortex::guidance::los::types{ } Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + + static Point point_from_ros( + const geometry_msgs::msg::Point& msg) { + return Point{msg.x, msg.y, msg.z}; + } + + }; struct CrossTrackError { @@ -28,7 +38,7 @@ namespace vortex::guidance::los::types{ } }; - struct Output { + struct Outputs { double psi_d{}; double theta_d{}; }; @@ -39,7 +49,7 @@ namespace vortex::guidance::los::types{ Point current_position{}; }; - enum class Active_LOSMethod { + enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index c12838ba4..4c9ca3f7b 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -5,113 +5,123 @@ #include #include #include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" #include #include #include +#include #include #include +#include -namespace vortex::guidance { - -class LOSGuidanceNode : public rclcpp::Node { - public: - explicit LOSGuidanceNode(); - - private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); - - // @brief Set the action server - void set_action_server(); - - // @brief Set the adaptive LOS guidance parameters - void set_adaptive_los_guidance(); - - // @brief Callback for the waypoint topic - // @param msg The reference message - void waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr msg); - - // @brief Callback for the pose topic - // @param msg The pose message - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal); - - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle); +namespace vortex::guidance::los { + + class LosGuidanceNode : public rclcpp::Node { + public: + explicit LosGuidanceNode(); + + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); + + // @brief Set the action server + void set_action_server(); + + // @brief Determine the LOS mode service + void set_los_mode_service(); + + // @brief Set the adaptive LOS guidance parameters + void set_adaptive_los_guidance(YAML::Node config); - // @brief Handle the accepted request - // @param goal_handle The goal handle - void handle_accepted(const std::shared_ptr> goal_handle); + // @brief Set the proportional LOS guidance parameters + void set_proportional_los_guidance(YAML::Node config); - // @brief Execute the goal - // @param goal_handle The goal handle - void execute(const std::shared_ptr> goal_handle); + // @brief Set the integral LOS guidance parameters + void set_integral_los_guidance(YAML::Node config); - // @brief Fill the lost waypoints - // @param goal The goal message - void fill_los_waypoints( - const geometry_msgs::msg::PointStamped& los_waypoint); + // @brief Callback for the waypoint topic + // @param msg The reference message + void waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr msg); - vortex_msgs::msg::LOSGuidance fill_los_reference(); + // @brief Callback for the pose topic + // @param msg The pose message + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - rclcpp_action::Server::SharedPtr - action_server_; + // @brief Handle the goal request + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); - rclcpp::Publisher::SharedPtr reference_pub_; + // @brief Handle the cancel request + // @param goal_handle The goal handle + // @return The cancel response + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle); - rclcpp::Subscription::SharedPtr - waypoint_sub_; + // @brief Handle the accepted request + // @param goal_handle The goal handle + void handle_accepted(const std::shared_ptr> goal_handle); - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + // @brief Execute the goal + // @param goal_handle The goal handle + void execute(const std::shared_ptr> goal_handle); - rclcpp::TimerBase::SharedPtr reference_pub_timer_; + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); - std::chrono::milliseconds time_step_; + YAML::Node get_los_config(std::string yaml_file_path); - std::mutex mutex_; + void parse_common_config(YAML::Node common_config); - rclcpp_action::GoalUUID preempted_goal_id_; + rclcpp_action::Server::SharedPtr + action_server_; - std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle_; + rclcpp::Service::SharedPtr los_mode_service_; - rclcpp::CallbackGroup::SharedPtr cb_group_; + rclcpp::Publisher::SharedPtr reference_pub_; - LOS::Point eta_; + rclcpp::Subscription::SharedPtr + waypoint_sub_; - LOS::Point last_point_; + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - LOS::Point next_point_; + rclcpp::TimerBase::SharedPtr reference_pub_timer_; + + std::chrono::milliseconds time_step_; + + std::mutex mutex_; + + rclcpp_action::GoalUUID preempted_goal_id_; + + std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle_; - std::unique_ptr adaptive_los_guidance_; + rclcpp::CallbackGroup::SharedPtr cb_group_; - double yaw_d_{}; + types::Inputs path_inputs_{}; - double pitch_d_{}; + double u_desired_{}; - double u_desired_{}; + double goal_reached_tol_{}; - double goal_reached_tol_{}; -}; + std::unique_ptr m_adaptive_los{}; + std::unique_ptr m_integral_los{}; + std::unique_ptr m_proportional_los{}; + types::ActiveLosMethod m_method{}; + }; -} // namespace vortex::guidance +} // namespace vortex::guidance::los #endif // LOS_GUIDANCE_ROS_HPP diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 03366e897..f726983f0 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -4,7 +4,7 @@ from launch import LaunchDescription from launch_ros.actions import Node -adapt_params = path.join( +los_params = path.join( get_package_share_directory("los_guidance"), "config", "guidance_params.yaml", @@ -25,7 +25,8 @@ def generate_launch_description(): namespace="orca", parameters=[ orca_params, - adapt_params, + {"los_config_file": los_params}, + {"time_step": 0.01}, ], output="screen", ) diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 86c9763f6..67f282fd4 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -1,4 +1,5 @@ -#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/types.hpp" +#include namespace vortex::guidance::los { @@ -7,7 +8,7 @@ namespace vortex::guidance::los { void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; const double dy = inputs.next_point.y - inputs.prev_point.y; - const double dz = inputs.next_point.z - inputs.prev_point.z; + const double dz = inputs.next_point.z - inputs.prev_point.z; pi_h_ = std::atan2(dy, dx); pi_v_ = std::atan2(-dz, std::sqrt(dx*dx + dy*dy)); @@ -16,7 +17,7 @@ namespace vortex::guidance::los { rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); } - types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) const { + const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) { const types::Point difference = inputs.current_position - inputs.prev_point; const Eigen::Vector3d difference_vector = difference.as_vector(); @@ -40,16 +41,16 @@ namespace vortex::guidance::los { alpha_c_hat_ += alpha_dot * m_params.time_step; } - types::Output AdaptiveLOSGuidance::calculate_outputs(const types::Inputs& inputs) { + types::Outputs AdaptiveLOSGuidance::calculate_outputs(const types::Inputs& inputs) { - update_angles(inputs) + update_angles(inputs); const types::CrossTrackError e = calculate_crosstrack_error(inputs); update_adaptive_estimates(e); - const double psi_d = pi_h_ - beta_c_hat_ - std::atan(e.y_e / params_.lookahead_distance_h); - const double theta_d = pi_v_ + alpha_c_hat_ + std::atan(e.z_e / params_.lookahead_distance_v); + const double psi_d = pi_h_ - beta_c_hat_ - std::atan(e.y_e / m_params.lookahead_distance_h); + const double theta_d = pi_v_ + alpha_c_hat_ + std::atan(e.z_e / m_params.lookahead_distance_v); - return types::Output{psi_d, theta_d}; + return types::Outputs{psi_d, theta_d}; } diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index 847f0d408..2a088d6b0 100755 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -1 +1,41 @@ - #include "los_guidance/lib/integral_los.hpp" \ No newline at end of file + #include + +namespace vortex::guidance::los { + + IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params): m_params{params} {} + + void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + } + + types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) { + + const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); + const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; + + return types::CrossTrackError::from_vector(e_perp); + } + + types::Outputs IntegralLOSGuidance::calculate_outputs(const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError e = calculate_crosstrack_error(inputs); + + int_h += e.y_e * m_params.time_step; + int_v += e.z_e * m_params.time_step; + + const double u_h = m_params.k_p_h * e.y_e + m_params.k_i_h * int_h; + const double u_v = m_params.k_p_v * e.z_e + m_params.k_i_v * int_v; + + const double psi_d = pi_h_ - std::atan(u_h); + const double theta_d = pi_v_ + std::atan(u_v); + + return types::Outputs{psi_d, theta_d}; + } + +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index e94cf57e0..f50dc81bf 100755 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -1,4 +1,4 @@ - #include "los_guidance/lib/proportional_los.hpp" + #include namespace vortex::guidance::los { @@ -12,27 +12,27 @@ namespace vortex::guidance::los { rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); - } + } types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) const { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d e_perp = rotation_y_.transpose() * rotation_z_.transpose() * diff_vec; + const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; return types::CrossTrackError::from_vector(e_perp); } - types::Output ProportionalLOSGuidance::calculate_outputs(const types::Inputs& inputs) { + types::Outputs ProportionalLOSGuidance::calculate_outputs(const types::Inputs& inputs) { update_angles(inputs); const types::CrossTrackError e = calculate_crosstrack_error(inputs); - const double k_p_h = 1.0 / std::max(params_.lookahead_distance_h, 1e-9); - const double k_p_v = 1.0 / std::max(params_.lookahead_distance_v, 1e-9); + const double k_p_h = 1.0 / std::max(m_params.lookahead_distance_h, 1e-9); + const double k_p_v = 1.0 / std::max(m_params.lookahead_distance_v, 1e-9); const double psi_d = pi_h_ - std::atan(k_p_h * e.y_e); const double theta_d = pi_v_ + std::atan(k_p_v * e.z_e); - return types::Output{psi_d, theta_d}; + return types::Outputs{psi_d, theta_d}; } } // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp index 931a57ded..439730a0c 100644 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ b/guidance/los_guidance/src/los_guidance_node.cpp @@ -4,7 +4,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 18884bb22..5d0e484bb 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,6 +1,9 @@ #include "los_guidance/los_guidance_ros.hpp" +#include "los_guidance/lib/types.hpp" #include +#include #include +#include const auto start_message = R"( _ ___ ____ ____ _ _ @@ -11,227 +14,268 @@ const auto start_message = R"( )"; -namespace vortex::guidance { - -LOSGuidanceNode::LOSGuidanceNode() : Node("los_guidance_node") { - time_step_ = std::chrono::milliseconds(10); - - set_subscribers_and_publisher(); - - set_action_server(); - - set_adaptive_los_guidance(); - - spdlog::info(start_message); -} - -void LOSGuidanceNode::set_subscribers_and_publisher() { - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.guidance.los"); - this->declare_parameter("topics.waypoint"); - - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - std::string guidance_topic = - this->get_parameter("topics.guidance.los").as_string(); - std::string waypoint_topic = - this->get_parameter("topics.waypoint").as_string(); - - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - - reference_pub_ = this->create_publisher( - guidance_topic, qos_sensor_data); - - waypoint_sub_ = this->create_subscription( - waypoint_topic, qos_sensor_data, - std::bind(&LOSGuidanceNode::waypoint_callback, this, - std::placeholders::_1)); - - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&LOSGuidanceNode::pose_callback, this, - std::placeholders::_1)); -} - -void LOSGuidanceNode::set_action_server() { - this->declare_parameter("action_servers.los"); - std::string action_server_name = - this->get_parameter("action_servers.los").as_string(); - cb_group_ = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - action_server_ = - rclcpp_action::create_server( - this, action_server_name, - std::bind(&LOSGuidanceNode::handle_goal, this, - std::placeholders::_1, std::placeholders::_2), - std::bind(&LOSGuidanceNode::handle_cancel, this, - std::placeholders::_1), - std::bind(&LOSGuidanceNode::handle_accepted, this, - std::placeholders::_1), - rcl_action_server_get_default_options(), cb_group_); -} - -void LOSGuidanceNode::set_adaptive_los_guidance() { - this->declare_parameter("los.lookahead_distance_h"); - this->declare_parameter("los.lookahead_distance_v"); - this->declare_parameter("los.gamma_h"); - this->declare_parameter("los.gamma_v"); - this->declare_parameter("los.time_step"); - this->declare_parameter("los.u_desired"); - this->declare_parameter("los.goal_reached_tol"); - - LOS::Params params; - params.lookahead_distance_h = - this->get_parameter("los.lookahead_distance_h").as_double(); - params.lookahead_distance_v = - this->get_parameter("los.lookahead_distance_v").as_double(); - params.gamma_h = this->get_parameter("los.gamma_h").as_double(); - params.gamma_v = this->get_parameter("los.gamma_v").as_double(); - params.time_step = this->get_parameter("los.time_step").as_double(); - - adaptive_los_guidance_ = std::make_unique(params); -} - -void LOSGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { - fill_los_waypoints(*los_waypoint); - adaptive_los_guidance_->update_angles(last_point_, next_point_); - spdlog::info("Received waypoint"); -} - -void LOSGuidanceNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose) { - eta_.x = current_pose->pose.pose.position.x; - eta_.y = current_pose->pose.pose.position.y; - eta_.z = current_pose->pose.pose.position.z; -} - -rclcpp_action::GoalResponse LOSGuidanceNode::handle_goal( - const rclcpp_action::GoalUUID&, - std::shared_ptr goal) { - (void)goal; - { - std::lock_guard lock(mutex_); - if (goal_handle_) { - if (goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } - } +namespace vortex::guidance::los{ + + LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node"){ + double time_step_s = this->declare_parameter("time_step"); + time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); + //auto config = this->declare_parameter("los_config_file"); + + + YAML::Node config = get_los_config("config/guidance_params.yaml"); + + parse_common_config(config["common"]); + set_subscribers_and_publisher(); + set_action_server(); + //set_los_mode_service(); + set_adaptive_los_guidance(config); + set_proportional_los_guidance(config); + set_integral_los_guidance(config); + + spdlog::info(start_message); + } + + void LosGuidanceNode::set_subscribers_and_publisher() { + + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.guidance.los"); + this->declare_parameter("topics.waypoint"); + + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + std::string guidance_topic = + this->get_parameter("topics.guidance.los").as_string(); + std::string waypoint_topic = + this->get_parameter("topics.waypoint").as_string(); + + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + + reference_pub_ = this->create_publisher( + guidance_topic, qos_sensor_data); + + waypoint_sub_ = this->create_subscription( + waypoint_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::waypoint_callback, this, + std::placeholders::_1)); + + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::pose_callback, this, + std::placeholders::_1)); } - spdlog::info("Accepted goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse LOSGuidanceNode::handle_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - spdlog::info("Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; -} - -void LOSGuidanceNode::handle_accepted( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - execute(goal_handle); -} - -void LOSGuidanceNode::fill_los_waypoints( - const geometry_msgs::msg::PointStamped& los_waypoint) { - last_point_.x = eta_.x; - last_point_.y = eta_.y; - last_point_.z = eta_.z; - - next_point_.x = los_waypoint.point.x; - next_point_.y = los_waypoint.point.y; - next_point_.z = los_waypoint.point.z; -} - -vortex_msgs::msg::LOSGuidance LOSGuidanceNode::fill_los_reference() { - vortex_msgs::msg::LOSGuidance reference_msg; - reference_msg.pitch = pitch_d_; - reference_msg.yaw = yaw_d_; - reference_msg.surge = u_desired_; - - return reference_msg; -} - -void LOSGuidanceNode::execute( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - { - std::lock_guard lock(mutex_); - this->goal_handle_ = goal_handle; + + void LosGuidanceNode::set_action_server() { + this->declare_parameter("action_servers.los"); + std::string action_server_name = + this->get_parameter("action_servers.los").as_string(); + cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + action_server_ = + rclcpp_action::create_server( + this, action_server_name, + std::bind(&LosGuidanceNode::handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&LosGuidanceNode::handle_cancel, this, + std::placeholders::_1), + std::bind(&LosGuidanceNode::handle_accepted, this, + std::placeholders::_1), + rcl_action_server_get_default_options(), cb_group_); } - u_desired_ = this->get_parameter("los.u_desired").as_double(); - goal_reached_tol_ = this->get_parameter("los.goal_reached_tol").as_double(); + + + void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { + auto adaptive_los_config = config["adaptive_los"]; + auto params = AdaptiveLosParams{}; + params.lookahead_distance_h = + adaptive_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + adaptive_los_config["lookahead_distance_v"].as(); + params.gamma_h = + adaptive_los_config["gama_h"].as(); + params.gamma_v = + adaptive_los_config["gama_v"].as(); + params.time_step = + adaptive_los_config["time_step"].as(); + + m_adaptive_los = std::make_unique(params); + } - spdlog::info("Executing goal"); + void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { + auto proportional_los_config = config["proportional_los"]; + auto params = ProportionalLosParams{}; + params.lookahead_distance_h = + proportional_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + proportional_los_config["lookahead_distance_v"].as(); + params.k_p_h = proportional_los_config["k_p_h"].as(); + params.k_p_v = proportional_los_config["k_p_v"].as(); + params.time_step = + proportional_los_config["time_step"].as(); + + m_proportional_los = std::make_unique(params); + } - const geometry_msgs::msg::PointStamped los_waypoint = - goal_handle->get_goal()->goal; + void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { + auto integral_los_config = config["integral_los"]; + auto params = IntegralLosParams{}; + params.lookahead_distance_h = + integral_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + integral_los_config["lookahead_distance_v"].as(); + params.k_p_h = integral_los_config["k_p_h"].as(); + params.k_p_v = integral_los_config["k_p_v"].as(); + params.k_i_h = integral_los_config["k_i_h"].as(); + params.k_i_v = integral_los_config["k_i_v"].as(); + params.time_step = + integral_los_config["time_step"].as(); + + m_integral_los = std::make_unique(params); + } - fill_los_waypoints(los_waypoint); + void LosGuidanceNode::waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { - adaptive_los_guidance_->update_angles(last_point_, next_point_); + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = types::Point::point_from_ros(los_waypoint->point); + spdlog::info("Received waypoint"); // remember to print waypoint that you get + } - auto feedback = - std::make_shared(); - auto result = std::make_shared(); + void LosGuidanceNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose) { - rclcpp::Rate loop_rate(1000.0 / time_step_.count()); + path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); + + } - while (rclcpp::ok()) { + rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( + const rclcpp_action::GoalUUID&, + std::shared_ptr goal) { + (void)goal; { std::lock_guard lock(mutex_); - if (goal_handle->get_goal_id() == preempted_goal_id_) { - result->success = false; - goal_handle->abort(result); - return; + if (goal_handle_) { + if (goal_handle_->is_active()) { + spdlog::info("Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); + } } } - if (goal_handle->is_canceling()) { - result->success = false; - goal_handle->canceled(result); - spdlog::info("Goal canceled"); - return; + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + spdlog::info("Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void LosGuidanceNode::handle_accepted( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + execute(goal_handle); + } + + vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference(types::Outputs outputs) { + vortex_msgs::msg::LOSGuidance reference_msg; + reference_msg.pitch = outputs.theta_d; + reference_msg.yaw = outputs.psi_d; + reference_msg.surge = u_desired_; + + return reference_msg; + } + + YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { + YAML::Node config = YAML::LoadFile(yaml_file_path); + return config; + } + + void LosGuidanceNode::parse_common_config(YAML::Node common_config) { + u_desired_ = common_config["u_desired"].as(); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); + } + + void LosGuidanceNode::execute( + + + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + { + std::lock_guard lock(mutex_); + this->goal_handle_ = goal_handle; } - LOS::CrossTrackError errors = - adaptive_los_guidance_->calculate_crosstrack_error(last_point_, - eta_); + spdlog::info("Executing goal"); + + const geometry_msgs::msg::PointStamped los_waypoint = goal_handle->get_goal()->goal; - yaw_d_ = adaptive_los_guidance_->calculate_psi_d(errors.y_e); - pitch_d_ = adaptive_los_guidance_->calculate_theta_d(errors.z_e); + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = types::Point::point_from_ros(los_waypoint.point); - adaptive_los_guidance_->update_adaptive_estimates(errors); + auto feedback = std::make_shared(); + auto result = std::make_shared(); - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); + rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - feedback->feedback = reference_msg; + while (rclcpp::ok()) { + { + std::lock_guard lock(mutex_); + if (goal_handle->get_goal_id() == preempted_goal_id_) { + result->success = false; + goal_handle->abort(result); + return; + } + } + + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } + + types::Outputs outputs; + + switch(m_method) { + case types::ActiveLosMethod::ADAPTIVE: + outputs = m_adaptive_los->calculate_outputs(path_inputs_); + break; + case types::ActiveLosMethod::PROPORTIONAL: + outputs = m_proportional_los->calculate_outputs(path_inputs_); + break; + case types::ActiveLosMethod::INTEGRAL: + outputs = m_integral_los->calculate_outputs(path_inputs_); + break; + default: + spdlog::error("Invalid LOS method selected"); + result->success = false; + goal_handle->abort(result); + return; + } + + vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(outputs); + feedback->feedback = reference_msg; - goal_handle->publish_feedback(feedback); - reference_pub_->publish(reference_msg); + goal_handle->publish_feedback(feedback); + reference_pub_->publish(reference_msg); - if ((eta_ - next_point_).as_vector().norm() < goal_reached_tol_) { + if ((path_inputs_.current_position - path_inputs_.next_point).as_vector().norm() < goal_reached_tol_) { result->success = true; goal_handle->succeed(result); - u_desired_ = 0.0; - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); - reference_pub_->publish(reference_msg); spdlog::info("Goal reached"); return; } - loop_rate.sleep(); - } -} + loop_rate.sleep(); + } + } -} // namespace vortex::guidance +} // namespace vortex::guidance diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index f2b81bdc2..e1552c071 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -6,17 +6,21 @@ include(GoogleTest) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) add_executable( ${TEST_BINARY_NAME} + test_main.cpp adaptive_los_test.cpp proportional_los_test.cpp integral_los_test.cpp ) -target_link_libraries( - ${TEST_BINARY_NAME} +target_link_libraries(${TEST_BINARY_NAME} PRIVATE ${LIB_NAME} - GTest::GTest + yaml-cpp + Eigen3::Eigen + spdlog::spdlog + fmt::fmt + GTest::GTest ) ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp index a0195a79a..42b5800d8 100644 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -17,7 +17,7 @@ namespace vortex::guidance::los{ //new namespace added los return p; } - AdaptiveLOSGuidance los_; + AdaptiveLOSGuidance los_; const double tol = 1e-9; }; /* @@ -88,16 +88,16 @@ namespace vortex::guidance::los{ //new namespace added los */ // Test commanded angles when drone is to the right of the track - TEST_F(AdaptiveLosTests, T06_test_commanded_angles) { + TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.5, 0.0}; - const types::Output O = los_.calculate_outputs(inputs); + const types::Outputs O = los_.calculate_outputs(inputs); // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d 0.0); + EXPECT_LT(O.psi_d, 0.0); EXPECT_GT(O.psi_d, -1.57); // Pitch cmd should be zero @@ -105,13 +105,13 @@ namespace vortex::guidance::los{ //new namespace added los } // Test commanded angles when drone is to the left of the track - TEST_F(AdaptiveLosTests, T07_test_commanded_angles) { + TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, -0.5, 0.0}; - const types::Output O = los_.calculate_outputs(inputs); + const types::Outputs O = los_.calculate_outputs(inputs); // Heading cmd should be between 0 and pi/2 EXPECT_GT(O.psi_d, 0.0); @@ -121,13 +121,13 @@ namespace vortex::guidance::los{ //new namespace added los } // Test commanded angles when drone is under the track - TEST_F(AdaptiveLosTests, T08_test_commanded_angles) { + TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.0, 0.5}; - const types::Output O = los_.calculate_outputs(inputs); + const types::Outputs O = los_.calculate_outputs(inputs); // Heading cmd should be 0 EXPECT_NEAR(O.psi_d, 0.0, tol); @@ -137,13 +137,13 @@ namespace vortex::guidance::los{ //new namespace added los } // Test commanded angles when drone is over the track - TEST_F(AdaptiveLosTests, T09_test_commanded_angles) { + TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.0, -0.5}; - const types::Output O = los_.calculate_outputs(inputs); + const types::Outputs O = los_.calculate_outputs(inputs); // Heading cmd should be 0 EXPECT_NEAR(O.psi_d, 0.0, tol); @@ -154,13 +154,13 @@ namespace vortex::guidance::los{ //new namespace added los // Test commanded angles when drone is over and to the right of the track - TEST_F(AdaptiveLosTests, T10_test_commanded_angles) { + TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.5, -0.5}; - const types::Output O = los_.calculate_outputs(inputs); + const types::Outputs O = los_.calculate_outputs(inputs); // Heading cmd should be between -pi/2 and 0 EXPECT_LT(O.psi_d, 0.0); @@ -170,10 +170,6 @@ namespace vortex::guidance::los{ //new namespace added los EXPECT_GT(O.theta_d, -1.57); } -} // namespace vortex::guidance +} // namespace vortex::guidance::los -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp index e69de29bb..218e818c1 100755 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -0,0 +1,110 @@ +#include +#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" + +namespace vortex::guidance::los{ + + class IntegralLosTest : public ::testing::Test { + protected: + IntegralLosTest() : Ilos_{get_params()} {} + + IntegralLosParams get_params() { + IntegralLosParams params; + params.lookahead_distance_h = 1.0; + params.lookahead_distance_v = 1.0; + params.k_i_h = 0.1; // needs tuning + params.k_i_v = 0.1; // needs tuning + params.k_p_h = 0.667; // needs tuning + params.k_p_v = 0.582; // needs tuning + params.time_step = 0.01; + return params; + } + + IntegralLOSGuidance Ilos_; + const double tol = 1e-9; + }; + + // Test commanded angles when drone is to the right of the track + TEST_F(IntegralLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is to the left of the track + TEST_F(IntegralLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); + } + + // Test commanded angles when drone is under the track + TEST_F(IntegralLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); + } + + // Test commanded angles when drone is over the track + TEST_F(IntegralLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + + // Test commanded angles when drone is over and to the right of the track + + TEST_F(IntegralLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); + } + +} // namespace vortex::guidance diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp index de040614c..3c890cbcd 100755 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -1,5 +1,5 @@ #include -#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" namespace vortex::guidance::los{ @@ -7,7 +7,7 @@ namespace vortex::guidance::los{ protected: ProportionalLosTest() : Plos_{get_params()} {} - ProportionalLosTest get_params() { + ProportionalLosParams get_params() { ProportionalLosParams params; params.lookahead_distance_h = 10.0; params.lookahead_distance_v = 10.0; @@ -19,7 +19,7 @@ namespace vortex::guidance::los{ ProportionalLOSGuidance Plos_; const double tol = 1e-9; - }; + }; // Test commanded angles when drone is to the right of the track TEST_F(ProportionalLosTest, T06_test_commanded_angles) { @@ -28,10 +28,10 @@ namespace vortex::guidance::los{ inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.5, 0.0}; - const types::Output O = Plos_.calculate_outputs(inputs); + const types::Outputs O = Plos_.calculate_outputs(inputs); // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d 0.0); + EXPECT_LT(O.psi_d, 0.0); EXPECT_GT(O.psi_d, -1.57); // Pitch cmd should be zero @@ -45,7 +45,7 @@ namespace vortex::guidance::los{ inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, -0.5, 0.0}; - const types::Output O = Plos_.calculate_outputs(inputs); + const types::Outputs O = Plos_.calculate_outputs(inputs); // Heading cmd should be between 0 and pi/2 EXPECT_GT(O.psi_d, 0.0); @@ -61,7 +61,7 @@ namespace vortex::guidance::los{ inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.0, 0.5}; - const types::Output O = Plos_.calculate_outputs(inputs); + const types::Outputs O = Plos_.calculate_outputs(inputs); // Heading cmd should be 0 EXPECT_NEAR(O.psi_d, 0.0, tol); @@ -77,7 +77,7 @@ namespace vortex::guidance::los{ inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.0, -0.5}; - const types::Output O = Plos_.calculate_outputs(inputs); + const types::Outputs O = Plos_.calculate_outputs(inputs); // Heading cmd should be 0 EXPECT_NEAR(O.psi_d, 0.0, tol); @@ -94,7 +94,7 @@ namespace vortex::guidance::los{ inputs.next_point = types::Point{1.0, 0.0, 0.0}; inputs.current_position = types::Point{0.0, 0.5, -0.5}; - const types::Output O = Plos_.calculate_outputs(inputs); + const types::Outputs O = Plos_.calculate_outputs(inputs); // Heading cmd should be between -pi/2 and 0 EXPECT_LT(O.psi_d, 0.0); @@ -106,8 +106,3 @@ namespace vortex::guidance::los{ } // namespace vortex::guidance -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/guidance/los_guidance/test/test_main.cpp b/guidance/los_guidance/test/test_main.cpp new file mode 100644 index 000000000..6ace5b598 --- /dev/null +++ b/guidance/los_guidance/test/test_main.cpp @@ -0,0 +1,8 @@ +// test/test_main.cpp +#include + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} \ No newline at end of file From c687343baea52c6b11f2de4bb47cb90d1d175ca1 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 12 Nov 2025 14:45:44 +0100 Subject: [PATCH 11/87] Complete los switchin --- auv_setup/config/robots/orca.yaml | 3 + .../los_guidance/config/guidance_params.yaml | 6 +- .../include/los_guidance/lib/adaptive_los.hpp | 1 - .../include/los_guidance/lib/types.hpp | 6 +- .../include/los_guidance/los_guidance_ros.hpp | 8 +- .../launch/los_guidance.launch.py | 3 + .../los_guidance/src/los_guidance_ros.cpp | 75 ++++++++++++------- 7 files changed, 64 insertions(+), 38 deletions(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 47039a4b9..1fef5f08b 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -136,6 +136,9 @@ reference_filter: "reference_filter" los: "los_guidance" + services: # Maybe not the right place for this? + los_mode: "set_los_mode" + fsm: docking: docking_station_offset: -1.0 diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index c65637117..0708d38a4 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -15,11 +15,11 @@ integer_los: lookahead_distance_v: 0.6 k_p_h: 0.5 k_p_v: 0.5 - K_i_h: 0.1 - K_i_v: 0.1 + k_i_h: 0.1 + k_i_v: 0.1 common: - los_method: "ADAPTIVE" + active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive u_desired: 0.3 goal_reached_tol: 1.0 \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 9282fc8b2..3b18afb47 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -34,7 +34,6 @@ namespace vortex::guidance::los { void update_adaptive_estimates(const types::CrossTrackError& cross_track_error); AdaptiveLosParams m_params{}; - //usikker om disse skal vaere med i den nye strukturen? Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); double pi_h_{}; diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index fd2340bf2..d20f873f6 100755 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -50,9 +50,9 @@ namespace vortex::guidance::los::types{ }; enum class ActiveLosMethod { - PROPORTIONAL, - INTEGRAL, - ADAPTIVE + PROPORTIONAL, // 0 + INTEGRAL, // 1 + ADAPTIVE // 2 }; } // namespace vortex::guidance::los::types diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 4c9ca3f7b..f3bdea8bd 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #include @@ -30,7 +30,7 @@ namespace vortex::guidance::los { void set_action_server(); // @brief Determine the LOS mode service - void set_los_mode_service(); + void set_service_server(); // @brief Set the adaptive LOS guidance parameters void set_adaptive_los_guidance(YAML::Node config); @@ -77,6 +77,10 @@ namespace vortex::guidance::los { void execute(const std::shared_ptr> goal_handle); + void set_los_mode( + const std::shared_ptr request, + std::shared_ptr response); + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); YAML::Node get_los_config(std::string yaml_file_path); diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index f726983f0..a9f0b628b 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -31,3 +31,6 @@ def generate_launch_description(): output="screen", ) return LaunchDescription([los_guidance_node]) + +# remeber to make them able to swich in the middle of a mission and if you swirch methid the parameters should'nt be reloaded +# unless its a new section \ No newline at end of file diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 5d0e484bb..639325328 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -21,13 +21,15 @@ namespace vortex::guidance::los{ time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); //auto config = this->declare_parameter("los_config_file"); + const std::string yaml_path =this->declare_parameter("los_config_file"); - YAML::Node config = get_los_config("config/guidance_params.yaml"); + + YAML::Node config = get_los_config(yaml_path); parse_common_config(config["common"]); set_subscribers_and_publisher(); set_action_server(); - //set_los_mode_service(); + set_service_server(); set_adaptive_los_guidance(config); set_proportional_los_guidance(config); set_integral_los_guidance(config); @@ -83,53 +85,60 @@ namespace vortex::guidance::los{ rcl_action_server_get_default_options(), cb_group_); } - + void LosGuidanceNode::set_service_server(){ + this->declare_parameter("services.los_mode", "set_los_mode"); + std::string service_name = + this->get_parameter("services.los_mode").as_string(); + + los_mode_service_ = this->create_service( + service_name, + std::bind(&LosGuidanceNode::set_los_mode, this, + std::placeholders::_1, std::placeholders::_2)); + + } void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { - auto adaptive_los_config = config["adaptive_los"]; + auto adaptive_los_config = config["adaptive_los"]; spdlog::info("A0"); auto params = AdaptiveLosParams{}; params.lookahead_distance_h = - adaptive_los_config["lookahead_distance_h"].as(); + adaptive_los_config["lookahead_distance_h"].as(); spdlog::info("A1"); params.lookahead_distance_v = - adaptive_los_config["lookahead_distance_v"].as(); + adaptive_los_config["lookahead_distance_v"].as();spdlog::info("A2"); params.gamma_h = - adaptive_los_config["gama_h"].as(); + adaptive_los_config["gamma_h"].as();spdlog::info("A3"); params.gamma_v = - adaptive_los_config["gama_v"].as(); - params.time_step = - adaptive_los_config["time_step"].as(); + adaptive_los_config["gamma_v"].as();spdlog::info("A4"); + params.time_step = static_cast(time_step_.count()) / 1000.0;spdlog::info("A5"); m_adaptive_los = std::make_unique(params); } void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { - auto proportional_los_config = config["proportional_los"]; + auto proportional_los_config = config["prop_los"];spdlog::info("P0"); auto params = ProportionalLosParams{}; params.lookahead_distance_h = - proportional_los_config["lookahead_distance_h"].as(); + proportional_los_config["lookahead_distance_h"].as();spdlog::info("P1"); params.lookahead_distance_v = - proportional_los_config["lookahead_distance_v"].as(); - params.k_p_h = proportional_los_config["k_p_h"].as(); - params.k_p_v = proportional_los_config["k_p_v"].as(); - params.time_step = - proportional_los_config["time_step"].as(); + proportional_los_config["lookahead_distance_v"].as();spdlog::info("P2"); + params.k_p_h = proportional_los_config["k_p_h"].as();spdlog::info("P3"); + params.k_p_v = proportional_los_config["k_p_v"].as();spdlog::info("P4"); + params.time_step = static_cast(time_step_.count()) / 1000.0; spdlog::info("P5"); m_proportional_los = std::make_unique(params); } void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { - auto integral_los_config = config["integral_los"]; + auto integral_los_config = config["integer_los"];spdlog::info("I0"); auto params = IntegralLosParams{}; params.lookahead_distance_h = - integral_los_config["lookahead_distance_h"].as(); + integral_los_config["lookahead_distance_h"].as();spdlog::info("I1"); params.lookahead_distance_v = - integral_los_config["lookahead_distance_v"].as(); - params.k_p_h = integral_los_config["k_p_h"].as(); - params.k_p_v = integral_los_config["k_p_v"].as(); - params.k_i_h = integral_los_config["k_i_h"].as(); - params.k_i_v = integral_los_config["k_i_v"].as(); - params.time_step = - integral_los_config["time_step"].as(); + integral_los_config["lookahead_distance_v"].as();spdlog::info("I2"); + params.k_p_h = integral_los_config["k_p_h"].as();spdlog::info("I3"); + params.k_p_v = integral_los_config["k_p_v"].as();spdlog::info("I4"); + params.k_i_h = integral_los_config["k_i_h"].as();spdlog::info("I5"); + params.k_i_v = integral_los_config["k_i_v"].as();spdlog::info("I6"); + params.time_step = static_cast(time_step_.count()) / 1000.0;spdlog::info("I7"); m_integral_los = std::make_unique(params); } @@ -182,7 +191,14 @@ namespace vortex::guidance::los{ goal_handle) { execute(goal_handle); } - + void LosGuidanceNode::set_los_mode( + const std::shared_ptr request, + std::shared_ptr response) { + + m_method = static_cast(request->mode); + spdlog::info("LOS mode set to {}", static_cast(m_method)); + response->success = true; + } vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference(types::Outputs outputs) { vortex_msgs::msg::LOSGuidance reference_msg; reference_msg.pitch = outputs.theta_d; @@ -198,8 +214,9 @@ namespace vortex::guidance::los{ } void LosGuidanceNode::parse_common_config(YAML::Node common_config) { - u_desired_ = common_config["u_desired"].as(); - goal_reached_tol_ = common_config["goal_reached_tol"].as(); + u_desired_ = common_config["u_desired"].as(); spdlog::info("C1"); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); spdlog::info("C2"); + m_method = static_cast(common_config["active_los_method"].as()); spdlog::info("C3"); } void LosGuidanceNode::execute( From 03560cf0204a231ee3d61d6fe09d8c8be033915e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 12 Nov 2025 15:08:04 +0000 Subject: [PATCH 12/87] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- guidance/los_guidance/CMakeLists.txt | 6 +- .../los_guidance/config/guidance_params.yaml | 2 +- .../include/los_guidance/lib/adaptive_los.hpp | 71 +-- .../include/los_guidance/lib/integral_los.hpp | 73 +-- .../los_guidance/lib/proportional_los.hpp | 63 +- .../include/los_guidance/lib/types.hpp | 79 ++- .../include/los_guidance/los_guidance_ros.hpp | 166 +++--- .../launch/los_guidance.launch.py | 5 +- .../los_guidance/src/lib/adaptive_los.cpp | 89 +-- .../los_guidance/src/lib/integral_los.cpp | 60 +- .../los_guidance/src/lib/proportional_los.cpp | 57 +- .../los_guidance/src/los_guidance_ros.cpp | 537 +++++++++--------- guidance/los_guidance/test/CMakeLists.txt | 4 +- .../los_guidance/test/adaptive_los_test.cpp | 332 ++++++----- .../los_guidance/test/integral_los_test.cpp | 210 +++---- .../test/proportional_los_test.cpp | 203 ++++--- guidance/los_guidance/test/test_main.cpp | 2 +- 17 files changed, 1000 insertions(+), 959 deletions(-) mode change 100755 => 100644 guidance/los_guidance/include/los_guidance/lib/integral_los.hpp mode change 100755 => 100644 guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp mode change 100755 => 100644 guidance/los_guidance/include/los_guidance/lib/types.hpp mode change 100755 => 100644 guidance/los_guidance/src/lib/integral_los.cpp mode change 100755 => 100644 guidance/los_guidance/src/lib/proportional_los.cpp mode change 100755 => 100644 guidance/los_guidance/test/integral_los_test.cpp mode change 100755 => 100644 guidance/los_guidance/test/proportional_los_test.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index b9f48e915..443e7cb51 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -25,7 +25,7 @@ include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED - src/lib/proportional_los.cpp + src/lib/proportional_los.cpp src/lib/integral_los.cpp src/lib/adaptive_los.cpp src/los_guidance_ros.cpp @@ -48,7 +48,7 @@ add_executable(los_guidance_node target_link_libraries(los_guidance_node ${LIB_NAME} - yaml-cpp + yaml-cpp ) install(TARGETS @@ -76,7 +76,7 @@ install(DIRECTORY ) if(BUILD_TESTING) - include(CTest) + include(CTest) add_subdirectory(test) endif() diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 0708d38a4..64f922a27 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -22,4 +22,4 @@ common: active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive u_desired: 0.3 goal_reached_tol: 1.0 - \ No newline at end of file + diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 3b18afb47..9b87a68cd 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -1,11 +1,11 @@ #ifndef ADAPTIVE_LOS_GUIDANCE_HPP #define ADAPTIVE_LOS_GUIDANCE_HPP +#include #include #include #include "los_guidance/lib/types.hpp" -#include - + /** * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 * in "Fossen 2024 Lecture on 2D and 3D path-following control". @@ -13,36 +13,37 @@ namespace vortex::guidance::los { - struct AdaptiveLosParams { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; - }; - - class AdaptiveLOSGuidance { - public: - AdaptiveLOSGuidance(const AdaptiveLosParams& params); - ~AdaptiveLOSGuidance() = default; - - types::Outputs calculate_outputs(const types::Inputs& inputs); - - private: - void update_angles(const types::Inputs& inputs); - const types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs); - void update_adaptive_estimates(const types::CrossTrackError& cross_track_error); - - AdaptiveLosParams m_params{}; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; - - - }; // namespace vortex::guidance::los - -} -#endif // LOS_GUIDANCE_HPP \ No newline at end of file +struct AdaptiveLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; +}; + +class AdaptiveLOSGuidance { + public: + AdaptiveLOSGuidance(const AdaptiveLosParams& params); + ~AdaptiveLOSGuidance() = default; + + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + void update_angles(const types::Inputs& inputs); + const types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs); + void update_adaptive_estimates( + const types::CrossTrackError& cross_track_error); + + AdaptiveLosParams m_params{}; + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + double pi_h_{}; + double pi_v_{}; + double beta_c_hat_{}; + double alpha_c_hat_{}; + +}; // namespace vortex::guidance::los + +} // namespace vortex::guidance::los +#endif // LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp old mode 100755 new mode 100644 index 3fbf122df..dbb437241 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -1,44 +1,45 @@ #ifndef INTEGRAL_LOS_GUIDANCE_HPP #define INTEGRAL_LOS_GUIDANCE_HPP +#include #include #include #include "los_guidance/lib/types.hpp" -#include - + namespace vortex::guidance::los { - struct IntegralLosParams { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double k_p_h{}; - double k_p_v{}; - double k_i_h{}; - double k_i_v{}; - double time_step{}; - }; - - class IntegralLOSGuidance { - public: - IntegralLOSGuidance(const IntegralLosParams& params); - ~IntegralLOSGuidance() = default; - - types::Outputs calculate_outputs(const types::Inputs& inputs); - - private: - void update_angles(const types::Inputs& inputs); - types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs); - - IntegralLosParams m_params{}; - - double int_h{}; - double int_v{}; - //again i dont know if i should have them here or just in the functions - double pi_h_{}; - double pi_v_{}; - Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; - Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; - }; -} - -#endif // INTEGRAL_LOS_GUIDANCE_HPP \ No newline at end of file +struct IntegralLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double k_p_h{}; + double k_p_v{}; + double k_i_h{}; + double k_i_v{}; + double time_step{}; +}; + +class IntegralLOSGuidance { + public: + IntegralLOSGuidance(const IntegralLosParams& params); + ~IntegralLOSGuidance() = default; + + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs); + + IntegralLosParams m_params{}; + + double int_h{}; + double int_v{}; + // again i dont know if i should have them here or just in the functions + double pi_h_{}; + double pi_v_{}; + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; +} // namespace vortex::guidance::los + +#endif // INTEGRAL_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp old mode 100755 new mode 100644 index fad30eecc..f5ff3d6d9 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -1,39 +1,40 @@ #ifndef PROPORTIONAL_LOS_GUIDANCE_HPP #define PROPORTIONAL_LOS_GUIDANCE_HPP +#include #include #include -#include "los_guidance/lib/types.hpp" -#include - +#include "los_guidance/lib/types.hpp" + namespace vortex::guidance::los { - struct ProportionalLosParams { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double k_p_h{}; - double k_p_v{}; - double time_step{}; - }; - - class ProportionalLOSGuidance { - public: - ProportionalLOSGuidance(const ProportionalLosParams& params); - ~ProportionalLOSGuidance() = default; - - types::Outputs calculate_outputs(const types::Inputs& inputs); - - private: - void update_angles(const types::Inputs& inputs); - types::CrossTrackError calculate_crosstrack_error(const types::Inputs& inputs) const; - - ProportionalLosParams m_params{}; - //again i dont know if i should have them here or just in the functions - double pi_h_{0.0}; - double pi_v_{0.0}; - Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; - Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; - }; - -} +struct ProportionalLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double k_p_h{}; + double k_p_v{}; + double time_step{}; +}; + +class ProportionalLOSGuidance { + public: + ProportionalLOSGuidance(const ProportionalLosParams& params); + ~ProportionalLOSGuidance() = default; + + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs) const; + + ProportionalLosParams m_params{}; + // again i dont know if i should have them here or just in the functions + double pi_h_{0.0}; + double pi_v_{0.0}; + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; + +} // namespace vortex::guidance::los #endif // PROPORTIONAL_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp old mode 100755 new mode 100644 index d20f873f6..5207855cc --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -1,60 +1,57 @@ #ifndef TYPES_HPP #define TYPES_HPP +#include #include #include #include #include -#include - -namespace vortex::guidance::los::types{ - - struct Point { - double x{}; - double y{}; - double z{}; - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } +namespace vortex::guidance::los::types { - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } +struct Point { + double x{}; + double y{}; + double z{}; - static Point point_from_ros( - const geometry_msgs::msg::Point& msg) { - return Point{msg.x, msg.y, msg.z}; - } + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } - }; + static Point point_from_ros(const geometry_msgs::msg::Point& msg) { + return Point{msg.x, msg.y, msg.z}; + } +}; - struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; +struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } - }; + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } +}; - struct Outputs { - double psi_d{}; - double theta_d{}; - }; +struct Outputs { + double psi_d{}; + double theta_d{}; +}; - struct Inputs{ - Point prev_point{}; - Point next_point{}; - Point current_position{}; - }; +struct Inputs { + Point prev_point{}; + Point next_point{}; + Point current_position{}; +}; - enum class ActiveLosMethod { - PROPORTIONAL, // 0 - INTEGRAL, // 1 - ADAPTIVE // 2 - }; +enum class ActiveLosMethod { + PROPORTIONAL, // 0 + INTEGRAL, // 1 + ADAPTIVE // 2 +}; -} // namespace vortex::guidance::los::types +} // namespace vortex::guidance::los::types -#endif \ No newline at end of file +#endif diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index f3bdea8bd..eb53d461c 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,130 +1,130 @@ #ifndef LOS_GUIDANCE_ROS_HPP #define LOS_GUIDANCE_ROS_HPP +#include #include #include #include -#include "los_guidance/lib/adaptive_los.hpp" -#include "los_guidance/lib/integral_los.hpp" -#include "los_guidance/lib/proportional_los.hpp" -#include "los_guidance/lib/types.hpp" #include #include #include -#include -#include +#include #include -#include +#include +#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { - class LosGuidanceNode : public rclcpp::Node { - public: - explicit LosGuidanceNode(); +class LosGuidanceNode : public rclcpp::Node { + public: + explicit LosGuidanceNode(); - private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); + private: + // @brief Set the subscribers and publishers + void set_subscribers_and_publisher(); - // @brief Set the action server - void set_action_server(); + // @brief Set the action server + void set_action_server(); - // @brief Determine the LOS mode service - void set_service_server(); + // @brief Determine the LOS mode service + void set_service_server(); - // @brief Set the adaptive LOS guidance parameters - void set_adaptive_los_guidance(YAML::Node config); + // @brief Set the adaptive LOS guidance parameters + void set_adaptive_los_guidance(YAML::Node config); - // @brief Set the proportional LOS guidance parameters - void set_proportional_los_guidance(YAML::Node config); + // @brief Set the proportional LOS guidance parameters + void set_proportional_los_guidance(YAML::Node config); - // @brief Set the integral LOS guidance parameters - void set_integral_los_guidance(YAML::Node config); + // @brief Set the integral LOS guidance parameters + void set_integral_los_guidance(YAML::Node config); - // @brief Callback for the waypoint topic - // @param msg The reference message - void waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr msg); + // @brief Callback for the waypoint topic + // @param msg The reference message + void waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr msg); - // @brief Callback for the pose topic - // @param msg The pose message - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + // @brief Callback for the pose topic + // @param msg The pose message + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal); + // @brief Handle the goal request + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle); + // @brief Handle the cancel request + // @param goal_handle The goal handle + // @return The cancel response + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle); - // @brief Handle the accepted request - // @param goal_handle The goal handle - void handle_accepted(const std::shared_ptr> goal_handle); + // @brief Handle the accepted request + // @param goal_handle The goal handle + void handle_accepted(const std::shared_ptr> goal_handle); - // @brief Execute the goal - // @param goal_handle The goal handle - void execute(const std::shared_ptr> goal_handle); + // @brief Execute the goal + // @param goal_handle The goal handle + void execute(const std::shared_ptr> goal_handle); - void set_los_mode( - const std::shared_ptr request, - std::shared_ptr response); + void set_los_mode( + const std::shared_ptr request, + std::shared_ptr response); - vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); - YAML::Node get_los_config(std::string yaml_file_path); + YAML::Node get_los_config(std::string yaml_file_path); - void parse_common_config(YAML::Node common_config); + void parse_common_config(YAML::Node common_config); - rclcpp_action::Server::SharedPtr - action_server_; + rclcpp_action::Server::SharedPtr + action_server_; - rclcpp::Service::SharedPtr los_mode_service_; + rclcpp::Service::SharedPtr los_mode_service_; - rclcpp::Publisher::SharedPtr reference_pub_; + rclcpp::Publisher::SharedPtr reference_pub_; - rclcpp::Subscription::SharedPtr - waypoint_sub_; + rclcpp::Subscription::SharedPtr + waypoint_sub_; - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - rclcpp::TimerBase::SharedPtr reference_pub_timer_; + rclcpp::TimerBase::SharedPtr reference_pub_timer_; - std::chrono::milliseconds time_step_; + std::chrono::milliseconds time_step_; - std::mutex mutex_; + std::mutex mutex_; - rclcpp_action::GoalUUID preempted_goal_id_; + rclcpp_action::GoalUUID preempted_goal_id_; - std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle_; + std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle_; - rclcpp::CallbackGroup::SharedPtr cb_group_; + rclcpp::CallbackGroup::SharedPtr cb_group_; - types::Inputs path_inputs_{}; + types::Inputs path_inputs_{}; - double u_desired_{}; + double u_desired_{}; - double goal_reached_tol_{}; + double goal_reached_tol_{}; - std::unique_ptr m_adaptive_los{}; - std::unique_ptr m_integral_los{}; - std::unique_ptr m_proportional_los{}; - types::ActiveLosMethod m_method{}; - }; + std::unique_ptr m_adaptive_los{}; + std::unique_ptr m_integral_los{}; + std::unique_ptr m_proportional_los{}; + types::ActiveLosMethod m_method{}; +}; } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index a9f0b628b..859c0a255 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -32,5 +32,6 @@ def generate_launch_description(): ) return LaunchDescription([los_guidance_node]) -# remeber to make them able to swich in the middle of a mission and if you swirch methid the parameters should'nt be reloaded -# unless its a new section \ No newline at end of file + +# remember to make them able to swich in the middle of a mission and if you swirch method the parameters shouldn't be reloaded +# unless its a new section diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 67f282fd4..319ce376c 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -1,57 +1,64 @@ -#include "los_guidance/lib/types.hpp" #include +#include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { - - AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params): m_params{params} {} - void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { - const double dx = inputs.next_point.x - inputs.prev_point.x; - const double dy = inputs.next_point.y - inputs.prev_point.y; - const double dz = inputs.next_point.z - inputs.prev_point.z; +AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) + : m_params{params} {} + +void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { + const double dx = inputs.next_point.x - inputs.prev_point.x; + const double dy = inputs.next_point.y - inputs.prev_point.y; + const double dz = inputs.next_point.z - inputs.prev_point.z; - pi_h_ = std::atan2(dy, dx); - pi_v_ = std::atan2(-dz, std::sqrt(dx*dx + dy*dy)); + pi_h_ = std::atan2(dy, dx); + pi_v_ = std::atan2(-dz, std::sqrt(dx * dx + dy * dy)); - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); - } + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} - const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) { +const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) { + const types::Point difference = inputs.current_position - inputs.prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); - const types::Point difference = inputs.current_position - inputs.prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); + const Eigen::Vector3d cross_track_error = + rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - const Eigen::Vector3d cross_track_error = - rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + return types::CrossTrackError::from_vector(cross_track_error); +} - return types::CrossTrackError::from_vector(cross_track_error); - } +void AdaptiveLOSGuidance::update_adaptive_estimates( + const types::CrossTrackError& e) { + const double denom_h = std::sqrt(m_params.lookahead_distance_h * + m_params.lookahead_distance_h + + e.y_e * e.y_e); + const double denom_v = std::sqrt(m_params.lookahead_distance_v * + m_params.lookahead_distance_v + + e.z_e * e.z_e); - void AdaptiveLOSGuidance::update_adaptive_estimates(const types::CrossTrackError& e) { - const double denom_h = std::sqrt( - m_params.lookahead_distance_h * m_params.lookahead_distance_h + e.y_e * e.y_e); - const double denom_v = std::sqrt( - m_params.lookahead_distance_v * m_params.lookahead_distance_v + e.z_e * e.z_e); + const double beta_dot = + m_params.gamma_h * (m_params.lookahead_distance_h / denom_h) * e.y_e; + const double alpha_dot = + m_params.gamma_v * (m_params.lookahead_distance_v / denom_v) * e.z_e; - const double beta_dot = m_params.gamma_h * (m_params.lookahead_distance_h / denom_h) * e.y_e; - const double alpha_dot = m_params.gamma_v * (m_params.lookahead_distance_v / denom_v) * e.z_e; + beta_c_hat_ += beta_dot * m_params.time_step; + alpha_c_hat_ += alpha_dot * m_params.time_step; +} - beta_c_hat_ += beta_dot * m_params.time_step; - alpha_c_hat_ += alpha_dot * m_params.time_step; - } - - types::Outputs AdaptiveLOSGuidance::calculate_outputs(const types::Inputs& inputs) { +types::Outputs AdaptiveLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError e = calculate_crosstrack_error(inputs); + update_adaptive_estimates(e); - update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); - update_adaptive_estimates(e); + const double psi_d = + pi_h_ - beta_c_hat_ - std::atan(e.y_e / m_params.lookahead_distance_h); + const double theta_d = + pi_v_ + alpha_c_hat_ + std::atan(e.z_e / m_params.lookahead_distance_v); - const double psi_d = pi_h_ - beta_c_hat_ - std::atan(e.y_e / m_params.lookahead_distance_h); - const double theta_d = pi_v_ + alpha_c_hat_ + std::atan(e.z_e / m_params.lookahead_distance_v); - - return types::Outputs{psi_d, theta_d}; - } + return types::Outputs{psi_d, theta_d}; +} - -} // namespace vortex::guidance +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp old mode 100755 new mode 100644 index 2a088d6b0..0ca1a3e2b --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -1,41 +1,47 @@ - #include +#include namespace vortex::guidance::los { - IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params): m_params{params} {} +IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) + : m_params{params} {} - void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { - const types::Point difference = inputs.next_point - inputs.prev_point; +void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + difference.y * difference.y)); - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); - } + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} - types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) { +types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * + diff_vec; - const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; + return types::CrossTrackError::from_vector(e_perp); +} - return types::CrossTrackError::from_vector(e_perp); - } +types::Outputs IntegralLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError e = calculate_crosstrack_error(inputs); - types::Outputs IntegralLOSGuidance::calculate_outputs(const types::Inputs& inputs) { - update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); + int_h += e.y_e * m_params.time_step; + int_v += e.z_e * m_params.time_step; - int_h += e.y_e * m_params.time_step; - int_v += e.z_e * m_params.time_step; + const double u_h = m_params.k_p_h * e.y_e + m_params.k_i_h * int_h; + const double u_v = m_params.k_p_v * e.z_e + m_params.k_i_v * int_v; - const double u_h = m_params.k_p_h * e.y_e + m_params.k_i_h * int_h; - const double u_v = m_params.k_p_v * e.z_e + m_params.k_i_v * int_v; + const double psi_d = pi_h_ - std::atan(u_h); + const double theta_d = pi_v_ + std::atan(u_v); - const double psi_d = pi_h_ - std::atan(u_h); - const double theta_d = pi_v_ + std::atan(u_v); + return types::Outputs{psi_d, theta_d}; +} - return types::Outputs{psi_d, theta_d}; - } - -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp old mode 100755 new mode 100644 index f50dc81bf..40a739e04 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -1,38 +1,45 @@ - #include +#include namespace vortex::guidance::los { - ProportionalLOSGuidance::ProportionalLOSGuidance(const ProportionalLosParams& params) : m_params{params} {} +ProportionalLOSGuidance::ProportionalLOSGuidance( + const ProportionalLosParams& params) + : m_params{params} {} - void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { - const types::Point difference = inputs.next_point - inputs.prev_point; +void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + difference.y * difference.y)); - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); - } + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} - types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error(const types::Inputs& inputs) const { +types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) const { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * + diff_vec; - const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; + return types::CrossTrackError::from_vector(e_perp); +} - return types::CrossTrackError::from_vector(e_perp); - } +types::Outputs ProportionalLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError e = calculate_crosstrack_error(inputs); - types::Outputs ProportionalLOSGuidance::calculate_outputs(const types::Inputs& inputs) { - update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); + const double k_p_h = 1.0 / std::max(m_params.lookahead_distance_h, 1e-9); + const double k_p_v = 1.0 / std::max(m_params.lookahead_distance_v, 1e-9); - const double k_p_h = 1.0 / std::max(m_params.lookahead_distance_h, 1e-9); - const double k_p_v = 1.0 / std::max(m_params.lookahead_distance_v, 1e-9); + const double psi_d = pi_h_ - std::atan(k_p_h * e.y_e); + const double theta_d = pi_v_ + std::atan(k_p_v * e.z_e); - const double psi_d = pi_h_ - std::atan(k_p_h * e.y_e); - const double theta_d = pi_v_ + std::atan(k_p_v * e.z_e); + return types::Outputs{psi_d, theta_d}; +} - return types::Outputs{psi_d, theta_d}; - } - -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 639325328..d365510a3 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,9 +1,9 @@ #include "los_guidance/los_guidance_ros.hpp" -#include "los_guidance/lib/types.hpp" #include +#include #include #include -#include +#include "los_guidance/lib/types.hpp" const auto start_message = R"( _ ___ ____ ____ _ _ @@ -14,285 +14,308 @@ const auto start_message = R"( )"; -namespace vortex::guidance::los{ - - LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node"){ - double time_step_s = this->declare_parameter("time_step"); - time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); - //auto config = this->declare_parameter("los_config_file"); - - const std::string yaml_path =this->declare_parameter("los_config_file"); - - - YAML::Node config = get_los_config(yaml_path); - - parse_common_config(config["common"]); - set_subscribers_and_publisher(); - set_action_server(); - set_service_server(); - set_adaptive_los_guidance(config); - set_proportional_los_guidance(config); - set_integral_los_guidance(config); - - spdlog::info(start_message); - } - - void LosGuidanceNode::set_subscribers_and_publisher() { - - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.guidance.los"); - this->declare_parameter("topics.waypoint"); - - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - std::string guidance_topic = - this->get_parameter("topics.guidance.los").as_string(); - std::string waypoint_topic = - this->get_parameter("topics.waypoint").as_string(); - - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); - - reference_pub_ = this->create_publisher( - guidance_topic, qos_sensor_data); - - waypoint_sub_ = this->create_subscription( - waypoint_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::waypoint_callback, this, - std::placeholders::_1)); - - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::pose_callback, this, - std::placeholders::_1)); - } - - void LosGuidanceNode::set_action_server() { - this->declare_parameter("action_servers.los"); - std::string action_server_name = - this->get_parameter("action_servers.los").as_string(); - cb_group_ = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - - action_server_ = - rclcpp_action::create_server( - this, action_server_name, - std::bind(&LosGuidanceNode::handle_goal, this, - std::placeholders::_1, std::placeholders::_2), - std::bind(&LosGuidanceNode::handle_cancel, this, - std::placeholders::_1), - std::bind(&LosGuidanceNode::handle_accepted, this, - std::placeholders::_1), - rcl_action_server_get_default_options(), cb_group_); - } - - void LosGuidanceNode::set_service_server(){ - this->declare_parameter("services.los_mode", "set_los_mode"); - std::string service_name = - this->get_parameter("services.los_mode").as_string(); - - los_mode_service_ = this->create_service( - service_name, - std::bind(&LosGuidanceNode::set_los_mode, this, - std::placeholders::_1, std::placeholders::_2)); - - } - - void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { - auto adaptive_los_config = config["adaptive_los"]; spdlog::info("A0"); - auto params = AdaptiveLosParams{}; - params.lookahead_distance_h = - adaptive_los_config["lookahead_distance_h"].as(); spdlog::info("A1"); - params.lookahead_distance_v = - adaptive_los_config["lookahead_distance_v"].as();spdlog::info("A2"); - params.gamma_h = - adaptive_los_config["gamma_h"].as();spdlog::info("A3"); - params.gamma_v = - adaptive_los_config["gamma_v"].as();spdlog::info("A4"); - params.time_step = static_cast(time_step_.count()) / 1000.0;spdlog::info("A5"); - - m_adaptive_los = std::make_unique(params); - } - - void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { - auto proportional_los_config = config["prop_los"];spdlog::info("P0"); - auto params = ProportionalLosParams{}; - params.lookahead_distance_h = - proportional_los_config["lookahead_distance_h"].as();spdlog::info("P1"); - params.lookahead_distance_v = - proportional_los_config["lookahead_distance_v"].as();spdlog::info("P2"); - params.k_p_h = proportional_los_config["k_p_h"].as();spdlog::info("P3"); - params.k_p_v = proportional_los_config["k_p_v"].as();spdlog::info("P4"); - params.time_step = static_cast(time_step_.count()) / 1000.0; spdlog::info("P5"); - - m_proportional_los = std::make_unique(params); - } - - void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { - auto integral_los_config = config["integer_los"];spdlog::info("I0"); - auto params = IntegralLosParams{}; - params.lookahead_distance_h = - integral_los_config["lookahead_distance_h"].as();spdlog::info("I1"); - params.lookahead_distance_v = - integral_los_config["lookahead_distance_v"].as();spdlog::info("I2"); - params.k_p_h = integral_los_config["k_p_h"].as();spdlog::info("I3"); - params.k_p_v = integral_los_config["k_p_v"].as();spdlog::info("I4"); - params.k_i_h = integral_los_config["k_i_h"].as();spdlog::info("I5"); - params.k_i_v = integral_los_config["k_i_v"].as();spdlog::info("I6"); - params.time_step = static_cast(time_step_.count()) / 1000.0;spdlog::info("I7"); - - m_integral_los = std::make_unique(params); - } - - void LosGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { - - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = types::Point::point_from_ros(los_waypoint->point); - spdlog::info("Received waypoint"); // remember to print waypoint that you get - } - - void LosGuidanceNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose) { - - path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); - - } - - rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( - const rclcpp_action::GoalUUID&, - std::shared_ptr goal) { - (void)goal; - { - std::lock_guard lock(mutex_); - if (goal_handle_) { - if (goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } +namespace vortex::guidance::los { + +LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { + double time_step_s = this->declare_parameter("time_step"); + time_step_ = + std::chrono::milliseconds(static_cast(time_step_s * 1000)); + // auto config = this->declare_parameter("los_config_file"); + + const std::string yaml_path = + this->declare_parameter("los_config_file"); + + YAML::Node config = get_los_config(yaml_path); + + parse_common_config(config["common"]); + set_subscribers_and_publisher(); + set_action_server(); + set_service_server(); + set_adaptive_los_guidance(config); + set_proportional_los_guidance(config); + set_integral_los_guidance(config); + + spdlog::info(start_message); +} + +void LosGuidanceNode::set_subscribers_and_publisher() { + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.guidance.los"); + this->declare_parameter("topics.waypoint"); + + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + std::string guidance_topic = + this->get_parameter("topics.guidance.los").as_string(); + std::string waypoint_topic = + this->get_parameter("topics.waypoint").as_string(); + + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + + reference_pub_ = this->create_publisher( + guidance_topic, qos_sensor_data); + + waypoint_sub_ = this->create_subscription( + waypoint_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::waypoint_callback, this, + std::placeholders::_1)); + + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::pose_callback, this, + std::placeholders::_1)); +} + +void LosGuidanceNode::set_action_server() { + this->declare_parameter("action_servers.los"); + std::string action_server_name = + this->get_parameter("action_servers.los").as_string(); + cb_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + action_server_ = + rclcpp_action::create_server( + this, action_server_name, + std::bind(&LosGuidanceNode::handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&LosGuidanceNode::handle_cancel, this, + std::placeholders::_1), + std::bind(&LosGuidanceNode::handle_accepted, this, + std::placeholders::_1), + rcl_action_server_get_default_options(), cb_group_); +} + +void LosGuidanceNode::set_service_server() { + this->declare_parameter("services.los_mode", "set_los_mode"); + std::string service_name = + this->get_parameter("services.los_mode").as_string(); + + los_mode_service_ = this->create_service( + service_name, std::bind(&LosGuidanceNode::set_los_mode, this, + std::placeholders::_1, std::placeholders::_2)); +} + +void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { + auto adaptive_los_config = config["adaptive_los"]; + spdlog::info("A0"); + auto params = AdaptiveLosParams{}; + params.lookahead_distance_h = + adaptive_los_config["lookahead_distance_h"].as(); + spdlog::info("A1"); + params.lookahead_distance_v = + adaptive_los_config["lookahead_distance_v"].as(); + spdlog::info("A2"); + params.gamma_h = adaptive_los_config["gamma_h"].as(); + spdlog::info("A3"); + params.gamma_v = adaptive_los_config["gamma_v"].as(); + spdlog::info("A4"); + params.time_step = static_cast(time_step_.count()) / 1000.0; + spdlog::info("A5"); + + m_adaptive_los = std::make_unique(params); +} + +void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { + auto proportional_los_config = config["prop_los"]; + spdlog::info("P0"); + auto params = ProportionalLosParams{}; + params.lookahead_distance_h = + proportional_los_config["lookahead_distance_h"].as(); + spdlog::info("P1"); + params.lookahead_distance_v = + proportional_los_config["lookahead_distance_v"].as(); + spdlog::info("P2"); + params.k_p_h = proportional_los_config["k_p_h"].as(); + spdlog::info("P3"); + params.k_p_v = proportional_los_config["k_p_v"].as(); + spdlog::info("P4"); + params.time_step = static_cast(time_step_.count()) / 1000.0; + spdlog::info("P5"); + + m_proportional_los = std::make_unique(params); +} + +void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { + auto integral_los_config = config["integer_los"]; + spdlog::info("I0"); + auto params = IntegralLosParams{}; + params.lookahead_distance_h = + integral_los_config["lookahead_distance_h"].as(); + spdlog::info("I1"); + params.lookahead_distance_v = + integral_los_config["lookahead_distance_v"].as(); + spdlog::info("I2"); + params.k_p_h = integral_los_config["k_p_h"].as(); + spdlog::info("I3"); + params.k_p_v = integral_los_config["k_p_v"].as(); + spdlog::info("I4"); + params.k_i_h = integral_los_config["k_i_h"].as(); + spdlog::info("I5"); + params.k_i_v = integral_los_config["k_i_v"].as(); + spdlog::info("I6"); + params.time_step = static_cast(time_step_.count()) / 1000.0; + spdlog::info("I7"); + + m_integral_los = std::make_unique(params); +} + +void LosGuidanceNode::waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = types::Point::point_from_ros(los_waypoint->point); + spdlog::info( + "Received waypoint"); // remember to print waypoint that you get +} + +void LosGuidanceNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose) { + path_inputs_.current_position = + types::Point::point_from_ros(current_pose->pose.pose.position); +} + +rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( + const rclcpp_action::GoalUUID&, + std::shared_ptr goal) { + (void)goal; + { + std::lock_guard lock(mutex_); + if (goal_handle_) { + if (goal_handle_->is_active()) { + spdlog::info("Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); } } - spdlog::info("Accepted goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } - - rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - spdlog::info("Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; + spdlog::info("Accepted goal request"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + spdlog::info("Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void LosGuidanceNode::handle_accepted( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + execute(goal_handle); +} +void LosGuidanceNode::set_los_mode( + const std::shared_ptr request, + std::shared_ptr response) { + m_method = static_cast(request->mode); + spdlog::info("LOS mode set to {}", static_cast(m_method)); + response->success = true; +} +vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( + types::Outputs outputs) { + vortex_msgs::msg::LOSGuidance reference_msg; + reference_msg.pitch = outputs.theta_d; + reference_msg.yaw = outputs.psi_d; + reference_msg.surge = u_desired_; + + return reference_msg; +} + +YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { + YAML::Node config = YAML::LoadFile(yaml_file_path); + return config; +} + +void LosGuidanceNode::parse_common_config(YAML::Node common_config) { + u_desired_ = common_config["u_desired"].as(); + spdlog::info("C1"); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); + spdlog::info("C2"); + m_method = static_cast( + common_config["active_los_method"].as()); + spdlog::info("C3"); +} + +void LosGuidanceNode::execute( + + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) { + { + std::lock_guard lock(mutex_); + this->goal_handle_ = goal_handle; } - void LosGuidanceNode::handle_accepted( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - execute(goal_handle); - } - void LosGuidanceNode::set_los_mode( - const std::shared_ptr request, - std::shared_ptr response) { - - m_method = static_cast(request->mode); - spdlog::info("LOS mode set to {}", static_cast(m_method)); - response->success = true; - } - vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference(types::Outputs outputs) { - vortex_msgs::msg::LOSGuidance reference_msg; - reference_msg.pitch = outputs.theta_d; - reference_msg.yaw = outputs.psi_d; - reference_msg.surge = u_desired_; + spdlog::info("Executing goal"); - return reference_msg; - } - - YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { - YAML::Node config = YAML::LoadFile(yaml_file_path); - return config; - } + const geometry_msgs::msg::PointStamped los_waypoint = + goal_handle->get_goal()->goal; - void LosGuidanceNode::parse_common_config(YAML::Node common_config) { - u_desired_ = common_config["u_desired"].as(); spdlog::info("C1"); - goal_reached_tol_ = common_config["goal_reached_tol"].as(); spdlog::info("C2"); - m_method = static_cast(common_config["active_los_method"].as()); spdlog::info("C3"); - } + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = types::Point::point_from_ros(los_waypoint.point); - void LosGuidanceNode::execute( + auto feedback = + std::make_shared(); + auto result = std::make_shared(); + rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { + while (rclcpp::ok()) { { std::lock_guard lock(mutex_); - this->goal_handle_ = goal_handle; + if (goal_handle->get_goal_id() == preempted_goal_id_) { + result->success = false; + goal_handle->abort(result); + return; + } } - spdlog::info("Executing goal"); - - const geometry_msgs::msg::PointStamped los_waypoint = goal_handle->get_goal()->goal; - - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = types::Point::point_from_ros(los_waypoint.point); - - auto feedback = std::make_shared(); - auto result = std::make_shared(); - - rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - - while (rclcpp::ok()) { - { - std::lock_guard lock(mutex_); - if (goal_handle->get_goal_id() == preempted_goal_id_) { - result->success = false; - goal_handle->abort(result); - return; - } - } + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + spdlog::info("Goal canceled"); + return; + } - if (goal_handle->is_canceling()) { + types::Outputs outputs; + + switch (m_method) { + case types::ActiveLosMethod::ADAPTIVE: + outputs = m_adaptive_los->calculate_outputs(path_inputs_); + break; + case types::ActiveLosMethod::PROPORTIONAL: + outputs = m_proportional_los->calculate_outputs(path_inputs_); + break; + case types::ActiveLosMethod::INTEGRAL: + outputs = m_integral_los->calculate_outputs(path_inputs_); + break; + default: + spdlog::error("Invalid LOS method selected"); result->success = false; - goal_handle->canceled(result); - spdlog::info("Goal canceled"); + goal_handle->abort(result); return; - } - - types::Outputs outputs; - - switch(m_method) { - case types::ActiveLosMethod::ADAPTIVE: - outputs = m_adaptive_los->calculate_outputs(path_inputs_); - break; - case types::ActiveLosMethod::PROPORTIONAL: - outputs = m_proportional_los->calculate_outputs(path_inputs_); - break; - case types::ActiveLosMethod::INTEGRAL: - outputs = m_integral_los->calculate_outputs(path_inputs_); - break; - default: - spdlog::error("Invalid LOS method selected"); - result->success = false; - goal_handle->abort(result); - return; - } + } - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(outputs); - feedback->feedback = reference_msg; + vortex_msgs::msg::LOSGuidance reference_msg = + fill_los_reference(outputs); + feedback->feedback = reference_msg; - goal_handle->publish_feedback(feedback); - reference_pub_->publish(reference_msg); + goal_handle->publish_feedback(feedback); + reference_pub_->publish(reference_msg); - if ((path_inputs_.current_position - path_inputs_.next_point).as_vector().norm() < goal_reached_tol_) { + if ((path_inputs_.current_position - path_inputs_.next_point) + .as_vector() + .norm() < goal_reached_tol_) { result->success = true; goal_handle->succeed(result); spdlog::info("Goal reached"); return; } - loop_rate.sleep(); - } - } + loop_rate.sleep(); + } +} -} // namespace vortex::guidance +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index e1552c071..8f5842651 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -10,7 +10,7 @@ add_executable( adaptive_los_test.cpp proportional_los_test.cpp integral_los_test.cpp - + ) target_link_libraries(${TEST_BINARY_NAME} @@ -20,7 +20,7 @@ target_link_libraries(${TEST_BINARY_NAME} Eigen3::Eigen spdlog::spdlog fmt::fmt - GTest::GTest + GTest::GTest ) ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp index 42b5800d8..5fd71027a 100644 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -1,175 +1,173 @@ -#include #include "los_guidance/lib/adaptive_los.hpp" +#include -namespace vortex::guidance::los{ //new namespace added los - - class AdaptiveLosTest : public ::testing::Test { - protected: - AdaptiveLosTest() : los_{get_params()} {} - - AdaptiveLosParams get_params() { - AdaptiveLosParams p; - p.lookahead_distance_h = 1.0; - p.lookahead_distance_v = 1.0; - p.gamma_h = 1.0; - p.gamma_v = 1.0; - p.time_step = 0.01; - return p; - } - - AdaptiveLOSGuidance los_; - const double tol = 1e-9; - }; - /* - TEST_F(AdaptiveLosTest, T01_test_cross_track_error_on_track){ - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); - } - - TEST_F(AdaptiveLosTests, T02_test_cross_track_error_right_off_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); - } - - TEST_F(AdaptiveLosTests, T03_test_cross_track_error_left_off_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, -0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); - } - - TEST_F(AdaptiveLosTests, T04_test_cross_track_error_under_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.5, tol); - } - - TEST_F(AdaptiveLosTests, T05_test_cross_track_error_over_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, -0.5, tol); - } - */ - - // Test commanded angles when drone is to the right of the track - TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is to the left of the track - TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(O.psi_d, 0.0); - EXPECT_LT(O.psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is under the track - TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); - } - - // Test commanded angles when drone is over the track - TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; +namespace vortex::guidance::los { // new namespace added los - const types::Outputs O = los_.calculate_outputs(inputs); +class AdaptiveLosTest : public ::testing::Test { + protected: + AdaptiveLosTest() : los_{get_params()} {} - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); + AdaptiveLosParams get_params() { + AdaptiveLosParams p; + p.lookahead_distance_h = 1.0; + p.lookahead_distance_v = 1.0; + p.gamma_h = 1.0; + p.gamma_v = 1.0; + p.time_step = 0.01; + return p; } - // Test commanded angles when drone is over and to the right of the track - - TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, -0.5}; - - const types::Outputs O = los_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); - } + AdaptiveLOSGuidance los_; + const double tol = 1e-9; +}; +/* +TEST_F(AdaptiveLosTest, T01_test_cross_track_error_on_track){ + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(AdaptiveLosTests, T02_test_cross_track_error_right_off_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(AdaptiveLosTests, T03_test_cross_track_error_left_off_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, -0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(AdaptiveLosTests, T04_test_cross_track_error_under_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.5, tol); +} + +TEST_F(AdaptiveLosTests, T05_test_cross_track_error_over_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, -0.5, tol); +} +*/ + +// Test commanded angles when drone is to the right of the track +TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track + +TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} } // namespace vortex::guidance::los - - diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp old mode 100755 new mode 100644 index 218e818c1..f94e93df1 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -1,110 +1,110 @@ +#include "los_guidance/lib/integral_los.hpp" #include #include "los_guidance/lib/adaptive_los.hpp" -#include "los_guidance/lib/integral_los.hpp" - -namespace vortex::guidance::los{ - - class IntegralLosTest : public ::testing::Test { - protected: - IntegralLosTest() : Ilos_{get_params()} {} - - IntegralLosParams get_params() { - IntegralLosParams params; - params.lookahead_distance_h = 1.0; - params.lookahead_distance_v = 1.0; - params.k_i_h = 0.1; // needs tuning - params.k_i_v = 0.1; // needs tuning - params.k_p_h = 0.667; // needs tuning - params.k_p_v = 0.582; // needs tuning - params.time_step = 0.01; - return params; - } - - IntegralLOSGuidance Ilos_; - const double tol = 1e-9; - }; - - // Test commanded angles when drone is to the right of the track - TEST_F(IntegralLosTest, T06_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is to the left of the track - TEST_F(IntegralLosTest, T07_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(O.psi_d, 0.0); - EXPECT_LT(O.psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is under the track - TEST_F(IntegralLosTest, T08_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); - } - - // Test commanded angles when drone is over the track - TEST_F(IntegralLosTest, T09_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); - } - - // Test commanded angles when drone is over and to the right of the track - - TEST_F(IntegralLosTest, T10_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, -0.5}; - - const types::Outputs O = Ilos_.calculate_outputs(inputs); - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); +namespace vortex::guidance::los { + +class IntegralLosTest : public ::testing::Test { + protected: + IntegralLosTest() : Ilos_{get_params()} {} + + IntegralLosParams get_params() { + IntegralLosParams params; + params.lookahead_distance_h = 1.0; + params.lookahead_distance_v = 1.0; + params.k_i_h = 0.1; // needs tuning + params.k_i_v = 0.1; // needs tuning + params.k_p_h = 0.667; // needs tuning + params.k_p_v = 0.582; // needs tuning + params.time_step = 0.01; + return params; } -} // namespace vortex::guidance + IntegralLOSGuidance Ilos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(IntegralLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(IntegralLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(IntegralLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(IntegralLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track + +TEST_F(IntegralLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp old mode 100755 new mode 100644 index 3c890cbcd..c704dec68 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -1,108 +1,107 @@ -#include #include "los_guidance/lib/proportional_los.hpp" +#include -namespace vortex::guidance::los{ - - class ProportionalLosTest : public ::testing::Test { - protected: - ProportionalLosTest() : Plos_{get_params()} {} - - ProportionalLosParams get_params() { - ProportionalLosParams params; - params.lookahead_distance_h = 10.0; - params.lookahead_distance_v = 10.0; - params.k_p_h = 0.667; // needs tuning - params.k_p_v = 0.582; // needs tuning - params.time_step = 0.01; - return params; - } - - ProportionalLOSGuidance Plos_; - const double tol = 1e-9; - }; - - // Test commanded angles when drone is to the right of the track - TEST_F(ProportionalLosTest, T06_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is to the left of the track - TEST_F(ProportionalLosTest, T07_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(O.psi_d, 0.0); - EXPECT_LT(O.psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(O.theta_d, 0.0, tol); - } - - // Test commanded angles when drone is under the track - TEST_F(ProportionalLosTest, T08_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); - } - - // Test commanded angles when drone is over the track - TEST_F(ProportionalLosTest, T09_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Outputs O = Plos_.calculate_outputs(inputs); - - // Heading cmd should be 0 - EXPECT_NEAR(O.psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); - } - - // Test commanded angles when drone is over and to the right of the track - - TEST_F(ProportionalLosTest, T10_test_commanded_angles) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, -0.5}; +namespace vortex::guidance::los { - const types::Outputs O = Plos_.calculate_outputs(inputs); +class ProportionalLosTest : public ::testing::Test { + protected: + ProportionalLosTest() : Plos_{get_params()} {} - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(O.psi_d, 0.0); - EXPECT_GT(O.psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); + ProportionalLosParams get_params() { + ProportionalLosParams params; + params.lookahead_distance_h = 10.0; + params.lookahead_distance_v = 10.0; + params.k_p_h = 0.667; // needs tuning + params.k_p_v = 0.582; // needs tuning + params.time_step = 0.01; + return params; } -} // namespace vortex::guidance - + ProportionalLOSGuidance Plos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(ProportionalLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(ProportionalLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(ProportionalLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(ProportionalLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track + +TEST_F(ProportionalLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/test_main.cpp b/guidance/los_guidance/test/test_main.cpp index 6ace5b598..af89a87e6 100644 --- a/guidance/los_guidance/test/test_main.cpp +++ b/guidance/los_guidance/test/test_main.cpp @@ -5,4 +5,4 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 66b252eed09cffff0e1e02cecc36982b9a29ccf6 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Thu, 13 Nov 2025 00:13:53 +0100 Subject: [PATCH 13/87] ci: trigger Industrial CI on PRs and main branch pushes --- .github/workflows/industrial-ci.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index 03c872f26..d2c0e782f 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -2,6 +2,10 @@ name: Industrial CI on: push: + branches: + - main + pull_request: + types: [ opened, synchronize, reopened, ready_for_review ] workflow_dispatch: schedule: - cron: '0 1 * * *' # Runs daily to check for dependency issues or flaking tests From a36297aa9ac82cb4a2a6f32441169d0a5cdce894 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Thu, 13 Nov 2025 00:43:36 +0100 Subject: [PATCH 14/87] Add C++ and CMake pre-commit hooks (#568) * ci(pre-commit): add CI workflow for checking pre-commit hooks * ci(pre-commit): add hooks for cpp and CMake * refactor(pre-commit.yml): now uses the workflow on main * docs(README.md): update status badge for pre-commit * refactor(ci): move local hooks into a different config file * docs(README.md): update status badge for pre-commit * refactor(ci): pre-commit.yml now only runs on pull_request and workflow_dispatch * ci(pre-commit-local): ignore build/include_order in ament_cpplint since conflict with clang-format * refactor: fix ament_cpplint and cppcheck warnings * ci(pre-commit-config-local): increase line length to 120 for ament_cpplint due to some lines being longer * refactor: fix ament_cpplint and cppcheck warnings * chore(pre-commit): exclude test dirs from ament_cppcheck to avoid false positives * ci(pre-commit): update workflow to run on push to main and pull_request --- .github/workflows/pre-commit.yml | 15 ++++++ .pre-commit-config-local.yaml | 47 +++++++++++++++++++ .../dp_adapt_backs_controller.hpp | 6 +-- .../dp_adapt_backs_controller_ros.hpp | 8 ++-- .../dp_adapt_backs_controller_utils.hpp | 6 +-- .../dp_adapt_backs_controller/typedefs.hpp | 6 +-- .../src/dp_adapt_backs_controller_ros.cpp | 2 +- .../pid_controller_dp/pid_controller.hpp | 8 ++-- .../pid_controller_conversions.hpp | 6 +-- .../pid_controller_dp/pid_controller_ros.hpp | 9 ++-- .../pid_controller_utils.hpp | 6 +-- .../include/pid_controller_dp/typedefs.hpp | 6 +-- .../pid_controller.hpp | 8 ++-- .../pid_controller_ros.hpp | 8 ++-- .../pid_controller_utils.hpp | 6 +-- .../pid_controller_dp_euler/typedefs.hpp | 6 +-- .../ekf_pose_filtering_ros.hpp | 10 ++-- .../pose_action_server_ros.hpp | 10 ++-- .../include/los_guidance/los_guidance_ros.hpp | 10 ++-- .../reference_filter_dp/eigen_typedefs.hpp | 6 +-- .../reference_filter_dp/reference_filter.hpp | 6 +-- .../reference_filter_ros.hpp | 7 +-- .../FSM/docking/include/docking/docking.hpp | 9 ++-- mission/FSM/docking/src/docking.cpp | 21 ++++----- .../eigen_vector6d_typedef.hpp | 6 +-- .../pseudoinverse_allocator.hpp | 6 +-- .../thrust_allocator_ros.hpp | 6 +-- .../thrust_allocator_utils.hpp | 7 +-- .../thruster_interface_auv_driver.hpp | 8 ++-- .../thruster_interface_auv_ros.hpp | 9 ++-- .../src/thruster_interface_auv_driver.cpp | 3 +- 31 files changed, 175 insertions(+), 102 deletions(-) create mode 100644 .github/workflows/pre-commit.yml create mode 100644 .pre-commit-config-local.yaml diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 000000000..7b3fccffe --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,15 @@ +name: pre-commit + +on: + push: + branches: + - main + pull_request: + types: [ opened, synchronize, reopened, ready_for_review ] + workflow_dispatch: +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-pre-commit.yml@main + with: + ros_distro: 'humble' + config_path: '.pre-commit-config-local.yaml' diff --git a/.pre-commit-config-local.yaml b/.pre-commit-config-local.yaml new file mode 100644 index 000000000..cb7994b70 --- /dev/null +++ b/.pre-commit-config-local.yaml @@ -0,0 +1,47 @@ +# To use: +# +# pre-commit run --all-files -c .pre-commit-config-local.yaml +# +# Or to install it for automatic checks on commit: +# +# pre-commit install -c .pre-commit-config-local.yaml +# +# To update this file: +# +# pre-commit autoupdate -c .pre-commit-config-local.yaml +# +# See https://pre-commit.com/ for documentation +# +# NOTE: This configuration uses local hooks specific to ROS2 (ament_* linters) + +repos: + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + exclude: (^|/)(test|tests)/.* # exclude test dirs since ament_cppcheck misparses GTest macros and throws false syntax errors + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: [ + "--linelength=120", + "--filter=-whitespace/newline,-legal/copyright,-build/include_order" # ignore build/include_order since it conflicts with .clang-format + ] + # CMake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp index 49cbe759f..6a44644e8 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp @@ -1,5 +1,5 @@ -#ifndef DP_ADAPT_BACKS_CONTROLLER_HPP -#define DP_ADAPT_BACKS_CONTROLLER_HPP +#ifndef DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ #include #include @@ -59,4 +59,4 @@ class DPAdaptBacksController { } // namespace vortex::control -#endif // DP_ADAPT_BACKS_CONTROLLER_HPP +#endif // DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp index 456bb7937..85b1c7f7a 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp @@ -1,16 +1,18 @@ -#ifndef DP_ADAPT_BACKS_CONTROLLER_ROS_HPP -#define DP_ADAPT_BACKS_CONTROLLER_ROS_HPP +#ifndef DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ #include #include #include #include #include +#include #include #include #include #include #include +#include #include #include #include "dp_adapt_backs_controller/dp_adapt_backs_controller.hpp" @@ -93,4 +95,4 @@ class DPAdaptBacksControllerNode : public rclcpp::Node { } // namespace vortex::control -#endif +#endif // DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp index 33ceeb295..b48a21867 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp @@ -1,5 +1,5 @@ -#ifndef DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP -#define DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP +#ifndef DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ #include #include "dp_adapt_backs_controller/typedefs.hpp" @@ -50,4 +50,4 @@ Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Nu& nu); } // namespace vortex::control -#endif +#endif // DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp index 4e43dbf63..49f908072 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/typedefs.hpp @@ -3,8 +3,8 @@ * @brief Contains the Eigen typedefs for the controller. */ -#ifndef VORTEX_DP_ADAPT_BACKSTEPPING_CONTROLLER_TYPEDEFS_H -#define VORTEX_DP_ADAPT_BACKSTEPPING_CONTROLLER_TYPEDEFS_H +#ifndef DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ #include @@ -19,4 +19,4 @@ typedef Eigen::Matrix Matrix12d; } // namespace Eigen -#endif +#endif // DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp index 4bb6d5cee..da70eaf79 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp @@ -8,7 +8,7 @@ #include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" #include "dp_adapt_backs_controller/typedefs.hpp" -const std::string start_message = R"( +constexpr std::string_view start_message = R"( ____ ____ ____ _ _ _ | _ \| _ \ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ | | | | |_) | | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp index 4ead4b716..1017b5bd6 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp @@ -1,11 +1,11 @@ -#ifndef PID_CONTROLLER_HPP -#define PID_CONTROLLER_HPP +#ifndef PID_CONTROLLER_DP__PID_CONTROLLER_HPP_ +#define PID_CONTROLLER_DP__PID_CONTROLLER_HPP_ #include "pid_controller_dp/typedefs.hpp" class PIDController { public: - explicit PIDController(); + PIDController(); // @brief Calculate the control input tau // @param eta: struct containing the vehicle pose [position, orientation] @@ -44,4 +44,4 @@ class PIDController { double dt_; }; -#endif +#endif // PID_CONTROLLER_DP__PID_CONTROLLER_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp index 16045e085..a28097e9f 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp @@ -1,5 +1,5 @@ -#ifndef PID_CONTROLLER_CONVERSIONS_HPP -#define PID_CONTROLLER_CONVERSIONS_HPP +#ifndef PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ +#define PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ #include #include @@ -15,4 +15,4 @@ types::Eta eta_convert_from_ros_to_eigen( types::Nu nu_convert_from_ros_to_eigen( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); -#endif +#endif // PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp index 06b57c111..128c8650c 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp @@ -1,5 +1,5 @@ -#ifndef PID_CONTROLLER_ROS_HPP -#define PID_CONTROLLER_ROS_HPP +#ifndef PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_ +#define PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_ #include #include @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include "pid_controller_dp/pid_controller.hpp" @@ -19,7 +20,7 @@ // @brief Class for the PID controller node class PIDControllerNode : public rclcpp::Node { public: - explicit PIDControllerNode(); + PIDControllerNode(); private: // @brief Callback function for the killswitch topic @@ -95,4 +96,4 @@ class PIDControllerNode : public rclcpp::Node { std::string software_mode_; }; -#endif +#endif // PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp index 36253ae38..1bdce214b 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp @@ -1,5 +1,5 @@ -#ifndef PID_UTILS_HPP -#define PID_UTILS_HPP +#ifndef PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ +#define PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ #include #include @@ -71,4 +71,4 @@ types::Vector7d anti_windup(const double dt, const types::Eta& error, const types::Vector7d& integral); -#endif +#endif // PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp index 474abb25c..beeea3174 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp @@ -3,8 +3,8 @@ * @brief Contains the typedef for a 6x1 Eigen vector and a 6x6 Eigen matrix. */ -#ifndef VORTEX_EIGEN_TYPEDEFS_H -#define VORTEX_EIGEN_TYPEDEFS_H +#ifndef PID_CONTROLLER_DP__TYPEDEFS_HPP_ +#define PID_CONTROLLER_DP__TYPEDEFS_HPP_ #include @@ -56,4 +56,4 @@ struct J_transformation { }; } // namespace types -#endif +#endif // PID_CONTROLLER_DP__TYPEDEFS_HPP_ diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller.hpp index a44c2ef4d..6872213a3 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller.hpp @@ -1,11 +1,11 @@ -#ifndef PID_CONTROLLER_HPP -#define PID_CONTROLLER_HPP +#ifndef PID_CONTROLLER_DP_EULER__PID_CONTROLLER_HPP_ +#define PID_CONTROLLER_DP_EULER__PID_CONTROLLER_HPP_ #include class PIDController { public: - explicit PIDController(); + PIDController(); Vector6d calculate_tau(const Eta& eta, const Eta& eta_d, @@ -28,4 +28,4 @@ class PIDController { double dt_; }; -#endif +#endif // PID_CONTROLLER_DP_EULER__PID_CONTROLLER_HPP_ diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp index 04f1ef097..22e3df334 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_ros.hpp @@ -1,5 +1,5 @@ -#ifndef PID_CONTROLLER_ROS_HPP -#define PID_CONTROLLER_ROS_HPP +#ifndef PID_CONTROLLER_DP_EULER__PID_CONTROLLER_ROS_HPP_ +#define PID_CONTROLLER_DP_EULER__PID_CONTROLLER_ROS_HPP_ #include #include @@ -17,7 +17,7 @@ class PIDControllerNode : public rclcpp::Node { public: - explicit PIDControllerNode(); + PIDControllerNode(); private: void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg); @@ -73,4 +73,4 @@ class PIDControllerNode : public rclcpp::Node { std::string software_mode_; }; -#endif +#endif // PID_CONTROLLER_DP_EULER__PID_CONTROLLER_ROS_HPP_ diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp index a9e622fe7..f1db15a2d 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/pid_controller_utils.hpp @@ -1,5 +1,5 @@ -#ifndef PID_UTILS_HPP -#define PID_UTILS_HPP +#ifndef PID_CONTROLLER_DP_EULER__PID_CONTROLLER_UTILS_HPP_ +#define PID_CONTROLLER_DP_EULER__PID_CONTROLLER_UTILS_HPP_ #include #include @@ -22,4 +22,4 @@ Vector6d clamp_values(const Vector6d& values, double min_val, double max_val); Vector6d limit_input(const Vector6d& input); -#endif +#endif // PID_CONTROLLER_DP_EULER__PID_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp index 0c2f1db08..5d659fa93 100644 --- a/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp +++ b/control/pid_controller_dp_euler/include/pid_controller_dp_euler/typedefs.hpp @@ -3,8 +3,8 @@ * @brief Contains the typedef for a 6x1 Eigen vector and a 6x6 Eigen matrix. */ -#ifndef VORTEX_EIGEN_TYPEDEFS_H -#define VORTEX_EIGEN_TYPEDEFS_H +#ifndef PID_CONTROLLER_DP_EULER__TYPEDEFS_HPP_ +#define PID_CONTROLLER_DP_EULER__TYPEDEFS_HPP_ #include #include @@ -102,4 +102,4 @@ struct Nu { } }; -#endif +#endif // PID_CONTROLLER_DP_EULER__TYPEDEFS_HPP_ diff --git a/filtering/ekf_pose_filtering/include/ekf_pose_filtering/ekf_pose_filtering_ros.hpp b/filtering/ekf_pose_filtering/include/ekf_pose_filtering/ekf_pose_filtering_ros.hpp index 94f3267b2..815b239ac 100644 --- a/filtering/ekf_pose_filtering/include/ekf_pose_filtering/ekf_pose_filtering_ros.hpp +++ b/filtering/ekf_pose_filtering/include/ekf_pose_filtering/ekf_pose_filtering_ros.hpp @@ -1,5 +1,5 @@ -#ifndef EKF_POSE_FILTERING_ROS_HPP -#define EKF_POSE_FILTERING_ROS_HPP +#ifndef EKF_POSE_FILTERING__EKF_POSE_FILTERING_ROS_HPP_ +#define EKF_POSE_FILTERING__EKF_POSE_FILTERING_ROS_HPP_ #include #include @@ -8,8 +8,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -18,7 +20,7 @@ class EKFPoseFilteringNode : public rclcpp::Node { public: EKFPoseFilteringNode(); - ~EKFPoseFilteringNode() {}; + ~EKFPoseFilteringNode() {} private: void reset_EFK_state( @@ -65,4 +67,4 @@ class EKFPoseFilteringNode : public rclcpp::Node { bool enu_orientation_; }; -#endif // EKF_POSE_FILTERING_ROS_HPP +#endif // EKF_POSE_FILTERING__EKF_POSE_FILTERING_ROS_HPP_ diff --git a/filtering/pose_action_server/include/pose_action_server/pose_action_server_ros.hpp b/filtering/pose_action_server/include/pose_action_server/pose_action_server_ros.hpp index 4ac479570..244d7d5f4 100644 --- a/filtering/pose_action_server/include/pose_action_server/pose_action_server_ros.hpp +++ b/filtering/pose_action_server/include/pose_action_server/pose_action_server_ros.hpp @@ -1,13 +1,15 @@ -#ifndef POSE_ACTION_SERVER_ROS_HPP -#define POSE_ACTION_SERVER_ROS_HPP +#ifndef POSE_ACTION_SERVER__POSE_ACTION_SERVER_ROS_HPP_ +#define POSE_ACTION_SERVER__POSE_ACTION_SERVER_ROS_HPP_ #include #include #include #include #include +#include #include #include +#include #include class PoseActionServerNode : public rclcpp::Node { @@ -17,7 +19,7 @@ class PoseActionServerNode : public rclcpp::Node { public: PoseActionServerNode(); - ~PoseActionServerNode() {}; + ~PoseActionServerNode() {} private: rclcpp_action::Server::SharedPtr @@ -53,4 +55,4 @@ class PoseActionServerNode : public rclcpp::Node { const std::vector& quaternions); }; -#endif // POSE_ACTION_SERVER_ROS_HPP +#endif // POSE_ACTION_SERVER__POSE_ACTION_SERVER_ROS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index eb53d461c..1a7bb74a2 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,10 +1,12 @@ -#ifndef LOS_GUIDANCE_ROS_HPP -#define LOS_GUIDANCE_ROS_HPP +#ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #include #include #include #include +#include +#include #include #include #include @@ -20,7 +22,7 @@ namespace vortex::guidance::los { class LosGuidanceNode : public rclcpp::Node { public: - explicit LosGuidanceNode(); + LosGuidanceNode(); private: // @brief Set the subscribers and publishers @@ -128,4 +130,4 @@ class LosGuidanceNode : public rclcpp::Node { } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE_ROS_HPP +#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp index 9828efb30..f9b29feae 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/eigen_typedefs.hpp @@ -3,8 +3,8 @@ * @brief Contains Eigen typedefs used in this package. */ -#ifndef REFERENCE_FILTER_DP_EIGEN_TYPEDEFS_HPP -#define REFERENCE_FILTER_DP_EIGEN_TYPEDEFS_HPP +#ifndef REFERENCE_FILTER_DP__EIGEN_TYPEDEFS_HPP_ +#define REFERENCE_FILTER_DP__EIGEN_TYPEDEFS_HPP_ #include @@ -18,4 +18,4 @@ typedef Eigen::Matrix Vector18d; } // namespace Eigen -#endif // REFERENCE_FILTER_DP_EIGEN_TYPEDEFS_HPP +#endif // REFERENCE_FILTER_DP__EIGEN_TYPEDEFS_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp index eff869bb4..dd9c10d98 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter.hpp @@ -1,5 +1,5 @@ -#ifndef REFERENCE_FILTER_HPP -#define REFERENCE_FILTER_HPP +#ifndef REFERENCE_FILTER_DP__REFERENCE_FILTER_HPP_ +#define REFERENCE_FILTER_DP__REFERENCE_FILTER_HPP_ #include "reference_filter_dp/eigen_typedefs.hpp" @@ -41,4 +41,4 @@ class ReferenceFilter { } // namespace vortex::guidance -#endif // REFERENCE_FILTER_HPP +#endif // REFERENCE_FILTER_DP__REFERENCE_FILTER_HPP_ diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp index c9553e761..6bc1584f1 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp @@ -1,9 +1,10 @@ -#ifndef REFERENCE_FILTER_ROS_HPP -#define REFERENCE_FILTER_ROS_HPP +#ifndef REFERENCE_FILTER_DP__REFERENCE_FILTER_ROS_HPP_ +#define REFERENCE_FILTER_DP__REFERENCE_FILTER_ROS_HPP_ #include #include #include +#include #include #include #include @@ -124,4 +125,4 @@ class ReferenceFilterNode : public rclcpp::Node { } // namespace vortex::guidance -#endif // REFERENCE_FILTER_ROS_HPP +#endif // REFERENCE_FILTER_DP__REFERENCE_FILTER_ROS_HPP_ diff --git a/mission/FSM/docking/include/docking/docking.hpp b/mission/FSM/docking/include/docking/docking.hpp index 82dd88349..906696228 100644 --- a/mission/FSM/docking/include/docking/docking.hpp +++ b/mission/FSM/docking/include/docking/docking.hpp @@ -1,5 +1,5 @@ -#ifndef VORTEX_DOCKING_HPP -#define VORTEX_DOCKING_HPP +#ifndef DOCKING__DOCKING_HPP_ +#define DOCKING__DOCKING_HPP_ #include #include @@ -98,7 +98,8 @@ std::string DockedState( class ReturnHomeState : public yasmin_ros::ActionState { public: - ReturnHomeState(std::shared_ptr blackboard); + explicit ReturnHomeState( + std::shared_ptr blackboard); docking_fsm::ReturnHomeAction::Goal create_goal_handler( std::shared_ptr blackboard); @@ -145,4 +146,4 @@ void add_states(std::shared_ptr sm, auto initialize_blackboard(); -#endif +#endif // DOCKING__DOCKING_HPP_ diff --git a/mission/FSM/docking/src/docking.cpp b/mission/FSM/docking/src/docking.cpp index 459bfb236..4476d90a1 100644 --- a/mission/FSM/docking/src/docking.cpp +++ b/mission/FSM/docking/src/docking.cpp @@ -9,7 +9,7 @@ FindDockingStationState::FindDockingStationState( blackboard->get("pose_action"), std::bind(&FindDockingStationState::create_goal_handler, this, _1), std::bind(&FindDockingStationState::response_handler, this, _1, _2), - std::bind(&FindDockingStationState::print_feedback, this, _1, _2)) {}; + std::bind(&FindDockingStationState::print_feedback, this, _1, _2)) {} docking_fsm::FindDockingAction::Goal FindDockingStationState::create_goal_handler( @@ -70,7 +70,7 @@ ApproachDockingStationState::ApproachDockingStationState( std::bind(&ApproachDockingStationState::print_feedback, this, _1, - _2)) {}; + _2)) {} docking_fsm::ApproachDockingAction::Goal ApproachDockingStationState::create_goal_handler( @@ -136,7 +136,7 @@ GoAboveDockingStationState::GoAboveDockingStationState( std::bind(&GoAboveDockingStationState::print_feedback, this, _1, - _2)) {}; + _2)) {} docking_fsm::GoAboveDockingAction::Goal GoAboveDockingStationState::create_goal_handler( @@ -204,7 +204,7 @@ ConvergeDockingStationState::ConvergeDockingStationState( std::bind(&ConvergeDockingStationState::print_feedback, this, _1, - _2)) {}; + _2)) {} docking_fsm::ConvergeDockingAction::Goal ConvergeDockingStationState::create_goal_handler( @@ -267,7 +267,7 @@ std::string DockedState( return yasmin_ros::basic_outcomes::ABORT; } } -}; +} ReturnHomeState::ReturnHomeState( std::shared_ptr blackboard) @@ -275,7 +275,7 @@ ReturnHomeState::ReturnHomeState( blackboard->get("reference_filter_action"), std::bind(&ReturnHomeState::create_goal_handler, this, _1), std::bind(&ReturnHomeState::response_handler, this, _1, _2), - std::bind(&ReturnHomeState::print_feedback, this, _1, _2)) {}; + std::bind(&ReturnHomeState::print_feedback, this, _1, _2)) {} docking_fsm::ReturnHomeAction::Goal ReturnHomeState::create_goal_handler( std::shared_ptr blackboard) { @@ -330,13 +330,13 @@ std::string AbortState( std::shared_ptr blackboard) { blackboard->set("is_abort", true); return yasmin_ros::basic_outcomes::ABORT; -}; +} std::string ErrorState( std::shared_ptr blackboard) { blackboard->set("is_error", true); return yasmin_ros::basic_outcomes::SUCCEED; -}; +} std::shared_ptr create_state_machines() { std::set outcomes = { @@ -375,7 +375,6 @@ void add_states(std::shared_ptr sm, "UPDATE_DOCKING_STATION_POSITION"}, {yasmin_ros::basic_outcomes::ABORT, "ABORT"}, {yasmin_ros::basic_outcomes::CANCEL, "APPROACH_DOCKING_STATION"}, - }); sm->add_state( @@ -385,7 +384,6 @@ void add_states(std::shared_ptr sm, {yasmin_ros::basic_outcomes::SUCCEED, "CONVERGE_DOCKING_STATION"}, {yasmin_ros::basic_outcomes::ABORT, "ABORT"}, {yasmin_ros::basic_outcomes::CANCEL, "GO_ABOVE_DOCKING_STATION"}, - }); sm->add_state( @@ -395,7 +393,6 @@ void add_states(std::shared_ptr sm, {yasmin_ros::basic_outcomes::SUCCEED, "DOCKED"}, {yasmin_ros::basic_outcomes::ABORT, "ABORT"}, {yasmin_ros::basic_outcomes::CANCEL, "GO_ABOVE_DOCKING_STATION"}, - }); sm->add_state("DOCKED", @@ -408,7 +405,6 @@ void add_states(std::shared_ptr sm, {yasmin_ros::basic_outcomes::SUCCEED, yasmin_ros::basic_outcomes::SUCCEED}, {yasmin_ros::basic_outcomes::ABORT, "ABORT"}, - }); sm->add_state( "RETURN_HOME", std::make_shared(blackboard), @@ -416,7 +412,6 @@ void add_states(std::shared_ptr sm, {yasmin_ros::basic_outcomes::SUCCEED, "FIND_DOCKING_STATION"}, {yasmin_ros::basic_outcomes::CANCEL, "error"}, {yasmin_ros::basic_outcomes::ABORT, "ABORT"}, - }); sm->add_state("ABORT", std::make_shared( diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp index e06511362..23ecff92f 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp @@ -3,8 +3,8 @@ * @brief Contains the typedef for a 6x1 Eigen vector. */ -#ifndef VORTEX_EIGEN_TYPEDEFS_H -#define VORTEX_EIGEN_TYPEDEFS_H +#ifndef THRUST_ALLOCATOR_AUV__EIGEN_VECTOR6D_TYPEDEF_HPP_ +#define THRUST_ALLOCATOR_AUV__EIGEN_VECTOR6D_TYPEDEF_HPP_ #include @@ -12,4 +12,4 @@ namespace Eigen { typedef Eigen::Matrix Vector6d; } -#endif // VORTEX_EIGEN_TYPEDEFS_H +#endif // THRUST_ALLOCATOR_AUV__EIGEN_VECTOR6D_TYPEDEF_HPP_ diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp index e1d96ce9f..6b99c6e4b 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp @@ -5,8 +5,8 @@ * Handbook of Marine Craft Hydrodynamics and Motion Control (chapter 12.3.2). */ -#ifndef VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP -#define VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP +#ifndef THRUST_ALLOCATOR_AUV__PSEUDOINVERSE_ALLOCATOR_HPP_ +#define THRUST_ALLOCATOR_AUV__PSEUDOINVERSE_ALLOCATOR_HPP_ #include @@ -36,4 +36,4 @@ class PseudoinverseAllocator { Eigen::MatrixXd T_pinv; }; -#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP +#endif // THRUST_ALLOCATOR_AUV__PSEUDOINVERSE_ALLOCATOR_HPP_ diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp index 4d000010e..9f0b5e307 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp @@ -3,8 +3,8 @@ * @brief Contains the ROS logic for the thruster allocator node. */ -#ifndef VORTEX_ALLOCATOR_ROS_HPP -#define VORTEX_ALLOCATOR_ROS_HPP +#ifndef THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_ROS_HPP_ +#define THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_ROS_HPP_ #include #include @@ -94,4 +94,4 @@ class ThrustAllocator : public rclcpp::Node { rclcpp::TimerBase::SharedPtr watchdog_timer_; }; -#endif // VORTEX_ALLOCATOR_ROS_HPP +#endif // THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_ROS_HPP_ diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp index 8e13a0fe4..7768c3090 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp @@ -4,9 +4,10 @@ * module. */ -#ifndef VORTEX_ALLOCATOR_UTILS_HPP -#define VORTEX_ALLOCATOR_UTILS_HPP +#ifndef THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_UTILS_HPP_ +#define THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_UTILS_HPP_ +#include #include #include #include @@ -150,4 +151,4 @@ inline Eigen::Vector6d wrench_to_vector( return msg_vector; } -#endif // VORTEX_ALLOCATOR_UTILS_HPP +#endif // THRUST_ALLOCATOR_AUV__THRUST_ALLOCATOR_UTILS_HPP_ diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 35c60d5e2..6e2f0ebd4 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -1,5 +1,5 @@ -#ifndef THRUSTER_INTERFACE_AUV_DRIVER_HPP -#define THRUSTER_INTERFACE_AUV_DRIVER_HPP +#ifndef THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ +#define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ #include #include @@ -61,7 +61,7 @@ class ThrusterInterfaceAUVDriver { * polynomial coefficients */ ThrusterInterfaceAUVDriver( - short i2c_bus, + std::int16_t i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, const std::vector>& poly_coeffs); @@ -152,4 +152,4 @@ class ThrusterInterfaceAUVDriver { } }; -#endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP +#endif // THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 4171ba968..58ca254fb 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -1,10 +1,13 @@ -#ifndef THRUSTER_INTERFACE_AUV_NODE_HPP -#define THRUSTER_INTERFACE_AUV_NODE_HPP +#ifndef THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_ROS_HPP_ +#define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_ROS_HPP_ +#include #include #include #include #include +#include +#include #include #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" @@ -94,4 +97,4 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { void update_debug_flag(const rclcpp::Parameter& p); }; -#endif // THRUSTER_INTERFACE_AUV_NODE_HPP +#endif // THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_ROS_HPP_ diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index a932a66d4..86834a8e7 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -1,10 +1,11 @@ #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" #include +#include #include #include ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( - short i2c_bus, + std::int16_t i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, const std::vector>& poly_coeffs) From a10e6392bfc4408be2d5e928f9ec0c60557356b5 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Thu, 13 Nov 2025 00:45:17 +0100 Subject: [PATCH 15/87] docs: add pre-commit CI badge to README --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 0fc3e31d0..23f4c5eb5 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,7 @@ [![Simulator Test](https://github.com/vortexntnu/vortex-auv/actions/workflows/simulator-test.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/simulator-test.yml) [![ROS Node Test](https://github.com/vortexntnu/vortex-auv/actions/workflows/ros-node-tests.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/ros-node-tests.yml) [![pre-commit.ci status](https://results.pre-commit.ci/badge/github/vortexntnu/vortex-auv/main.svg)](https://results.pre-commit.ci/latest/github/vortexntnu/vortex-auv/main) +[![pre-commit](https://github.com/vortexntnu/vortex-auv/actions/workflows/pre-commit.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/pre-commit.yml) [![codecov](https://codecov.io/github/vortexntnu/vortex-auv/graph/badge.svg?token=UXIGc2qG7N)](https://codecov.io/github/vortexntnu/vortex-auv) ![Banner](docs/banner_image.png) From b2e632b5b80da952b83ef20e9704c9a8eff02e0c Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Sun, 16 Nov 2025 21:53:02 +0100 Subject: [PATCH 16/87] refactor: merge the two pre-commit files to replace the pre-commit-ci bot --- .github/workflows/pre-commit.yml | 2 +- .pre-commit-config-local.yaml | 47 -------------------------------- .pre-commit-config.yaml | 30 ++++++++++++++++++++ README.md | 1 - 4 files changed, 31 insertions(+), 49 deletions(-) delete mode 100644 .pre-commit-config-local.yaml diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index 7b3fccffe..76e7310bc 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -12,4 +12,4 @@ jobs: uses: vortexntnu/vortex-ci/.github/workflows/reusable-pre-commit.yml@main with: ros_distro: 'humble' - config_path: '.pre-commit-config-local.yaml' + config_path: '.pre-commit-config.yaml' diff --git a/.pre-commit-config-local.yaml b/.pre-commit-config-local.yaml deleted file mode 100644 index cb7994b70..000000000 --- a/.pre-commit-config-local.yaml +++ /dev/null @@ -1,47 +0,0 @@ -# To use: -# -# pre-commit run --all-files -c .pre-commit-config-local.yaml -# -# Or to install it for automatic checks on commit: -# -# pre-commit install -c .pre-commit-config-local.yaml -# -# To update this file: -# -# pre-commit autoupdate -c .pre-commit-config-local.yaml -# -# See https://pre-commit.com/ for documentation -# -# NOTE: This configuration uses local hooks specific to ROS2 (ament_* linters) - -repos: - - repo: local - hooks: - - id: ament_cppcheck - name: ament_cppcheck - description: Static code analysis of C/C++ files. - entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck - language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - exclude: (^|/)(test|tests)/.* # exclude test dirs since ament_cppcheck misparses GTest macros and throws false syntax errors - - repo: local - hooks: - - id: ament_cpplint - name: ament_cpplint - description: Static code analysis of C/C++ files. - entry: ament_cpplint - language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - args: [ - "--linelength=120", - "--filter=-whitespace/newline,-legal/copyright,-build/include_order" # ignore build/include_order since it conflicts with .clang-format - ] - # CMake hooks - - repo: local - hooks: - - id: ament_lint_cmake - name: ament_lint_cmake - description: Check format of CMakeLists.txt files. - entry: ament_lint_cmake - language: system - files: CMakeLists\.txt$ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c7452a826..6609d6cbc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -74,6 +74,36 @@ repos: hooks: - id: clang-format args: [--style=file] + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + exclude: (^|/)(test|tests)/.* # exclude test dirs since ament_cppcheck misparses GTest macros and throws false syntax errors + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: [ + "--linelength=120", + "--filter=-whitespace/newline,-legal/copyright,-build/include_order" # ignore build/include_order since it conflicts with .clang-format + ] + # CMake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ # Spellcheck in comments and docs - repo: https://github.com/codespell-project/codespell rev: v2.4.1 diff --git a/README.md b/README.md index 23f4c5eb5..8d819ed16 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,6 @@ [![Industrial CI](https://github.com/vortexntnu/vortex-auv/actions/workflows/industrial-ci.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/industrial-ci.yml) [![Simulator Test](https://github.com/vortexntnu/vortex-auv/actions/workflows/simulator-test.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/simulator-test.yml) [![ROS Node Test](https://github.com/vortexntnu/vortex-auv/actions/workflows/ros-node-tests.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/ros-node-tests.yml) -[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/vortexntnu/vortex-auv/main.svg)](https://results.pre-commit.ci/latest/github/vortexntnu/vortex-auv/main) [![pre-commit](https://github.com/vortexntnu/vortex-auv/actions/workflows/pre-commit.yml/badge.svg)](https://github.com/vortexntnu/vortex-auv/actions/workflows/pre-commit.yml) [![codecov](https://codecov.io/github/vortexntnu/vortex-auv/graph/badge.svg?token=UXIGc2qG7N)](https://codecov.io/github/vortexntnu/vortex-auv) From ef96e1eed2fc7e1312cc9dc43e834e5e3f2481fa Mon Sep 17 00:00:00 2001 From: jorgenfj <144696109+jorgenfj@users.noreply.github.com> Date: Fri, 19 Dec 2025 11:48:51 +0100 Subject: [PATCH 17/87] Feat/waypoint manager (#645) * waypoint mode handling * update tests with new message * initial package setup * ros interface function declaration * node setup * working prototype * reentrant cb, multithreaded * single threaded impl * conv threshold action goal * default thresholdref conv value * removed switching logic * removed timer execution * sim test utils * waypoint_manager_test setup * no rendering test arg * waypoint tests, timeout error * test refactor * format * rename utils package * test suite and description * first waypoint test * removed unused function * renamed service field to priority. Added simple tests * waypoint manager readme * uniform attitude naming convention * fix pr requests * update tests with new service fields * four corner test * update util func name * update with new action def * removed failing build type * test dependencies * ignore failing yasmin package * remove __init__ files * quat_to_euler in make_pose helper * added __init__ file * removed sim deps for test packages * added action shutdown handling * removed waypoint manager set setup * added waypoint manager node tests * waypoint manager 4 corner sim test * added missing launch testing test dependency * add sleep for topic discovery * fix action member field name * removed unnecessary if here --- .github/workflows/simulator-test.yml | 3 +- .gitignore | 3 - .../reference_filter_ros.hpp | 5 +- .../src/reference_filter_ros.cpp | 78 +++- mission/FSM/docking/COLCON_IGNORE | 2 + mission/FSM/docking/src/docking.cpp | 72 ++-- mission/waypoint_manager/CMakeLists.txt | 69 ++++ mission/waypoint_manager/README.md | 84 ++++ .../waypoint_manager/waypoint_manager_ros.hpp | 120 ++++++ .../launch/waypoint_manager.launch.py | 14 + mission/waypoint_manager/package.xml | 28 ++ .../src/waypoint_manager_ros.cpp | 378 ++++++++++++++++++ mission/waypoint_manager/test/CMakeLists.txt | 16 + mission/waypoint_manager/test/test_service.py | 153 +++++++ .../test/test_single_waypoint.py | 147 +++++++ .../reference_filter_node_test.sh | 5 +- .../waypoint_manager_test/check_goal.py | 103 +++++ .../waypoint_manager_test/send_goal.py | 91 +++++ .../waypoint_manager_test/simulator_test.sh | 115 ++++++ .../waypoint_navigation/send_goal.py | 15 +- 20 files changed, 1445 insertions(+), 56 deletions(-) create mode 100644 mission/FSM/docking/COLCON_IGNORE create mode 100644 mission/waypoint_manager/CMakeLists.txt create mode 100644 mission/waypoint_manager/README.md create mode 100644 mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp create mode 100644 mission/waypoint_manager/launch/waypoint_manager.launch.py create mode 100644 mission/waypoint_manager/package.xml create mode 100644 mission/waypoint_manager/src/waypoint_manager_ros.cpp create mode 100644 mission/waypoint_manager/test/CMakeLists.txt create mode 100644 mission/waypoint_manager/test/test_service.py create mode 100644 mission/waypoint_manager/test/test_single_waypoint.py mode change 100644 => 100755 tests/ros_node_tests/reference_filter_node_test.sh create mode 100644 tests/simulator_tests/waypoint_manager_test/check_goal.py create mode 100644 tests/simulator_tests/waypoint_manager_test/send_goal.py create mode 100755 tests/simulator_tests/waypoint_manager_test/simulator_test.sh diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index e73d03ac5..052af48de 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -16,5 +16,6 @@ jobs: setup_script: "tests/setup.sh" test_scripts: '[ "tests/simulator_tests/waypoint_navigation/simulator_test.sh", - "tests/simulator_tests/los_test/simulator_test.sh" + "tests/simulator_tests/los_test/simulator_test.sh", + "tests/simulator_tests/waypoint_manager_test/simulator_test.sh" ]' diff --git a/.gitignore b/.gitignore index 2a085ac8d..52ae0a3c0 100644 --- a/.gitignore +++ b/.gitignore @@ -56,7 +56,4 @@ qtcreator-* # Emacs .#* -# Catkin custom files -COLCON_IGNORE - # End of https://www.gitignore.io/api/ros diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp index 6bc1584f1..2fb46649e 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp @@ -74,8 +74,9 @@ class ReferenceFilterNode : public rclcpp::Node { Eigen::Vector18d fill_reference_state(); - Eigen::Vector6d fill_reference_goal( - const geometry_msgs::msg::PoseStamped& goal); + Eigen::Vector6d fill_reference_goal(const geometry_msgs::msg::Pose& goal); + + Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, uint8_t mode); vortex_msgs::msg::ReferenceFilter fill_reference_msg(); diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index 02d463c0f..614e10e93 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -195,12 +195,12 @@ Eigen::Vector18d ReferenceFilterNode::fill_reference_state() { } Eigen::Vector6d ReferenceFilterNode::fill_reference_goal( - const geometry_msgs::msg::PoseStamped& goal) { - double x{goal.pose.position.x}; - double y{goal.pose.position.y}; - double z{goal.pose.position.z}; + const geometry_msgs::msg::Pose& goal) { + double x{goal.position.x}; + double y{goal.position.y}; + double z{goal.position.z}; - const auto& o = goal.pose.orientation; + const auto& o = goal.orientation; Eigen::Quaterniond q(o.w, o.x, o.y, o.z); Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); double roll{euler_angles(0)}; @@ -231,6 +231,43 @@ vortex_msgs::msg::ReferenceFilter ReferenceFilterNode::fill_reference_msg() { return feedback_msg; } +Eigen::Vector6d ReferenceFilterNode::apply_mode_logic( + const Eigen::Vector6d& r_in, + uint8_t mode) { + Eigen::Vector6d r_out = r_in; + + switch (mode) { + case vortex_msgs::msg::Waypoint::FULL_POSE: + break; + + case vortex_msgs::msg::Waypoint::ONLY_POSITION: + r_out(3) = x_(3); + r_out(4) = x_(4); + r_out(5) = x_(5); + break; + + case vortex_msgs::msg::Waypoint::FORWARD_HEADING: { + double dx = r_in(0) - x_(0); + double dy = r_in(1) - x_(1); + + double forward_heading = std::atan2(dy, dx); + + r_out(3) = 0.0; + r_out(4) = 0.0; + r_out(5) = vortex::utils::math::ssa(forward_heading); + break; + } + + case vortex_msgs::msg::Waypoint::ONLY_ORIENTATION: + r_out(0) = x_(0); + r_out(1) = x_(1); + r_out(2) = x_(2); + break; + } + + return r_out; +} + void ReferenceFilterNode::execute( const std::shared_ptr> goal_handle) { @@ -243,9 +280,21 @@ void ReferenceFilterNode::execute( x_ = fill_reference_state(); - const geometry_msgs::msg::PoseStamped goal = goal_handle->get_goal()->goal; + const geometry_msgs::msg::Pose goal = + goal_handle->get_goal()->waypoint.pose; + uint8_t mode = goal_handle->get_goal()->waypoint.mode; + double convergence_threshold = + goal_handle->get_goal()->convergence_threshold; + + if (convergence_threshold <= 0.0) { + convergence_threshold = 0.1; + spdlog::warn( + "ReferenceFilter: Invalid convergence_threshold received (<= 0). " + "Using default 0.1"); + } - r_ = fill_reference_goal(goal); + Eigen::Vector6d r_temp = fill_reference_goal(goal); + r_ = apply_mode_logic(r_temp, mode); auto feedback = std::make_shared< vortex_msgs::action::ReferenceFilterWaypoint::Feedback>(); @@ -274,12 +323,12 @@ void ReferenceFilterNode::execute( vortex_msgs::msg::ReferenceFilter feedback_msg = fill_reference_msg(); - feedback->feedback = feedback_msg; + feedback->reference = feedback_msg; goal_handle->publish_feedback(feedback); reference_pub_->publish(feedback_msg); - if ((x_.head(6) - r_.head(6)).norm() < 0.1) { + if ((x_.head(6) - r_.head(6)).norm() < convergence_threshold) { result->success = true; goal_handle->succeed(result); x_.head(6) = r_.head(6); @@ -292,6 +341,17 @@ void ReferenceFilterNode::execute( loop_rate.sleep(); } + if (!rclcpp::ok() && goal_handle->is_active()) { + auto result = std::make_shared< + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); + result->success = false; + + try { + goal_handle->abort(result); + } catch (...) { + // Ignore exceptions during shutdown + } + } } RCLCPP_COMPONENTS_REGISTER_NODE(ReferenceFilterNode) diff --git a/mission/FSM/docking/COLCON_IGNORE b/mission/FSM/docking/COLCON_IGNORE new file mode 100644 index 000000000..f4db3a26d --- /dev/null +++ b/mission/FSM/docking/COLCON_IGNORE @@ -0,0 +1,2 @@ +# Ignored temporarily to skip building while debugging yasmin upgrade +# Remove this file to include the package in colcon builds again diff --git a/mission/FSM/docking/src/docking.cpp b/mission/FSM/docking/src/docking.cpp index 4476d90a1..c251dda75 100644 --- a/mission/FSM/docking/src/docking.cpp +++ b/mission/FSM/docking/src/docking.cpp @@ -79,16 +79,18 @@ ApproachDockingStationState::create_goal_handler( blackboard->set("is_home", false); - docking_fsm::PoseStamped docking_offset_goal = - blackboard->get("docking_offset_goal"); + docking_fsm::Pose docking_offset_goal = + blackboard->get("docking_offset_goal").pose; + + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = docking_offset_goal; - goal.goal = docking_offset_goal; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", - docking_offset_goal.pose.position.x, - docking_offset_goal.pose.position.y, - docking_offset_goal.pose.position.z); + docking_offset_goal.position.x, docking_offset_goal.position.y, + docking_offset_goal.position.z); return goal; } @@ -113,15 +115,15 @@ void ApproachDockingStationState::print_feedback( std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } GoAboveDockingStationState::GoAboveDockingStationState( @@ -145,7 +147,9 @@ GoAboveDockingStationState::create_goal_handler( auto docking_offset_goal = blackboard->get("docking_offset_goal"); - goal.goal = docking_offset_goal; + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = docking_offset_goal.pose; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", @@ -179,15 +183,15 @@ void GoAboveDockingStationState::print_feedback( std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } ConvergeDockingStationState::ConvergeDockingStationState( @@ -213,7 +217,9 @@ ConvergeDockingStationState::create_goal_handler( docking_fsm::PoseStamped dock_pose = blackboard->get("dock_pose"); - goal.goal = dock_pose; + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = dock_pose.pose; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", @@ -242,14 +248,14 @@ void ConvergeDockingStationState::print_feedback( std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } std::string DockedState( @@ -286,7 +292,9 @@ docking_fsm::ReturnHomeAction::Goal ReturnHomeState::create_goal_handler( docking_fsm::PoseStamped start_pose = blackboard->get("start_pose"); - goal.goal = start_pose; + vortex_msgs::msg::Waypoint waypoint; + waypoint.pose = start_pose.pose; + goal.waypoint = waypoint; spdlog::info("Goal sent to action server:"); spdlog::info(" Position: x = {}, y = {}, z = {}", start_pose.pose.position.x, start_pose.pose.position.y, @@ -313,17 +321,17 @@ void ReturnHomeState::print_feedback( std::shared_ptr blackboard, std::shared_ptr feedback) { docking_fsm::Pose current_pose = docking_fsm::Pose(); - current_pose.position.x = feedback->feedback.x; - current_pose.position.y = feedback->feedback.y; - current_pose.position.z = feedback->feedback.z; - current_pose.orientation.x = feedback->feedback.roll; - current_pose.orientation.y = feedback->feedback.pitch; - current_pose.orientation.z = feedback->feedback.yaw; + current_pose.position.x = feedback->reference.x; + current_pose.position.y = feedback->reference.y; + current_pose.position.z = feedback->reference.z; + current_pose.orientation.x = feedback->reference.roll; + current_pose.orientation.y = feedback->reference.pitch; + current_pose.orientation.z = feedback->reference.yaw; blackboard->set("current_pose", current_pose); spdlog::debug("Current position: x = {}, y = {}, z = {}", - feedback->feedback.x, feedback->feedback.y, - feedback->feedback.z); + feedback->reference.x, feedback->reference.y, + feedback->reference.z); } std::string AbortState( diff --git a/mission/waypoint_manager/CMakeLists.txt b/mission/waypoint_manager/CMakeLists.txt new file mode 100644 index 000000000..0a76ae510 --- /dev/null +++ b/mission/waypoint_manager/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(waypoint_manager) + +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_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/waypoint_manager_ros.cpp) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + rclcpp_action + vortex_msgs + vortex_utils + tf2_geometry_msgs + spdlog + fmt +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "WaypointManagerNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() diff --git a/mission/waypoint_manager/README.md b/mission/waypoint_manager/README.md new file mode 100644 index 000000000..3e6c6411a --- /dev/null +++ b/mission/waypoint_manager/README.md @@ -0,0 +1,84 @@ + + +# Waypoint Manager — ROS 2 Node + + +The **Waypoint Manager** node coordinates mission-level navigation by managing waypoint queues, forwarding them to a **Reference Filter** for trajectory generation, and exposing both an **action interface** (for mission planners) and a **service interface** (for perception-driven waypoint updates). + + + +# Interfaces + +* **[WaypointManager](https://github.com/vortexntnu/vortex-msgs/blob/main/action/WaypointManager.action) (action server)** +* **[WaypointAddition](https://github.com/vortexntnu/vortex-msgs/blob/main/srv/WaypointAddition.srv) (service server)** +* **[ReferenceFilterWaypoint](https://github.com/vortexntnu/vortex-msgs/blob/main/action/ReferenceFilterWaypoint.action) (action client)** + + +# Behavior Summary + +### **Mission Start** + +* On receiving a new action goal, any existing mission is aborted. +* Waypoints are stored, state is reset, and the first waypoint is sent to the reference filter. + +### **During Mission** + +* Each waypoint is executed sequentially. +* Pose feedback from the reference filter is forwarded to the mission planner. +* `convergence_threshold` determines when a waypoint is reached. +* If `persistent = true`, the action does not end even when waypoints run out. + +### **Mission End** + +* If `persistent = false`, the action completes when the last waypoint is reached. +* If cancelled externally, all state is cleared and reference filter goals are cancelled. + +### **Dynamic Waypoint Addition** + +Allowed only during **persistent** missions: + +* `overwrite = true` → clears old queue and restarts from new waypoints +* `overwrite = false` → appends waypoints to queue +* `priority = true` → turn on priority mode. In this mode additional service requests with `priority = false` are ignored. Priority mode is set to false again when there are no more waypoints in queue. + +--- + +### Check available interfaces: + +```bash +ros2 action list +ros2 service list +``` +beware of namespace! + + +## Example Action Goal (CLI) + +```bash +ros2 action send_goal /orca/waypoint_manager vortex_msgs/action/WaypointManager \ +'{ + waypoints:[ + { + pose:{position:{x:5.0,y:0.0,z:0.0}, + orientation:{x:0,y:0,z:0,w:1}}, + mode:1 + } + ], + convergence_threshold:0.1, + persistent:false +}' +``` + + +## Example Waypoint Addition Service Call + +```bash +ros2 service call /orca/waypoint_addition vortex_msgs/srv/WaypointAddition \ +'{ + waypoints:[ + {pose:{position:{x:2,y:3,z:0},orientation:{x:0,y:0,z:0,w:1}},mode:1} + ], + overwrite:false, + priority:true +}' +``` diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp new file mode 100644 index 000000000..86ba42d3e --- /dev/null +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp @@ -0,0 +1,120 @@ +#ifndef WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_ +#define WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace vortex::mission { + +using WaypointManager = vortex_msgs::action::WaypointManager; +using WaypointManagerGoalHandle = + rclcpp_action::ServerGoalHandle; + +using ReferenceFilterAction = vortex_msgs::action::ReferenceFilterWaypoint; +using ReferenceFilterGoalHandle = + rclcpp_action::ClientGoalHandle; + +class WaypointManagerNode : public rclcpp::Node { + public: + explicit WaypointManagerNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + ~WaypointManagerNode() override; + + private: + // @brief Create the action server for WaypointManager. + void set_waypoint_action_server(); + + // @brief Create the action client for ReferenceFilterWaypoint. + void set_reference_action_client(); + + // @brief Create the service servers for SendWaypoints. + void set_waypoint_service_server(); + + // @brief Construct the result message for the WaypointManager action + // @param success Whether the action was successful + // @return The constructed result message + std::shared_ptr + construct_result(bool success) const; + + // @brief Clean up the mission state after completion or cancellation of a + // waypoint action. Cancel active goals and reset internal variables. Make + // system ready for next action. + void cleanup_mission_state(); + + // @brief Send the next goal to the ReferenceFilter action server based on + // the current waypoint index or finish the waypoint action if all waypoints + // have been processed. + void send_next_reference_filter_goal(); + + // @brief Handle incoming action goal requests + // @param uuid The goal UUID + // @param goal The goal message + // @return The goal response + rclcpp_action::GoalResponse handle_waypoint_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr + goal_msg); + + // @brief Handle requests to cancel the waypoint action + // @param goal_handle The goal handle + // @return The cancel response + rclcpp_action::CancelResponse handle_waypoint_cancel( + const std::shared_ptr> goal_handle); + + // @brief Handle the accepted goal request + // @param goal_handle The goal handle + void handle_waypoint_accepted( + const std::shared_ptr> goal_handle); + + // @brief Handle incoming send waypoints service requests + // Only accepted if waypoint action is running. + // @param request Incoming service request containing waypoint information. + // @param response Service response that should be populated and sent back + // to the caller. + void handle_send_waypoints_service_request( + const std::shared_ptr request, + std::shared_ptr response); + + // @brief Send a goal to the reference filter + // @param goal_msg The action goal + void send_reference_filter_goal( + const vortex_msgs::action::ReferenceFilterWaypoint::Goal& goal_msg); + + rclcpp_action::Client:: + SharedPtr reference_filter_client_; + rclcpp_action::Server::SharedPtr + waypoint_action_server_; + rclcpp::Service::SharedPtr + waypoint_service_server_; + + std::vector waypoints_{}; + std::size_t current_index_{0}; + double convergence_threshold_{0.1}; + + bool persistent_action_mode_active_{false}; + bool priority_mode_active_{false}; + + ReferenceFilterAction::Feedback latest_ref_feedback_; + bool has_reference_pose_{false}; + bool is_cancel_in_progress_{false}; + + std::uint64_t mission_id_ = 0; + + std::shared_ptr active_reference_filter_goal_; + std::shared_ptr active_action_goal_; +}; + +} // namespace vortex::mission + +#endif // WAYPOINT_MANAGER__WAYPOINT_MANAGER_ROS_HPP_ diff --git a/mission/waypoint_manager/launch/waypoint_manager.launch.py b/mission/waypoint_manager/launch/waypoint_manager.launch.py new file mode 100644 index 000000000..65286ceb0 --- /dev/null +++ b/mission/waypoint_manager/launch/waypoint_manager.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + waypoint_manager_node = Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + parameters=[], + output='screen', + ) + return LaunchDescription([waypoint_manager_node]) diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml new file mode 100644 index 000000000..db3feabde --- /dev/null +++ b/mission/waypoint_manager/package.xml @@ -0,0 +1,28 @@ + + + + waypoint_manager + 0.0.0 + Waypoint manager for waypoint navigation with dp reference filter + Jorgen Fjermedal + MIT + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + vortex_utils + tf2_geometry_msgs + + launch_testing + launch_testing_ros + launch_testing_ament_cmake + reference_filter_dp + auv_setup + + + ament_cmake + + diff --git a/mission/waypoint_manager/src/waypoint_manager_ros.cpp b/mission/waypoint_manager/src/waypoint_manager_ros.cpp new file mode 100644 index 000000000..1d4928470 --- /dev/null +++ b/mission/waypoint_manager/src/waypoint_manager_ros.cpp @@ -0,0 +1,378 @@ +#include "waypoint_manager/waypoint_manager_ros.hpp" +#include +#include +#include +#include + +namespace vortex::mission { + +WaypointManagerNode::WaypointManagerNode(const rclcpp::NodeOptions& options) + : Node("waypoint_manager_node", options) { + set_reference_action_client(); + set_waypoint_action_server(); + set_waypoint_service_server(); + + spdlog::info("WaypointManagerNode started"); +} + +WaypointManagerNode::~WaypointManagerNode() { + if (active_action_goal_ && (active_action_goal_->is_active() || + active_action_goal_->is_canceling())) { + try { + auto res = construct_result(false); + active_action_goal_->abort(res); + } catch (...) { + } + } + + if (active_reference_filter_goal_) { + try { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + } catch (...) { + } + active_reference_filter_goal_.reset(); + } +} + +// --------------------------------------------------------- +// SETUP INTERFACES +// --------------------------------------------------------- + +void WaypointManagerNode::set_reference_action_client() { + reference_filter_client_ = + rclcpp_action::create_client(this, + "reference_filter"); + + if (!reference_filter_client_->wait_for_action_server( + std::chrono::seconds(3))) { + spdlog::warn("ReferenceFilter server not ready"); + } +} + +void WaypointManagerNode::set_waypoint_action_server() { + waypoint_action_server_ = rclcpp_action::create_server( + this, "waypoint_manager", + + [this](auto goal_id, auto goal) { + return handle_waypoint_goal(goal_id, goal); + }, + + [this](auto goal_id) { return handle_waypoint_cancel(goal_id); }, + + [this](auto goal_handle) { + return handle_waypoint_accepted(goal_handle); + }); +} + +void WaypointManagerNode::set_waypoint_service_server() { + waypoint_service_server_ = + this->create_service( + "waypoint_addition", + std::bind( + &WaypointManagerNode::handle_send_waypoints_service_request, + this, std::placeholders::_1, std::placeholders::_2)); +} + +// --------------------------------------------------------- +// HELPERS +// --------------------------------------------------------- + +std::shared_ptr +WaypointManagerNode::construct_result(bool success) const { + auto result = + std::make_shared(); + result->success = success; + result->pose_valid = has_reference_pose_; + if (has_reference_pose_) { + result->final_pose = + vortex::utils::ros_conversions::pose_like_to_pose_msg( + latest_ref_feedback_.reference); + } + return result; +} + +void WaypointManagerNode::cleanup_mission_state() { + waypoints_.clear(); + current_index_ = 0; + persistent_action_mode_active_ = false; + priority_mode_active_ = false; + has_reference_pose_ = false; + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + active_action_goal_.reset(); +} + +void WaypointManagerNode::send_next_reference_filter_goal() { + if (current_index_ >= waypoints_.size()) { + if (!persistent_action_mode_active_ && active_action_goal_ && + active_action_goal_->is_active()) { + auto wm_res = construct_result(true); + active_action_goal_->succeed(wm_res); + cleanup_mission_state(); + } + return; + } + + ReferenceFilterAction::Goal rf_goal; + rf_goal.waypoint = waypoints_[current_index_]; + rf_goal.convergence_threshold = convergence_threshold_; + + send_reference_filter_goal(rf_goal); +} + +// --------------------------------------------------------- +// WAYPOINT MANAGER ACTION SERVER +// --------------------------------------------------------- + +rclcpp_action::GoalResponse WaypointManagerNode::handle_waypoint_goal( + const rclcpp_action::GoalUUID& /*goal_uuid*/, + std::shared_ptr goal) { + if (active_action_goal_ && active_action_goal_->is_active()) { + auto wp_res = construct_result(false); + active_action_goal_->abort(wp_res); + } + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + ++mission_id_; + + waypoints_ = goal->waypoints; + current_index_ = 0; + persistent_action_mode_active_ = goal->persistent; + priority_mode_active_ = false; + has_reference_pose_ = false; + convergence_threshold_ = goal->convergence_threshold; + + if (waypoints_.empty() && !persistent_action_mode_active_) { + spdlog::warn( + "WaypointManager: received empty waypoint list and non-persistent " + "mode"); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void WaypointManagerNode::handle_waypoint_accepted( + const std::shared_ptr goal_handle) { + spdlog::info("WaypointManager: action goal accepted"); + active_action_goal_ = goal_handle; + + send_next_reference_filter_goal(); +} + +rclcpp_action::CancelResponse WaypointManagerNode::handle_waypoint_cancel( + const std::shared_ptr /*goal_handle*/) { + spdlog::info("WaypointManagerAction: cancel requested"); + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + return rclcpp_action::CancelResponse::ACCEPT; +} + +// --------------------------------------------------------- +// WAYPOINT MANAGER SERVICE SERVER +// --------------------------------------------------------- + +void WaypointManagerNode::handle_send_waypoints_service_request( + const std::shared_ptr request, + std::shared_ptr response) { + if (!persistent_action_mode_active_ || !active_action_goal_ || + !active_action_goal_->is_active()) { + response->success = false; + return; + } + + if (priority_mode_active_ && !request->take_priority && + current_index_ < waypoints_.size()) { + response->success = false; + return; + } + + priority_mode_active_ = request->take_priority; + + if (request->overwrite_prior_waypoints) { + waypoints_ = request->waypoints; + current_index_ = 0; + has_reference_pose_ = false; + + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + if (!waypoints_.empty()) { + send_next_reference_filter_goal(); + } + + response->success = true; + return; + } + + waypoints_.insert(waypoints_.end(), request->waypoints.begin(), + request->waypoints.end()); + + if (!active_reference_filter_goal_ && current_index_ < waypoints_.size()) { + send_next_reference_filter_goal(); + } + + response->success = true; +} + +// --------------------------------------------------------- +// REFERENCE FILTER ACTION CLIENT +// --------------------------------------------------------- + +void WaypointManagerNode::send_reference_filter_goal( + const ReferenceFilterAction::Goal& goal_msg) { + if (active_reference_filter_goal_) { + reference_filter_client_->async_cancel_goal( + active_reference_filter_goal_); + active_reference_filter_goal_.reset(); + } + + const std::uint64_t this_mission = mission_id_; + + rclcpp_action::Client::SendGoalOptions options; + + options.goal_response_callback = + [this, this_mission](ReferenceFilterGoalHandle::SharedPtr gh) { + if (!gh) { + spdlog::warn("ReferenceFilter goal rejected"); + return; + } + + if (this_mission == mission_id_) { + active_reference_filter_goal_ = gh; + } else { + spdlog::info( + "RF goal response for old mission, ignoring handle"); + } + }; + + options.feedback_callback = + [this, this_mission]( + ReferenceFilterGoalHandle::SharedPtr, + const std::shared_ptr fb) { + if (this_mission != mission_id_) { + return; + } + + latest_ref_feedback_ = *fb; + has_reference_pose_ = true; + + if (!active_action_goal_ || !active_action_goal_->is_active()) + return; + + geometry_msgs::msg::Pose robot_pose = + vortex::utils::ros_conversions::pose_like_to_pose_msg( + fb->reference); + + if (current_index_ < waypoints_.size()) { + auto wm_fb = std::make_shared(); + wm_fb->current_pose = robot_pose; + wm_fb->current_waypoint = waypoints_[current_index_]; + active_action_goal_->publish_feedback(wm_fb); + } + }; + + options + .result_callback = [this, this_mission]( + const ReferenceFilterGoalHandle::WrappedResult& + res) { + if (this_mission != mission_id_) { + spdlog::info( + "ReferenceFilter result received for old mission, ignoring."); + return; + } + + active_reference_filter_goal_.reset(); + + if (!active_action_goal_) { + spdlog::info( + "ReferenceFilter result received but no active WM goal"); + return; + } + + const bool wm_canceling = active_action_goal_->is_canceling(); + const bool wm_active = active_action_goal_->is_active(); + + switch (res.code) { + case rclcpp_action::ResultCode::SUCCEEDED: { + spdlog::info("ReferenceFilter goal reached waypoint"); + + if (wm_canceling) { + bool action_success = false; + if (persistent_action_mode_active_ && + current_index_ >= waypoints_.size()) { + action_success = true; + } + + auto wp_res = construct_result(action_success); + + active_action_goal_->canceled(wp_res); + + cleanup_mission_state(); + } else { + current_index_++; + send_next_reference_filter_goal(); + } + break; + } + + case rclcpp_action::ResultCode::CANCELED: { + spdlog::info("ReferenceFilter goal cancelled"); + + if (wm_canceling && wm_active) { + bool action_success = false; + if (persistent_action_mode_active_ && + current_index_ >= waypoints_.size()) { + action_success = true; + } + + auto wp_res = construct_result(action_success); + + active_action_goal_->canceled(wp_res); + cleanup_mission_state(); + } + break; + } + + case rclcpp_action::ResultCode::ABORTED: { + spdlog::warn("ReferenceFilter goal aborted unexpectedly"); + if (wm_active) { + auto wp_res = construct_result(false); + active_action_goal_->abort(wp_res); + cleanup_mission_state(); + } + break; + } + + default: + spdlog::error( + "ReferenceFilter goal returned unknown result code"); + break; + } + }; + + reference_filter_client_->async_send_goal(goal_msg, options); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(WaypointManagerNode) + +} // namespace vortex::mission diff --git a/mission/waypoint_manager/test/CMakeLists.txt b/mission/waypoint_manager/test/CMakeLists.txt new file mode 100644 index 000000000..d63481816 --- /dev/null +++ b/mission/waypoint_manager/test/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(waypoint_manager_test) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +find_package(ament_cmake_ros REQUIRED) +find_package(launch_testing_ament_cmake REQUIRED) + +function(add_ros_isolated_launch_test path) + set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") + add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) +endfunction() + +add_ros_isolated_launch_test(test_single_waypoint.py TIMEOUT 300) +add_ros_isolated_launch_test(test_service.py TIMEOUT 300) diff --git a/mission/waypoint_manager/test/test_service.py b/mission/waypoint_manager/test/test_service.py new file mode 100644 index 000000000..6bc827b97 --- /dev/null +++ b/mission/waypoint_manager/test/test_service.py @@ -0,0 +1,153 @@ +import os +import unittest +import uuid + +import launch +import launch_ros.actions +import launch_testing +import launch_testing.actions +import rclpy +from ament_index_python.packages import get_package_share_directory +from rclpy.action import ActionClient +from vortex_msgs.action import WaypointManager +from vortex_msgs.msg import Waypoint +from vortex_msgs.srv import SendWaypoints + + +def generate_test_description(): + rf_pkg_share = get_package_share_directory('reference_filter_dp') + rf_config = os.path.join(rf_pkg_share, 'config', 'reference_filter_params.yaml') + + auv_setup_share = get_package_share_directory('auv_setup') + orca_config = os.path.join(auv_setup_share, 'config', 'robots', 'orca.yaml') + + wm_node = launch_ros.actions.Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + output='screen', + ) + + rf_node = launch_ros.actions.Node( + package='reference_filter_dp', + executable='reference_filter_dp_node', + name='reference_filter_node', + namespace='orca', + parameters=[rf_config, orca_config], + output='screen', + ) + + return launch.LaunchDescription( + [ + wm_node, + rf_node, + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestWaypointManagerService(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node( + f'test_waypoint_manager_client_{uuid.uuid4().hex[:8]}' + ) + + def tearDown(self): + self.node.destroy_node() + + def _call_send_waypoints( + self, + waypoints, + switching_threshold=0.3, + overwrite=False, + take_priority=False, + timeout=5.0, + ): + client = self.node.create_client(SendWaypoints, '/orca/waypoint_addition') + assert client.wait_for_service(timeout_sec=5.0), ( + 'SendWaypoints service not available' + ) + + req = SendWaypoints.Request() + req.waypoints = list(waypoints) + req.switching_threshold = float(switching_threshold) + req.overwrite_prior_waypoints = bool(overwrite) + req.take_priority = bool(take_priority) + + fut = client.call_async(req) + rclpy.spin_until_future_complete(self.node, fut, timeout_sec=timeout) + assert fut.done(), 'Timed out waiting for SendWaypoints response' + return fut.result() + + def test_service_returns_false_when_no_persistent_action(self): + # When no persistent action is active, service should return false + wp = Waypoint() + wp.pose.position.x = 1.0 + wp.pose.position.y = 2.0 + wp.pose.position.z = 1.0 + + resp = self._call_send_waypoints([wp], overwrite=False, take_priority=False) + assert not resp.success, ( + 'Service should return false when no persistent action is active' + ) + + def test_priority_blocks_non_priority_calls(self): + # Send a persistent action goal so the waypoint manager is in persistent mode + action_client = ActionClient( + self.node, WaypointManager, '/orca/waypoint_manager' + ) + assert action_client.wait_for_server(timeout_sec=10.0), ( + 'WaypointManager action server not available' + ) + + goal_msg = WaypointManager.Goal() + wp1 = Waypoint() + wp1.pose.position.x = 0.0 + wp1.pose.position.y = 0.0 + wp1.pose.position.z = 1.0 + wp2 = Waypoint() + wp2.pose.position.x = 1.0 + wp2.pose.position.y = 1.0 + wp2.pose.position.z = 1.0 + + goal_msg.waypoints = [wp1, wp2] + goal_msg.persistent = True + goal_msg.convergence_threshold = 0.3 + + send_fut = action_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self.node, send_fut, timeout_sec=20.0) + assert send_fut.done(), 'Timed out waiting for goal response' + goal_handle = send_fut.result() + assert goal_handle.accepted, 'Goal was rejected' + + # Call service to take priority + resp1 = self._call_send_waypoints([], overwrite=False, take_priority=True) + assert resp1.success, 'Priority service call should succeed' + + # Now a non-priority call while priority is active should be rejected + wp_extra = Waypoint() + wp_extra.pose.position.x = 2.0 + wp_extra.pose.position.y = 2.0 + wp_extra.pose.position.z = 1.0 + + resp2 = self._call_send_waypoints( + [wp_extra], overwrite=False, take_priority=False + ) + assert not resp2.success, ( + 'Non-priority call should be rejected while priority is active' + ) + + +@launch_testing.post_shutdown_test() +class TestAfterShutdown(unittest.TestCase): + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/mission/waypoint_manager/test/test_single_waypoint.py b/mission/waypoint_manager/test/test_single_waypoint.py new file mode 100644 index 000000000..35e02b79e --- /dev/null +++ b/mission/waypoint_manager/test/test_single_waypoint.py @@ -0,0 +1,147 @@ +import os +import time +import unittest +import uuid + +import launch +import launch_ros.actions +import launch_testing +import launch_testing.actions +import rclpy +from ament_index_python.packages import get_package_share_directory +from nav_msgs.msg import Odometry +from rclpy.action import ActionClient +from rclpy.qos import qos_profile_sensor_data +from vortex_msgs.action import WaypointManager +from vortex_msgs.msg import Waypoint + + +def generate_test_description(): + rf_pkg_share = get_package_share_directory('reference_filter_dp') + rf_config = os.path.join(rf_pkg_share, 'config', 'reference_filter_params.yaml') + + auv_setup_share = get_package_share_directory('auv_setup') + orca_config = os.path.join(auv_setup_share, 'config', 'robots', 'orca.yaml') + + wm_node = launch_ros.actions.Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager_node', + namespace='orca', + output='screen', + ) + + rf_node = launch_ros.actions.Node( + package='reference_filter_dp', + executable='reference_filter_dp_node', + name='reference_filter_node', + namespace='orca', + parameters=[rf_config, orca_config], + output='screen', + ) + + return launch.LaunchDescription( + [ + wm_node, + rf_node, + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestWaypointManagerAcceptsGoal(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node( + f'test_waypoint_manager_client_{uuid.uuid4().hex[:8]}' + ) + + def tearDown(self): + self.node.destroy_node() + + def _publish_fake_odom(self, x, y, z, duration_sec=5.0, rate_hz=10.0): + pub = self.node.create_publisher( + Odometry, + '/orca/odom', + qos_profile_sensor_data, + ) + + msg = Odometry() + msg.header.frame_id = 'odom' + msg.child_frame_id = 'base_link' + msg.pose.pose.position.x = x + msg.pose.pose.position.y = y + msg.pose.pose.position.z = z + + end_time = time.time() + duration_sec + period = 1.0 / rate_hz + + while time.time() < end_time: + msg.header.stamp = self.node.get_clock().now().to_msg() + pub.publish(msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(period) + + self.node.destroy_publisher(pub) + + def test_accepts_and_executes_goal(self): + client = ActionClient(self.node, WaypointManager, '/orca/waypoint_manager') + + assert client.wait_for_server(timeout_sec=10.0), ( + 'WaypointManager action server not available' + ) + + goal_msg = WaypointManager.Goal() + wp = Waypoint() + wp.pose.position.x = 0.0 + wp.pose.position.y = 0.0 + wp.pose.position.z = 1.0 + + goal_msg.waypoints = [wp] + goal_msg.persistent = False + goal_msg.convergence_threshold = 0.3 + + send_fut = client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete( + self.node, + send_fut, + timeout_sec=20.0, + ) + + assert send_fut.done(), 'Timed out waiting for goal response' + + goal_handle = send_fut.result() + assert goal_handle.accepted, 'Goal was rejected' + + # Publish fake odometry at the goal position + self._publish_fake_odom( + x=wp.pose.position.x, + y=wp.pose.position.y, + z=wp.pose.position.z, + duration_sec=5.0, + ) + + result_fut = goal_handle.get_result_async() + rclpy.spin_until_future_complete( + self.node, + result_fut, + timeout_sec=20.0, + ) + + assert result_fut.done(), 'Timed out waiting for result' + + result = result_fut.result().result + assert result.success, 'Waypoint execution failed' + + +@launch_testing.post_shutdown_test() +class TestAfterShutdown(unittest.TestCase): + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/tests/ros_node_tests/reference_filter_node_test.sh b/tests/ros_node_tests/reference_filter_node_test.sh old mode 100644 new mode 100755 index f9278d31c..8561bf190 --- a/tests/ros_node_tests/reference_filter_node_test.sh +++ b/tests/ros_node_tests/reference_filter_node_test.sh @@ -28,7 +28,10 @@ fi # Send action goal echo "Sending goal..." -ros2 action send_goal /orca/reference_filter vortex_msgs/action/ReferenceFilterWaypoint "{goal: {pose: {position: {x: 1.0}}}}" & +ros2 action send_goal /orca/reference_filter vortex_msgs/action/ReferenceFilterWaypoint \ +"{waypoint: {pose: {position: {x: 1.0,y: 0.0,z: 0.0}, orientation:{x: 0,y: 0,z: 0,w: 1}}, mode: 0}}" & + +sleep 2 # Check if controller correctly publishes guidance echo "Waiting for guidance data..." diff --git a/tests/simulator_tests/waypoint_manager_test/check_goal.py b/tests/simulator_tests/waypoint_manager_test/check_goal.py new file mode 100644 index 000000000..89f4a023e --- /dev/null +++ b/tests/simulator_tests/waypoint_manager_test/check_goal.py @@ -0,0 +1,103 @@ +import math +import os +import time + +import rclpy +import yaml +from geometry_msgs.msg import PoseWithCovarianceStamped +from rclpy.node import Node +from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy +from vortex_utils.python_utils import quat_to_euler + +best_effort_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=QoSReliabilityPolicy.BEST_EFFORT, +) + +# Read goal from temp file +file_path = 'goal_pose.yaml' +with open(file_path) as f: + data = yaml.safe_load(f) + +# Remove temp file +os.remove(file_path) +print(f"Temp file {file_path} deleted") +goal_pos = data['pos'] +goal_ori = data['ori'] + +pos_tol = 0.6 # meters (match python test tolerance) +ori_tol = 0.5 # rad (loose) + + +class CheckGoalNode(Node): + def __init__(self): + super().__init__('check_goal_node') + self.pose_sub_ = self.create_subscription( + PoseWithCovarianceStamped, '/orca/pose', self.pose_callback, best_effort_qos + ) + + self.current_pose_: PoseWithCovarianceStamped = None + self.received_pose_: bool = False + + def pose_callback(self, msg: PoseWithCovarianceStamped): + self.current_pose_ = msg + self.received_pose_ = True + + +def main(args=None): + rclpy.init(args=args) + node = CheckGoalNode() + + print(f"Waiting for vehicle to reach goal: {goal_pos} with orientation {goal_ori}") + + start_time = time.time() + timeout = 240 # seconds; waypoint manager may take longer + + x = y = z = None + current_ori = (0.0, 0.0, 0.0) + + while rclpy.ok() and time.time() - start_time < timeout: + rclpy.spin_once(node) + if node.received_pose_: + x = node.current_pose_.pose.pose.position.x + y = node.current_pose_.pose.pose.position.y + z = node.current_pose_.pose.pose.position.z + q_w = node.current_pose_.pose.pose.orientation.w + q_x = node.current_pose_.pose.pose.orientation.x + q_y = node.current_pose_.pose.pose.orientation.y + q_z = node.current_pose_.pose.pose.orientation.z + current_ori = quat_to_euler(x=q_x, y=q_y, z=q_z, w=q_w) + dist = math.sqrt( + (goal_pos[0] - x) ** 2 + (goal_pos[1] - y) ** 2 + (goal_pos[2] - z) ** 2 + ) + + dist_ori = math.sqrt( + (goal_ori[0] - current_ori[0]) ** 2 + + (goal_ori[1] - current_ori[1]) ** 2 + + (goal_ori[2] - current_ori[2]) ** 2 + ) + + if dist < pos_tol and dist_ori < ori_tol: + print( + f"Vehicle reached final waypoint: {goal_pos} and orientation: {goal_ori}" + ) + print(f"Final vehicle pose: ({x, y, z}), {current_ori}") + print(f"Euclidean error: {dist}, {dist_ori}") + rclpy.shutdown() + exit(0) + + print( + f"Vehicle did not reach final waypoint: {goal_pos} and orientation: {goal_ori}" + ) + if x is not None: + print(f"Current_vehicle pose: ({x, y, z}), {current_ori}") + print( + f"Euclidean error: {dist if 'dist' in locals() else 'N/A'}, {dist_ori if 'dist_ori' in locals() else 'N/A'}" + ) + rclpy.shutdown() + exit(1) + + +if __name__ == '__main__': + main() diff --git a/tests/simulator_tests/waypoint_manager_test/send_goal.py b/tests/simulator_tests/waypoint_manager_test/send_goal.py new file mode 100644 index 000000000..427bcb853 --- /dev/null +++ b/tests/simulator_tests/waypoint_manager_test/send_goal.py @@ -0,0 +1,91 @@ +import random + +import rclpy +import yaml +from rclpy.action import ActionClient +from rclpy.node import Node +from vortex_msgs.action import ReferenceFilterWaypoint +from vortex_utils.python_utils import PoseData, euler_to_quat + + +def randomize_pose() -> PoseData: + pose: PoseData = PoseData() + pose.x = random.uniform(-10.0, 10.0) + pose.y = random.uniform(-10.0, 10.0) + pose.z = random.uniform(0.5, 3.0) + pose.roll = 0.0 + pose.pitch = random.uniform(-1.0, 1.0) + pose.yaw = random.uniform(-1.57, 1.57) + + return pose + + +class ReferenceFilterWaypointClient(Node): + def __init__(self): + super().__init__('reference_filter_waypoint_client') + + self._action_client = ActionClient( + self, ReferenceFilterWaypoint, '/orca/reference_filter' + ) + self.send_goal() + + def send_goal(self): + goal_pose = randomize_pose() + goal_msg = ReferenceFilterWaypoint.Goal() + goal_msg.waypoint.pose.position.x = goal_pose.x + goal_msg.waypoint.pose.position.y = goal_pose.y + goal_msg.waypoint.pose.position.z = goal_pose.z + roll = goal_pose.roll + pitch = goal_pose.pitch + yaw = goal_pose.yaw + + quat = euler_to_quat(roll=roll, pitch=pitch, yaw=yaw) + + goal_msg.waypoint.pose.orientation.x = quat[0] + goal_msg.waypoint.pose.orientation.y = quat[1] + goal_msg.waypoint.pose.orientation.z = quat[2] + goal_msg.waypoint.pose.orientation.w = quat[3] + + # Write goal pose to temp file + file_path = "goal_pose.yaml" + + data = { + "pos": [goal_pose.x, goal_pose.y, goal_pose.z], + "ori": [goal_pose.roll, goal_pose.pitch, goal_pose.yaw], + } + + with open(file_path, "w") as f: + yaml.safe_dump(data, f) + + # Send the goal asynchronously + self._action_client.wait_for_server(timeout_sec=10.0) + self.get_logger().info(f'Sending goal {goal_pose}...') + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + + self.get_logger().info('Goal accepted :)') + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result.success + self.get_logger().info(f'Goal result: {result}') + self.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +def main(args=None): + rclpy.init(args=args) + action_client = ReferenceFilterWaypointClient() + rclpy.spin(action_client) + + +if __name__ == '__main__': + main() diff --git a/tests/simulator_tests/waypoint_manager_test/simulator_test.sh b/tests/simulator_tests/waypoint_manager_test/simulator_test.sh new file mode 100755 index 000000000..1d726305f --- /dev/null +++ b/tests/simulator_tests/waypoint_manager_test/simulator_test.sh @@ -0,0 +1,115 @@ +#!/bin/bash +set -e +set -o pipefail + +echo "Setting up ROS 2 environment..." +. /opt/ros/humble/setup.sh +. "${WORKSPACE:-$HOME/ros2_ws}/install/setup.bash" +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib + +# Get the directory of this script dynamically +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + +# Function to terminate processes safely on error +cleanup() { + echo "Error detected. Cleaning up..." + # Safely kill any started PIDs (ignore empty values) + for _pid in "$SIM_PID" "$ORCA_PID" "$WM_PID" "$CONTROLLER_PID" "$FILTER_PID" "$BAG_PID"; do + if [ -n "$_pid" ]; then + kill -TERM "$_pid" 2>/dev/null || true + fi + done + exit 1 +} +trap cleanup ERR + +setsid ros2 bag record -o ${WORKSPACE}/bags/recording -s mcap -a & +BAG_PID=$! +echo "Started bagging with PID: $BAG_PID" + +# Launch Stonefish Simulator +setsid ros2 launch stonefish_sim simulation.launch.py rendering:=false scenario:=orca_no_gpu & +SIM_PID=$! +echo "Launched simulator with PID: $SIM_PID" + +echo "Waiting for simulator to start..." +timeout 30s bash -c ' + while ! ros2 topic list | grep -q "/orca/odom"; do + sleep 1 + done || true' +echo "Simulator started" + +# Check for ROS errors in logs +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Wait for odometry data +echo "Waiting for odom data..." +timeout 10s ros2 topic echo /orca/odom --once +echo "Got odom data" + +# Launch ORCA Simulation +setsid ros2 launch stonefish_sim orca_sim.launch.py & +ORCA_PID=$! +echo "Launched orca with PID: $ORCA_PID" + +setsid ros2 launch waypoint_manager waypoint_manager.launch.py & +WM_PID=$! +echo "Launched waypoint manager with PID: $WM_PID" + +echo "Waiting for sim interface to start..." +timeout 30s bash -c 'until ros2 topic list | grep -q "/orca/pose"; do sleep 1; done' +echo "Simulator started" + +# Check for ROS errors again +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Wait for pose data +echo "Waiting for pose data..." +timeout 10s ros2 topic echo /orca/pose --once +echo "Got pose data" + +# Launch controller and reference filter +setsid ros2 launch auv_setup dp.launch.py & +CONTROLLER_PID=$! +echo "Launched controller and reference filter with PID: $CONTROLLER_PID" + +# Check for ROS errors before continuing +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Set operation mode +echo "Turning off killswitch and setting operation mode to autonomous mode" +ros2 topic pub /orca/killswitch std_msgs/msg/Bool "{data: false}" -t 5 # Ensure the message arrives +ros2 topic pub /orca/operation_mode std_msgs/msg/String "{data: 'autonomous mode'}" -t 5 # Ensure the message arrives + +# Send waypoint goal +echo "Sending goal" +python3 "$SCRIPT_DIR/send_goal.py" + +# Check if goal reached +echo "Checking if goal reached" +python3 "$SCRIPT_DIR/check_goal.py" + +if [ $? -ne 0 ]; then + echo "Test failed: Vehicle did not reach final waypoint." + exit 1 +else + echo "Test passed: Vehicle reached final waypoint." +fi + +# Terminate processes (safely) +for _pid in "$SIM_PID" "$ORCA_PID" "$WM_PID" "$CONTROLLER_PID" "$BAG_PID"; do + if [ -n "$_pid" ]; then + kill -TERM "$_pid" 2>/dev/null || true + fi +done + +echo "Test completed successfully." diff --git a/tests/simulator_tests/waypoint_navigation/send_goal.py b/tests/simulator_tests/waypoint_navigation/send_goal.py index bb04a7686..25cb3275d 100644 --- a/tests/simulator_tests/waypoint_navigation/send_goal.py +++ b/tests/simulator_tests/waypoint_navigation/send_goal.py @@ -33,19 +33,18 @@ def send_goal(self): goal_pose = randomize_pose() goal_msg = ReferenceFilterWaypoint.Goal() - goal_msg.goal.pose.position.x = goal_pose.x - goal_msg.goal.pose.position.y = goal_pose.y - goal_msg.goal.pose.position.z = goal_pose.z + goal_msg.waypoint.pose.position.x = goal_pose.x + goal_msg.waypoint.pose.position.y = goal_pose.y + goal_msg.waypoint.pose.position.z = goal_pose.z roll = goal_pose.roll pitch = goal_pose.pitch yaw = goal_pose.yaw quat = euler_to_quat(roll=roll, pitch=pitch, yaw=yaw) - - goal_msg.goal.pose.orientation.x = quat[0] - goal_msg.goal.pose.orientation.y = quat[1] - goal_msg.goal.pose.orientation.z = quat[2] - goal_msg.goal.pose.orientation.w = quat[3] + goal_msg.waypoint.pose.orientation.x = quat[0] + goal_msg.waypoint.pose.orientation.y = quat[1] + goal_msg.waypoint.pose.orientation.z = quat[2] + goal_msg.waypoint.pose.orientation.w = quat[3] # Write goal pose to temp file file_path = "goal_pose.yaml" From 8f1555276224d3d4333911f25cf2a07013ec7fda Mon Sep 17 00:00:00 2001 From: jorgenfj <144696109+jorgenfj@users.noreply.github.com> Date: Mon, 22 Dec 2025 10:55:07 +0100 Subject: [PATCH 18/87] Refactor/type rename (#647) * waypoint mode handling * update tests with new message * initial package setup * ros interface function declaration * node setup * working prototype * reentrant cb, multithreaded * single threaded impl * conv threshold action goal * default thresholdref conv value * removed switching logic * removed timer execution * sim test utils * waypoint_manager_test setup * no rendering test arg * waypoint tests, timeout error * test refactor * format * rename utils package * test suite and description * first waypoint test * removed unused function * renamed service field to priority. Added simple tests * waypoint manager readme * uniform attitude naming convention * fix pr requests * update tests with new service fields * four corner test * update util func name * update with new action def * removed failing build type * test dependencies * ignore failing yasmin package * remove __init__ files * quat_to_euler in make_pose helper * added __init__ file * removed sim deps for test packages * added action shutdown handling * removed waypoint manager set setup * added waypoint manager node tests * waypoint manager 4 corner sim test * added missing launch testing test dependency * add sleep for topic discovery * fix action member field name * updated to new utils type names * renamed variables to match types * update function arg to reflect vortex type * update variable name in tests * renamed function arg names --- .../dp_adapt_backs_controller.hpp | 13 +- .../dp_adapt_backs_controller_ros.hpp | 6 +- .../dp_adapt_backs_controller_utils.hpp | 40 ++-- .../src/dp_adapt_backs_controller.cpp | 28 +-- .../src/dp_adapt_backs_controller_ros.cpp | 40 ++-- .../src/dp_adapt_backs_controller_utils.cpp | 72 +++---- .../test/dp_adapt_backs_controller_tests.cpp | 192 +++++++++--------- .../src/reference_filter_ros.cpp | 42 ++-- .../src/waypoint_manager_ros.cpp | 8 +- 9 files changed, 222 insertions(+), 219 deletions(-) diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp index 6a44644e8..a26969cee 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp @@ -23,15 +23,16 @@ class DPAdaptBacksController { explicit DPAdaptBacksController(const DPAdaptParams& dp_adapt_params); // @brief Calculate thecontrol input tau - // @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, + // @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, // yaw] - // @param eta_d: 6D vector containing the desired vehicle pose [x, y, z, + // @param pose_d: 6D vector containing the desired vehicle pose [x, y, z, // roll, pitch, yaw] - // @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] + // @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, + // r] // @return 6D vector containing the control input tau [X, Y, Z, K, M, N] - Eigen::Vector6d calculate_tau(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Eta& eta_d, - const vortex::utils::types::Nu& nu); + Eigen::Vector6d calculate_tau(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::PoseEuler& pose_d, + const vortex::utils::types::Twist& twist); // @brief Reset the adaptive parameters void reset_adap_param(); diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp index 85b1c7f7a..478bdce2f 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_ros.hpp @@ -80,11 +80,11 @@ class DPAdaptBacksControllerNode : public rclcpp::Node { std::chrono::milliseconds time_step_{}; - vortex::utils::types::Eta eta_; + vortex::utils::types::PoseEuler pose_; - vortex::utils::types::Eta eta_d_; + vortex::utils::types::PoseEuler pose_d_; - vortex::utils::types::Nu nu_; + vortex::utils::types::Twist twist_; std::unique_ptr dp_adapt_backs_controller_{}; diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp index b48a21867..7a0e214a9 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp @@ -8,45 +8,49 @@ namespace vortex::control { // @brief Calculate the derivative of the rotation matrix -// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] -// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, +// yaw] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] // @return 3x3 derivative of the rotation matrix -Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu); +Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist); // @brief Calculate the derivative of the transformation matrix -// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] -// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, +// yaw] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] // @return 3x3 derivative of the transformation matrix -Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu); +Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist); // @brief Calculate the pseudo-inverse of the Jacobian matrix -// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] +// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, +// yaw] // @return 6x6 pseudo-inverse Jacobian matrix -Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta); +Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::PoseEuler& pose); // @brief calculate the derivative of the Jacobian matrix -// @param eta: 6D vector containing the vehicle pose [x, y, z, roll, pitch, yaw] -// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] -Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu); +// @param pose: 6D vector containing the vehicle pose [x, y, z, roll, pitch, +// yaw] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist); // @brief Calculate the coriolis matrix // @param m: mass of the vehicle // @param r_b_bg: 3D vector of the body frame to the center of gravity -// @param nu_2: 3D vector containing angular velocity of the vehicle +// @param twist: 6D vector containing linear and angular velocity of the vehicle // @param I_b : 3D matrix containing the inertia matrix // @return 6x6 coriolis matrix Eigen::Matrix6d calculate_coriolis(const double mass, const Eigen::Vector3d& r_b_bg, - const vortex::utils::types::Nu& nu, + const vortex::utils::types::Twist& twist, const Eigen::Matrix3d& I_b); // @brief Calculate the damping matrix for the adaptive backstepping controller -// @param nu: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] // @return 6x6 damping matrix -Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Nu& nu); +Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist); } // namespace vortex::control diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp index e95af7ba5..78102c0bf 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp @@ -7,8 +7,8 @@ namespace vortex::control { -using vortex::utils::types::Eta; -using vortex::utils::types::Nu; +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Twist; DPAdaptBacksController::DPAdaptBacksController( const DPAdaptParams& dp_adapt_params) @@ -24,29 +24,29 @@ DPAdaptBacksController::DPAdaptBacksController( m_(dp_adapt_params.mass), dt_(0.01) {} -Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Eta& eta, - const Eta& eta_d, - const Nu& nu) { - Eta error = eta - eta_d; +Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose, + const PoseEuler& pose_d, + const Twist& twist) { + PoseEuler error = pose - pose_d; error.roll = vortex::utils::math::ssa(error.roll); error.pitch = vortex::utils::math::ssa(error.pitch); error.yaw = vortex::utils::math::ssa(error.yaw); - Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, nu, I_b_); - Eigen::Matrix6d J_inv = calculate_J_inv(eta); - Eigen::Matrix6d J_dot = calculate_J_dot(eta, nu); + Eigen::Matrix6d C = calculate_coriolis(m_, r_b_bg_, twist, I_b_); + Eigen::Matrix6d J_inv = calculate_J_inv(pose); + Eigen::Matrix6d J_dot = calculate_J_dot(pose, twist); Eigen::Vector6d alpha = -J_inv * K1_ * error.to_vector(); Eigen::Vector6d z_1 = error.to_vector(); - Eigen::Vector6d z_2 = nu.to_vector() - alpha; + Eigen::Vector6d z_2 = twist.to_vector() - alpha; Eigen::Vector6d alpha_dot = ((J_inv * J_dot * J_inv) * K1_ * z_1) - - (J_inv * K1_ * eta.as_j_matrix() * nu.to_vector()); - Eigen::Matrix6x12d Y_v = calculate_Y_v(nu); + (J_inv * K1_ * pose.as_j_matrix() * twist.to_vector()); + Eigen::Matrix6x12d Y_v = calculate_Y_v(twist); Eigen::Vector12d adapt_param_dot = adapt_gain_ * Y_v.transpose() * z_2; Eigen::Vector6d d_est_dot = d_gain_ * z_2; Eigen::Vector6d F_est = Y_v * adapt_param_; - Eigen::Vector6d tau = (mass_matrix_ * alpha_dot) + (C * nu.to_vector()) - - (eta.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - + Eigen::Vector6d tau = (mass_matrix_ * alpha_dot) + (C * twist.to_vector()) - + (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; tau = tau.cwiseMax(-80.0).cwiseMin(80.0); diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp index da70eaf79..e2cb298e5 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp @@ -100,31 +100,31 @@ void DPAdaptBacksControllerNode::software_mode_callback( spdlog::info("Software mode: {}", software_mode_); if (software_mode_ == "autonomous mode") { - eta_d_ = eta_; + pose_d_ = pose_; } } void DPAdaptBacksControllerNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - eta_.x = msg->pose.pose.position.x; - eta_.y = msg->pose.pose.position.y; - eta_.z = msg->pose.pose.position.z; + pose_.x = msg->pose.pose.position.x; + pose_.y = msg->pose.pose.position.y; + pose_.z = msg->pose.pose.position.z; const auto& o = msg->pose.pose.orientation; Eigen::Quaterniond q(o.w, o.x, o.y, o.z); Eigen::Vector3d euler_angles = vortex::utils::math::quat_to_euler(q); - eta_.roll = euler_angles(0); - eta_.pitch = euler_angles(1); - eta_.yaw = euler_angles(2); + pose_.roll = euler_angles(0); + pose_.pitch = euler_angles(1); + pose_.yaw = euler_angles(2); } void DPAdaptBacksControllerNode::twist_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - nu_.u = msg->twist.twist.linear.x; - nu_.v = msg->twist.twist.linear.y; - nu_.w = msg->twist.twist.linear.z; - nu_.p = msg->twist.twist.angular.x; - nu_.q = msg->twist.twist.angular.y; - nu_.r = msg->twist.twist.angular.z; + twist_.u = msg->twist.twist.linear.x; + twist_.v = msg->twist.twist.linear.y; + twist_.w = msg->twist.twist.linear.z; + twist_.p = msg->twist.twist.angular.x; + twist_.q = msg->twist.twist.angular.y; + twist_.r = msg->twist.twist.angular.z; } void DPAdaptBacksControllerNode::set_adap_params() { @@ -185,7 +185,7 @@ void DPAdaptBacksControllerNode::publish_tau() { } Eigen::Vector6d tau = - dp_adapt_backs_controller_->calculate_tau(eta_, eta_d_, nu_); + dp_adapt_backs_controller_->calculate_tau(pose_, pose_d_, twist_); geometry_msgs::msg::WrenchStamped tau_msg; tau_msg.header.stamp = this->now(); @@ -203,12 +203,12 @@ void DPAdaptBacksControllerNode::publish_tau() { void DPAdaptBacksControllerNode::guidance_callback( const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { - eta_d_.x = msg->x; - eta_d_.y = msg->y; - eta_d_.z = msg->z; - eta_d_.roll = msg->roll; - eta_d_.pitch = msg->pitch; - eta_d_.yaw = msg->yaw; + pose_d_.x = msg->x; + pose_d_.y = msg->y; + pose_d_.z = msg->z; + pose_d_.roll = msg->roll; + pose_d_.pitch = msg->pitch; + pose_d_.yaw = msg->yaw; } RCLCPP_COMPONENTS_REGISTER_NODE(DPAdaptBacksControllerNode) diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp index f62216ab1..116326a6b 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_utils.cpp @@ -6,13 +6,13 @@ namespace vortex::control { -Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta) { - Eigen::Matrix6d J = eta.as_j_matrix(); +Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::PoseEuler& pose) { + Eigen::Matrix6d J = pose.as_j_matrix(); constexpr double tolerance = 1e-8; if (std::abs(J.determinant()) < tolerance) { - spdlog::error("J(eta) is singular"); + spdlog::error("J is singular"); // Moore-Penrose pseudoinverse in case of near singular matrix, better // result for smaller singular values @@ -22,26 +22,26 @@ Eigen::Matrix6d calculate_J_inv(const vortex::utils::types::Eta& eta) { return J.inverse(); } -Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu) { - return eta.as_rotation_matrix() * +Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist) { + return pose.as_rotation_matrix() * vortex::utils::math::get_skew_symmetric_matrix( - nu.to_vector().tail(3)); + twist.to_vector().tail(3)); } -Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu) { - double cos_phi{std::cos(eta.roll)}; - double sin_phi{std::sin(eta.roll)}; - double cos_theta{std::cos(eta.pitch)}; - double sin_theta{std::sin(eta.pitch)}; +Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist) { + double cos_phi{std::cos(pose.roll)}; + double sin_phi{std::sin(pose.roll)}; + double cos_theta{std::cos(pose.pitch)}; + double sin_theta{std::sin(pose.pitch)}; double tan_theta{sin_theta / cos_theta}; double inv_cos2{1.0 / (cos_theta * cos_theta)}; - Eigen::Vector6d eta_dot = eta.as_j_matrix() * nu.to_vector(); + Eigen::Vector6d pose_dot = pose.as_j_matrix() * twist.to_vector(); - double phi_dot{eta_dot(3)}; - double theta_dot{eta_dot(4)}; + double phi_dot{pose_dot(3)}; + double theta_dot{pose_dot(4)}; Eigen::Matrix3d dt_dphi; dt_dphi << 0.0, cos_phi * tan_theta * phi_dot, @@ -58,10 +58,10 @@ Eigen::Matrix3d calculate_T_dot(const vortex::utils::types::Eta& eta, return dt_dphi + dt_dtheta; } -Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta, - const vortex::utils::types::Nu& nu) { - Eigen::Matrix3d R_dot = calculate_R_dot(eta, nu); - Eigen::Matrix3d T_dot = calculate_T_dot(eta, nu); +Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::PoseEuler& pose, + const vortex::utils::types::Twist& twist) { + Eigen::Matrix3d R_dot = calculate_R_dot(pose, twist); + Eigen::Matrix3d T_dot = calculate_T_dot(pose, twist); Eigen::Matrix6d J_dot = Eigen::Matrix6d::Zero(); J_dot.topLeftCorner<3, 3>() = R_dot; @@ -72,11 +72,11 @@ Eigen::Matrix6d calculate_J_dot(const vortex::utils::types::Eta& eta, Eigen::Matrix6d calculate_coriolis(const double mass, const Eigen::Vector3d& r_b_bg, - const vortex::utils::types::Nu& nu, + const vortex::utils::types::Twist& twist, const Eigen::Matrix3d& I_b) { using vortex::utils::math::get_skew_symmetric_matrix; - Eigen::Vector3d linear_speed = nu.to_vector().head(3); - Eigen::Vector3d angular_speed = nu.to_vector().tail(3); + Eigen::Vector3d linear_speed = twist.to_vector().head(3); + Eigen::Vector3d angular_speed = twist.to_vector().tail(3); Eigen::Matrix6d C; C.topLeftCorner<3, 3>() = mass * vortex::utils::math::get_skew_symmetric_matrix(linear_speed); @@ -93,27 +93,27 @@ Eigen::Matrix6d calculate_coriolis(const double mass, return C; } -Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Nu& nu) { +Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist) { Eigen::Matrix6x12d Y_v; Y_v.setZero(); - Y_v(0, 0) = nu.u; - Y_v(0, 1) = nu.u * std::abs(nu.u); + Y_v(0, 0) = twist.u; + Y_v(0, 1) = twist.u * std::abs(twist.u); - Y_v(1, 2) = nu.v; - Y_v(1, 3) = nu.v * std::abs(nu.v); + Y_v(1, 2) = twist.v; + Y_v(1, 3) = twist.v * std::abs(twist.v); - Y_v(2, 4) = nu.w; - Y_v(2, 5) = nu.w * std::abs(nu.w); + Y_v(2, 4) = twist.w; + Y_v(2, 5) = twist.w * std::abs(twist.w); - Y_v(3, 6) = nu.p; - Y_v(3, 7) = nu.p * std::abs(nu.p); + Y_v(3, 6) = twist.p; + Y_v(3, 7) = twist.p * std::abs(twist.p); - Y_v(4, 8) = nu.q; - Y_v(4, 9) = nu.q * std::abs(nu.q); + Y_v(4, 8) = twist.q; + Y_v(4, 9) = twist.q * std::abs(twist.q); - Y_v(5, 10) = nu.r; - Y_v(5, 11) = nu.r * std::abs(nu.r); + Y_v(5, 10) = twist.r; + Y_v(5, 11) = twist.r * std::abs(twist.r); return Y_v; } diff --git a/control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp b/control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp index 5e179a411..b3adf9d17 100644 --- a/control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp +++ b/control/dp_adapt_backs_controller/test/dp_adapt_backs_controller_tests.cpp @@ -8,8 +8,8 @@ namespace vortex::control { -using vortex::utils::types::Eta; -using vortex::utils::types::Nu; +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Twist; class DPAdaptBacksControllerTests : public ::testing::Test { protected: @@ -29,32 +29,32 @@ class DPAdaptBacksControllerTests : public ::testing::Test { return params; } - Eta generate_current_pose(const double north_pos, - const double east_pos, - const double down_pos, - const double roll_angle, - const double pitch_angle, - const double yaw_angle) { + PoseEuler generate_current_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { return {north_pos, east_pos, down_pos, roll_angle, pitch_angle, yaw_angle}; } - Eta generate_reference_pose(const double north_pos, - const double east_pos, - const double down_pos, - const double roll_angle, - const double pitch_angle, - const double yaw_angle) { + PoseEuler generate_reference_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { return {north_pos, east_pos, down_pos, roll_angle, pitch_angle, yaw_angle}; } - Nu generate_current_velocity(const double surge_vel, - const double sway_vel, - const double heave_vel, - const double roll_rate, - const double pitch_rate, - const double yaw_rate) { + Twist generate_current_velocity(const double surge_vel, + const double sway_vel, + const double heave_vel, + const double roll_rate, + const double pitch_rate, + const double yaw_rate) { return {surge_vel, sway_vel, heave_vel, roll_rate, pitch_rate, yaw_rate}; } @@ -68,11 +68,11 @@ Test that negative north error only (in body) gives positive surge command only. TEST_F(DPAdaptBacksControllerTests, T01_neg_north_error_with_zero_heading_gives_surge_only_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_GT(tau[0], 0.0); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -89,11 +89,11 @@ command and negative sway command. TEST_F( DPAdaptBacksControllerTests, T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; - Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + PoseEuler pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_GT(tau[0], 0.0); EXPECT_LT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -110,11 +110,11 @@ command and positive sway command. TEST_F( DPAdaptBacksControllerTests, T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; - Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + PoseEuler pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_GT(tau[0], 0.0); EXPECT_GT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -131,11 +131,11 @@ command. TEST_F( DPAdaptBacksControllerTests, T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_GT(tau[2], 0.0); @@ -152,11 +152,11 @@ heave and positive surge command. TEST_F( DPAdaptBacksControllerTests, T05_neg_down_error_with_zero_roll_and_neg_pitch_gives_positive_heave_and_positive_surge_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_GT(tau[0], 0.0); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_GT(tau[2], 0.0); @@ -173,11 +173,11 @@ heave and negative surge command. TEST_F( DPAdaptBacksControllerTests, T06_neg_down_error_with_zero_roll_and_pos_pitch_gives_positive_heave_and_negative_surge_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_LT(tau[0], 0.0); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_GT(tau[2], 0.0); @@ -192,11 +192,11 @@ Test that negative east error with zero heading gives a positive sway command. TEST_F(DPAdaptBacksControllerTests, T07_neg_east_error_with_zero_heading_gives_positive_sway_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_GT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -211,11 +211,11 @@ Test that positive east error with zero heading gives a negative sway command. TEST_F(DPAdaptBacksControllerTests, T08_pos_east_error_with_zero_heading_gives_pos_sway_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_LT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -232,11 +232,11 @@ sway command. TEST_F( DPAdaptBacksControllerTests, T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; - Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + PoseEuler pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_GT(tau[0], 0.0); EXPECT_GT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -253,11 +253,11 @@ positive sway command. TEST_F( DPAdaptBacksControllerTests, T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; - Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + PoseEuler pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_LT(tau[0], 0.0); EXPECT_GT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -272,11 +272,11 @@ Test that negative roll error gives positive roll command. TEST_F(DPAdaptBacksControllerTests, T11_neg_roll_error_gives_positive_roll_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -290,11 +290,11 @@ Test that positive roll error gives negative roll command. */ TEST_F(DPAdaptBacksControllerTests, T12_pos_roll_error_gives_neg_roll_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -309,11 +309,11 @@ Test that negative pitch error gives positive pitch command. TEST_F(DPAdaptBacksControllerTests, T13_neg_pitch_error_gives_pos_pitch_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -328,11 +328,11 @@ Test that positive pitch error gives negative pitch command. TEST_F(DPAdaptBacksControllerTests, T14_pos_pitch_error_gives_neg_pitch_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -346,11 +346,11 @@ Test that negative yaw error gives positive yaw command. */ TEST_F(DPAdaptBacksControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -364,11 +364,11 @@ Test that positive yaw error gives negative yaw command. */ TEST_F(DPAdaptBacksControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -384,11 +384,11 @@ Test that positive surge velocity only results in negative surge command TEST_F(DPAdaptBacksControllerTests, T17_pos_surge_vel_gives_negative_surge_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_LT(tau[0], 0.0); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -404,11 +404,11 @@ effect). TEST_F(DPAdaptBacksControllerTests, T18_pos_sway_vel_gives_negative_sway_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_LT(tau[1], 0.0); EXPECT_NEAR(tau[2], 0.0, 0.01); @@ -424,11 +424,11 @@ Test that positive heave velocity only results in negative heave command TEST_F(DPAdaptBacksControllerTests, T19_pos_heave_vel_gives_negative_heave_command) { - Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; - Nu nu{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)}; + PoseEuler pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + PoseEuler pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)}; Eigen::Vector6d tau{ - dp_adapt_backs_controller_.calculate_tau(eta, eta_d, nu)}; + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; EXPECT_NEAR(tau[0], 0.0, 0.01); EXPECT_NEAR(tau[1], 0.0, 0.01); EXPECT_LT(tau[2], 0.0); diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index 614e10e93..a08f415c7 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -169,27 +169,27 @@ Eigen::Vector18d ReferenceFilterNode::fill_reference_state() { x(4) = vortex::utils::math::ssa(pitch); x(5) = vortex::utils::math::ssa(yaw); - vortex::utils::types::Eta eta{current_pose_.pose.pose.position.x, - current_pose_.pose.pose.position.y, - current_pose_.pose.pose.position.z, - roll, - pitch, - yaw}; - Eigen::Matrix6d J = eta.as_j_matrix(); - vortex::utils::types::Nu nu{current_twist_.twist.twist.linear.x, - current_twist_.twist.twist.linear.y, - current_twist_.twist.twist.linear.z, - current_twist_.twist.twist.angular.x, - current_twist_.twist.twist.angular.y, - current_twist_.twist.twist.angular.z}; - Eigen::Vector6d eta_dot = J * nu.to_vector(); - - x(6) = eta_dot(0); - x(7) = eta_dot(1); - x(8) = eta_dot(2); - x(9) = eta_dot(3); - x(10) = eta_dot(4); - x(11) = eta_dot(5); + vortex::utils::types::PoseEuler pose{current_pose_.pose.pose.position.x, + current_pose_.pose.pose.position.y, + current_pose_.pose.pose.position.z, + roll, + pitch, + yaw}; + Eigen::Matrix6d J = pose.as_j_matrix(); + vortex::utils::types::Twist twist{current_twist_.twist.twist.linear.x, + current_twist_.twist.twist.linear.y, + current_twist_.twist.twist.linear.z, + current_twist_.twist.twist.angular.x, + current_twist_.twist.twist.angular.y, + current_twist_.twist.twist.angular.z}; + Eigen::Vector6d pose_dot = J * twist.to_vector(); + + x(6) = pose_dot(0); + x(7) = pose_dot(1); + x(8) = pose_dot(2); + x(9) = pose_dot(3); + x(10) = pose_dot(4); + x(11) = pose_dot(5); return x; } diff --git a/mission/waypoint_manager/src/waypoint_manager_ros.cpp b/mission/waypoint_manager/src/waypoint_manager_ros.cpp index 1d4928470..472cf9378 100644 --- a/mission/waypoint_manager/src/waypoint_manager_ros.cpp +++ b/mission/waypoint_manager/src/waypoint_manager_ros.cpp @@ -85,9 +85,8 @@ WaypointManagerNode::construct_result(bool success) const { result->success = success; result->pose_valid = has_reference_pose_; if (has_reference_pose_) { - result->final_pose = - vortex::utils::ros_conversions::pose_like_to_pose_msg( - latest_ref_feedback_.reference); + result->final_pose = vortex::utils::ros_conversions::to_pose_msg( + latest_ref_feedback_.reference); } return result; } @@ -280,8 +279,7 @@ void WaypointManagerNode::send_reference_filter_goal( return; geometry_msgs::msg::Pose robot_pose = - vortex::utils::ros_conversions::pose_like_to_pose_msg( - fb->reference); + vortex::utils::ros_conversions::to_pose_msg(fb->reference); if (current_index_ < waypoints_.size()) { auto wm_fb = std::make_shared(); From 84ae150e78f8c2eaab3665c58d9efe437016644e Mon Sep 17 00:00:00 2001 From: jorgenfj <144696109+jorgenfj@users.noreply.github.com> Date: Thu, 25 Dec 2025 10:24:27 +0100 Subject: [PATCH 19/87] refactor to new utils ros packages (#648) * refactor to new utils ros packages * added utils dep for velocity controller * fix typo --- control/dp_adapt_backs_controller/CMakeLists.txt | 2 ++ control/dp_adapt_backs_controller/package.xml | 1 + .../src/dp_adapt_backs_controller_ros.cpp | 2 +- control/velocity_controller_lqr/CMakeLists.txt | 2 ++ control/velocity_controller_lqr/package.xml | 1 + .../scripts/velocity_controller_lqr_node.py | 4 ++-- guidance/los_guidance/CMakeLists.txt | 4 ++-- guidance/los_guidance/package.xml | 2 +- guidance/los_guidance/src/los_guidance_ros.cpp | 2 +- guidance/reference_filter_dp/CMakeLists.txt | 2 ++ guidance/reference_filter_dp/package.xml | 1 + guidance/reference_filter_dp/src/reference_filter_ros.cpp | 2 +- mission/waypoint_manager/CMakeLists.txt | 4 ++-- mission/waypoint_manager/package.xml | 2 +- mission/waypoint_manager/src/waypoint_manager_ros.cpp | 2 +- motion/thrust_allocator_auv/CMakeLists.txt | 4 ++-- motion/thrust_allocator_auv/package.xml | 2 +- motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp | 2 +- motion/thruster_interface_auv/CMakeLists.txt | 4 ++-- motion/thruster_interface_auv/package.xml | 2 +- .../thruster_interface_auv/src/thruster_interface_auv_ros.cpp | 2 +- 21 files changed, 29 insertions(+), 20 deletions(-) diff --git a/control/dp_adapt_backs_controller/CMakeLists.txt b/control/dp_adapt_backs_controller/CMakeLists.txt index 09bf9709c..4aae02409 100644 --- a/control/dp_adapt_backs_controller/CMakeLists.txt +++ b/control/dp_adapt_backs_controller/CMakeLists.txt @@ -11,6 +11,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) @@ -41,6 +42,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC spdlog vortex_msgs vortex_utils + vortex_utils_ros ) rclcpp_components_register_node( diff --git a/control/dp_adapt_backs_controller/package.xml b/control/dp_adapt_backs_controller/package.xml index 2d1b6f547..0b708c8b7 100644 --- a/control/dp_adapt_backs_controller/package.xml +++ b/control/dp_adapt_backs_controller/package.xml @@ -16,6 +16,7 @@ tf2 vortex_msgs vortex_utils + vortex_utils_ros ament_cmake diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp index e2cb298e5..18be10a52 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include "dp_adapt_backs_controller/dp_adapt_backs_controller_utils.hpp" #include "dp_adapt_backs_controller/typedefs.hpp" diff --git a/control/velocity_controller_lqr/CMakeLists.txt b/control/velocity_controller_lqr/CMakeLists.txt index 05f937270..791fdefcf 100644 --- a/control/velocity_controller_lqr/CMakeLists.txt +++ b/control/velocity_controller_lqr/CMakeLists.txt @@ -6,6 +6,8 @@ find_package(rclpy REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) install(DIRECTORY launch diff --git a/control/velocity_controller_lqr/package.xml b/control/velocity_controller_lqr/package.xml index 8f2935a41..9e389e70f 100644 --- a/control/velocity_controller_lqr/package.xml +++ b/control/velocity_controller_lqr/package.xml @@ -14,6 +14,7 @@ nav_msgs vortex_msgs vortex_utils + vortex_utils_ros python-control-pip std_msgs diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index 1d9f9dba4..0ae504430 100755 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -16,8 +16,8 @@ ) from vortex_msgs.msg import LOSGuidance from vortex_utils.python_utils import State -from vortex_utils.qos_profiles import reliable_profile, sensor_data_profile -from vortex_utils.ros_converter import pose_from_ros, twist_from_ros +from vortex_utils_ros.qos_profiles import reliable_profile, sensor_data_profile +from vortex_utils_ros.ros_converter import pose_from_ros, twist_from_ros class LinearQuadraticRegulator(Node): diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 443e7cb51..6a51f5780 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) @@ -36,7 +36,7 @@ ament_target_dependencies(${LIB_NAME} rclcpp_action geometry_msgs vortex_msgs - vortex_utils + vortex_utils_ros Eigen3 spdlog fmt diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 9ee0850b1..5ff6218fe 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -13,7 +13,7 @@ rclcpp_action geometry_msgs vortex_msgs - vortex_utils + vortex_utils_ros eigen diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index d365510a3..6447ecc51 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include #include "los_guidance/lib/types.hpp" const auto start_message = R"( diff --git a/guidance/reference_filter_dp/CMakeLists.txt b/guidance/reference_filter_dp/CMakeLists.txt index e2c22f522..d7754462a 100644 --- a/guidance/reference_filter_dp/CMakeLists.txt +++ b/guidance/reference_filter_dp/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(Eigen3 REQUIRED) @@ -38,6 +39,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC Eigen3 vortex_msgs vortex_utils + vortex_utils_ros spdlog fmt ) diff --git a/guidance/reference_filter_dp/package.xml b/guidance/reference_filter_dp/package.xml index 659e742b2..40793be91 100644 --- a/guidance/reference_filter_dp/package.xml +++ b/guidance/reference_filter_dp/package.xml @@ -14,6 +14,7 @@ geometry_msgs vortex_msgs vortex_utils + vortex_utils_ros nav_msgs eigen diff --git a/guidance/reference_filter_dp/src/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/reference_filter_ros.cpp index a08f415c7..e7b1c231d 100644 --- a/guidance/reference_filter_dp/src/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/reference_filter_ros.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include const auto start_message = R"( diff --git a/mission/waypoint_manager/CMakeLists.txt b/mission/waypoint_manager/CMakeLists.txt index 0a76ae510..451de3132 100644 --- a/mission/waypoint_manager/CMakeLists.txt +++ b/mission/waypoint_manager/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) @@ -31,7 +31,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp_components rclcpp_action vortex_msgs - vortex_utils + vortex_utils_ros tf2_geometry_msgs spdlog fmt diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml index db3feabde..be6d9148b 100644 --- a/mission/waypoint_manager/package.xml +++ b/mission/waypoint_manager/package.xml @@ -13,7 +13,7 @@ rclcpp_action geometry_msgs vortex_msgs - vortex_utils + vortex_utils_ros tf2_geometry_msgs launch_testing diff --git a/mission/waypoint_manager/src/waypoint_manager_ros.cpp b/mission/waypoint_manager/src/waypoint_manager_ros.cpp index 472cf9378..93a9da5d2 100644 --- a/mission/waypoint_manager/src/waypoint_manager_ros.cpp +++ b/mission/waypoint_manager/src/waypoint_manager_ros.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include namespace vortex::mission { diff --git a/motion/thrust_allocator_auv/CMakeLists.txt b/motion/thrust_allocator_auv/CMakeLists.txt index 1b8cd0b99..3063537a0 100644 --- a/motion/thrust_allocator_auv/CMakeLists.txt +++ b/motion/thrust_allocator_auv/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) @@ -33,7 +33,7 @@ add_library(${LIB_NAME} SHARED geometry_msgs Eigen3 vortex_msgs - vortex_utils + vortex_utils_ros fmt spdlog ) diff --git a/motion/thrust_allocator_auv/package.xml b/motion/thrust_allocator_auv/package.xml index feb767aaf..f5b492348 100644 --- a/motion/thrust_allocator_auv/package.xml +++ b/motion/thrust_allocator_auv/package.xml @@ -10,7 +10,7 @@ rclcpp geometry_msgs vortex_msgs - vortex_utils + vortex_utils_ros eigen ament_cmake diff --git a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp index 3785b718f..eba02da21 100644 --- a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp +++ b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include "thrust_allocator_auv/pseudoinverse_allocator.hpp" #include "thrust_allocator_auv/thrust_allocator_utils.hpp" diff --git a/motion/thruster_interface_auv/CMakeLists.txt b/motion/thruster_interface_auv/CMakeLists.txt index 0ee76eb4a..9813202b5 100644 --- a/motion/thruster_interface_auv/CMakeLists.txt +++ b/motion/thruster_interface_auv/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) @@ -27,7 +27,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp_components std_msgs vortex_msgs - vortex_utils + vortex_utils_ros spdlog fmt ) diff --git a/motion/thruster_interface_auv/package.xml b/motion/thruster_interface_auv/package.xml index 0b1860e4f..d4a05278a 100644 --- a/motion/thruster_interface_auv/package.xml +++ b/motion/thruster_interface_auv/package.xml @@ -12,7 +12,7 @@ rclcpp std_msgs vortex_msgs - vortex_utils + vortex_utils_ros ament_cmake diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 8681fa025..603b2d46e 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include const auto start_message = R"( _____ _ _ ___ _ __ From cb4156b15a5b7a2bf537ab1d267ce66c81dde215 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 6 Jan 2026 12:32:51 +0100 Subject: [PATCH 20/87] Fix: Issues from feedback on PR draft --- guidance/los_guidance/CMakeLists.txt | 8 ++ .../los_guidance/config/guidance_params.yaml | 2 + .../include/los_guidance/lib/adaptive_los.hpp | 2 +- .../include/los_guidance/lib/integral_los.hpp | 2 +- .../los_guidance/lib/proportional_los.hpp | 2 +- .../include/los_guidance/los_guidance_ros.hpp | 13 ++- .../los_guidance/src/los_guidance_ros.cpp | 85 ++++++++----------- 7 files changed, 56 insertions(+), 58 deletions(-) diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 6a51f5780..00182bc1c 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -46,6 +46,14 @@ add_executable(los_guidance_node src/los_guidance_node.cpp ) +ament_target_dependencies(los_guidance_node + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + vortex_utils_ros +) + target_link_libraries(los_guidance_node ${LIB_NAME} yaml-cpp diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 64f922a27..0639512ed 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -18,6 +18,8 @@ integer_los: k_i_h: 0.1 k_i_v: 0.1 + + common: active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive u_desired: 0.3 diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 9b87a68cd..4f8bc189e 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -41,7 +41,7 @@ class AdaptiveLOSGuidance { double pi_h_{}; double pi_v_{}; double beta_c_hat_{}; - double alpha_c_hat_{}; + double alpha_c_hat_{}; }; // namespace vortex::guidance::los diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index dbb437241..a8e501709 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -16,7 +16,7 @@ struct IntegralLosParams { double k_i_h{}; double k_i_v{}; double time_step{}; -}; +}; class IntegralLOSGuidance { public: diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index f5ff3d6d9..2a5ea00a1 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -12,7 +12,7 @@ struct ProportionalLosParams { double lookahead_distance_v{}; double k_p_h{}; double k_p_v{}; - double time_step{}; + double time_step{}; }; class ProportionalLOSGuidance { diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 1a7bb74a2..68ef71e49 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -5,7 +5,6 @@ #include #include #include -#include #include #include #include @@ -15,8 +14,8 @@ #include #include "los_guidance/lib/adaptive_los.hpp" #include "los_guidance/lib/integral_los.hpp" -#include "los_guidance/lib/proportional_los.hpp" -#include "los_guidance/lib/types.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { @@ -122,10 +121,10 @@ class LosGuidanceNode : public rclcpp::Node { double goal_reached_tol_{}; - std::unique_ptr m_adaptive_los{}; - std::unique_ptr m_integral_los{}; - std::unique_ptr m_proportional_los{}; - types::ActiveLosMethod m_method{}; + std::unique_ptr adaptive_los_{}; + std::unique_ptr integral_los_{}; + std::unique_ptr proportional_los_{}; + types::ActiveLosMethod method_{}; }; } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 6447ecc51..04facf075 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -97,66 +97,57 @@ void LosGuidanceNode::set_service_server() { void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { auto adaptive_los_config = config["adaptive_los"]; - spdlog::info("A0"); auto params = AdaptiveLosParams{}; params.lookahead_distance_h = adaptive_los_config["lookahead_distance_h"].as(); - spdlog::info("A1"); params.lookahead_distance_v = adaptive_los_config["lookahead_distance_v"].as(); - spdlog::info("A2"); - params.gamma_h = adaptive_los_config["gamma_h"].as(); - spdlog::info("A3"); - params.gamma_v = adaptive_los_config["gamma_v"].as(); - spdlog::info("A4"); - params.time_step = static_cast(time_step_.count()) / 1000.0; - spdlog::info("A5"); - - m_adaptive_los = std::make_unique(params); + params.gamma_h = + adaptive_los_config["gamma_h"].as(); + params.gamma_v = + adaptive_los_config["gamma_v"].as(); + params.time_step = + static_cast(time_step_.count()) / 1000.0; + + adaptive_los_ = std::make_unique(params); } void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { auto proportional_los_config = config["prop_los"]; - spdlog::info("P0"); auto params = ProportionalLosParams{}; params.lookahead_distance_h = proportional_los_config["lookahead_distance_h"].as(); - spdlog::info("P1"); + params.lookahead_distance_v = proportional_los_config["lookahead_distance_v"].as(); - spdlog::info("P2"); - params.k_p_h = proportional_los_config["k_p_h"].as(); - spdlog::info("P3"); - params.k_p_v = proportional_los_config["k_p_v"].as(); - spdlog::info("P4"); - params.time_step = static_cast(time_step_.count()) / 1000.0; - spdlog::info("P5"); - - m_proportional_los = std::make_unique(params); + params.k_p_h = + proportional_los_config["k_p_h"].as(); + params.k_p_v = + proportional_los_config["k_p_v"].as(); + params.time_step = + static_cast(time_step_.count()) / 1000.0; + + proportional_los_ = std::make_unique(params); } void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; - spdlog::info("I0"); auto params = IntegralLosParams{}; params.lookahead_distance_h = integral_los_config["lookahead_distance_h"].as(); - spdlog::info("I1"); params.lookahead_distance_v = integral_los_config["lookahead_distance_v"].as(); - spdlog::info("I2"); - params.k_p_h = integral_los_config["k_p_h"].as(); - spdlog::info("I3"); - params.k_p_v = integral_los_config["k_p_v"].as(); - spdlog::info("I4"); - params.k_i_h = integral_los_config["k_i_h"].as(); - spdlog::info("I5"); - params.k_i_v = integral_los_config["k_i_v"].as(); - spdlog::info("I6"); - params.time_step = static_cast(time_step_.count()) / 1000.0; - spdlog::info("I7"); - - m_integral_los = std::make_unique(params); + params.k_p_h = + integral_los_config["k_p_h"].as(); + params.k_p_v = + integral_los_config["k_p_v"].as(); + params.k_i_h = + integral_los_config["k_i_h"].as(); + params.k_i_v = + integral_los_config["k_i_v"].as(); + params.time_step = + static_cast(time_step_.count()) / 1000.0; + integral_los_ = std::make_unique(params); } void LosGuidanceNode::waypoint_callback( @@ -209,8 +200,8 @@ void LosGuidanceNode::handle_accepted( void LosGuidanceNode::set_los_mode( const std::shared_ptr request, std::shared_ptr response) { - m_method = static_cast(request->mode); - spdlog::info("LOS mode set to {}", static_cast(m_method)); + method_ = static_cast(request->mode); + spdlog::info("LOS mode set to {}", static_cast(method_)); response->success = true; } vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( @@ -229,13 +220,11 @@ YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { } void LosGuidanceNode::parse_common_config(YAML::Node common_config) { + std::lock_guard lock(mutex_); u_desired_ = common_config["u_desired"].as(); - spdlog::info("C1"); goal_reached_tol_ = common_config["goal_reached_tol"].as(); - spdlog::info("C2"); - m_method = static_cast( + method_ = static_cast( common_config["active_los_method"].as()); - spdlog::info("C3"); } void LosGuidanceNode::execute( @@ -281,15 +270,15 @@ void LosGuidanceNode::execute( types::Outputs outputs; - switch (m_method) { + switch (method_) { case types::ActiveLosMethod::ADAPTIVE: - outputs = m_adaptive_los->calculate_outputs(path_inputs_); + outputs = adaptive_los_->calculate_outputs(path_inputs_); break; case types::ActiveLosMethod::PROPORTIONAL: - outputs = m_proportional_los->calculate_outputs(path_inputs_); + outputs = proportional_los_->calculate_outputs(path_inputs_); break; case types::ActiveLosMethod::INTEGRAL: - outputs = m_integral_los->calculate_outputs(path_inputs_); + outputs = integral_los_->calculate_outputs(path_inputs_); break; default: spdlog::error("Invalid LOS method selected"); @@ -318,4 +307,4 @@ void LosGuidanceNode::execute( } } -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los From 8c61007ad8fc3a78b77e654672ec523cabb31d98 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 7 Jan 2026 11:31:22 +0100 Subject: [PATCH 21/87] Start vector Feild LOS & Clean code --- .../los_guidance/config/guidance_params.yaml | 5 ++ .../include/los_guidance/lib/adaptive_los.hpp | 2 +- .../include/los_guidance/lib/integral_los.hpp | 2 +- .../los_guidance/lib/vector_field_los.hpp | 41 ++++++++++++++++ .../los_guidance/src/lib/adaptive_los.cpp | 32 ++++++------ .../los_guidance/src/lib/integral_los.cpp | 16 +++--- .../los_guidance/src/lib/proportional_los.cpp | 27 ++++++---- .../los_guidance/src/lib/vector_field_los.cpp | 49 +++++++++++++++++++ 8 files changed, 139 insertions(+), 35 deletions(-) create mode 100644 guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp create mode 100644 guidance/los_guidance/src/lib/vector_field_los.cpp diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 0639512ed..5e56f6908 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -18,6 +18,11 @@ integer_los: k_i_h: 0.1 k_i_v: 0.1 +vector_field_los: + max_approach_angle_h: 1.0 # rad + max_approach_angle_v: 0.6 # rad + k_p_h: 0.5 + k_p_v: 0.5 common: diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 4f8bc189e..d82684469 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -35,7 +35,7 @@ class AdaptiveLOSGuidance { void update_adaptive_estimates( const types::CrossTrackError& cross_track_error); - AdaptiveLosParams m_params{}; + AdaptiveLosParams params_{}; Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); double pi_h_{}; diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index a8e501709..4af1b60ac 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -8,7 +8,7 @@ namespace vortex::guidance::los { -struct IntegralLosParams { +struct IntegralLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; double k_p_h{}; diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp new file mode 100644 index 000000000..a0a171089 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -0,0 +1,41 @@ +#ifndef VECTOR_FIELD_LOS_GUIDANCE_HPP +#define VECTOR_FIELD_LOS_GUIDANCE_HPP + +#include +#include +#include +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +struct VectorFieldLosParams { + double max_approach_angle_h{}; + double max_approach_angle_v{}; + double k_p_h{}; + double k_p_v{}; + double time_step{}; +}; + +class VectorFieldLOSGuidance { + public: + VectorFieldLOSGuidance(const VectorFieldLosParams& params); + ~VectorFieldLOSGuidance() = default; + + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs) const; + + VectorFieldLosParams m_params{}; + + double pi_h_{0.0}; + double pi_v_{0.0}; + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; + +} // namespace vortex::guidance::los + +#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 319ce376c..4a34ccb79 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -4,7 +4,7 @@ namespace vortex::guidance::los { AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) - : m_params{params} {} + : params_{params} {} void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; @@ -30,33 +30,33 @@ const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( } void AdaptiveLOSGuidance::update_adaptive_estimates( - const types::CrossTrackError& e) { - const double denom_h = std::sqrt(m_params.lookahead_distance_h * - m_params.lookahead_distance_h + - e.y_e * e.y_e); - const double denom_v = std::sqrt(m_params.lookahead_distance_v * - m_params.lookahead_distance_v + - e.z_e * e.z_e); + const types::CrossTrackError& cross_track_error) { + const double denom_h = std::sqrt(params_.lookahead_distance_h * + params_.lookahead_distance_h + + cross_track_error.y_e * cross_track_error.y_e); + const double denom_v = std::sqrt(params_.lookahead_distance_v * + params_.lookahead_distance_v + + cross_track_error.z_e * cross_track_error.z_e); const double beta_dot = - m_params.gamma_h * (m_params.lookahead_distance_h / denom_h) * e.y_e; + params_.gamma_h * (params_.lookahead_distance_h / denom_h) * cross_track_error.y_e; const double alpha_dot = - m_params.gamma_v * (m_params.lookahead_distance_v / denom_v) * e.z_e; + params_.gamma_v * (params_.lookahead_distance_v / denom_v) * cross_track_error.z_e; - beta_c_hat_ += beta_dot * m_params.time_step; - alpha_c_hat_ += alpha_dot * m_params.time_step; + beta_c_hat_ += beta_dot * params_.time_step; + alpha_c_hat_ += alpha_dot * params_.time_step; } types::Outputs AdaptiveLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); - update_adaptive_estimates(e); + const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + update_adaptive_estimates(cross_track_error); const double psi_d = - pi_h_ - beta_c_hat_ - std::atan(e.y_e / m_params.lookahead_distance_h); + pi_h_ - beta_c_hat_ - std::atan(cross_track_error.y_e / params_.lookahead_distance_h); const double theta_d = - pi_v_ + alpha_c_hat_ + std::atan(e.z_e / m_params.lookahead_distance_v); + pi_v_ + alpha_c_hat_ + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); return types::Outputs{psi_d, theta_d}; } diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index 0ca1a3e2b..b1a4e3962 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -13,30 +13,30 @@ void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { difference.y * difference.y)); rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); } types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * + const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; - return types::CrossTrackError::from_vector(e_perp); + return types::CrossTrackError::from_vector(path_frame_error); } types::Outputs IntegralLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); + const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); - int_h += e.y_e * m_params.time_step; - int_v += e.z_e * m_params.time_step; + int_h += cross_track_error.y_e * m_params.time_step; + int_v += cross_track_error.z_e * m_params.time_step; - const double u_h = m_params.k_p_h * e.y_e + m_params.k_i_h * int_h; - const double u_v = m_params.k_p_v * e.z_e + m_params.k_i_v * int_v; + const double u_h = m_params.k_p_h * cross_track_error.y_e + m_params.k_i_h * int_h; + const double u_v = m_params.k_p_v * cross_track_error.z_e + m_params.k_i_v * int_v; const double psi_d = pi_h_ - std::atan(u_h); const double theta_d = pi_v_ + std::atan(u_v); diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index 40a739e04..0bb65ba01 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -3,8 +3,17 @@ namespace vortex::guidance::los { ProportionalLOSGuidance::ProportionalLOSGuidance( - const ProportionalLosParams& params) - : m_params{params} {} + const ProportionalLosParams& params) : m_params{params} { + + if (m_params.lookahead_distance_h <= 0.0) { + m_params.lookahead_distance_h = 1e-9; + } + + if (m_params.lookahead_distance_v <= 0.0) { + m_params.lookahead_distance_v = 1e-9; + } + + } void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; @@ -21,23 +30,23 @@ types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) const { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d e_perp = rotation_y_.toRotationMatrix().transpose() * + const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; - return types::CrossTrackError::from_vector(e_perp); + return types::CrossTrackError::from_vector(path_frame_error); } types::Outputs ProportionalLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError e = calculate_crosstrack_error(inputs); + const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); - const double k_p_h = 1.0 / std::max(m_params.lookahead_distance_h, 1e-9); - const double k_p_v = 1.0 / std::max(m_params.lookahead_distance_v, 1e-9); + const double k_p_h = 1.0 / m_params.lookahead_distance_h; + const double k_p_v = 1.0 / m_params.lookahead_distance_v; - const double psi_d = pi_h_ - std::atan(k_p_h * e.y_e); - const double theta_d = pi_v_ + std::atan(k_p_v * e.z_e); + const double psi_d = pi_h_ - std::atan(k_p_h * cross_track_error.y_e); + const double theta_d = pi_v_ + std::atan(k_p_v * cross_track_error.z_e); return types::Outputs{psi_d, theta_d}; } diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp new file mode 100644 index 000000000..e008bfe57 --- /dev/null +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -0,0 +1,49 @@ +#include + +namespace vortex::guidance::los { + +VectorFieldLOSGuidance::VectorFieldLOSGuidance(const VectorFieldLosParams& params) + : m_params{params} {} + +void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +types::CrossTrackError VectorFieldLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) const { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + + const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * + diff_vec; + + return types::CrossTrackError::from_vector(path_frame_error); +} + +types::Outputs VectorFieldLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + + const double approach_h = + m_params.max_approach_angle_h * (2.0 / M_PI) * std::atan(m_params.k_p_h * cross_track_error.y_e); + + const double approach_v = + m_params.max_approach_angle_v * (2.0 / M_PI) * std::atan(m_params.k_p_v * cross_track_error.z_e); + + const double psi_d = pi_h_ - approach_h; + + const double theta_d = pi_v_ + approach_v; + + return types::Outputs{psi_d, theta_d}; +} + +} // namespace vortex::guidance::los From 294ab778e135f9c7e441f6da48027e9bd24879d7 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 7 Jan 2026 12:37:40 +0100 Subject: [PATCH 22/87] Adda vector field LOS & Test --- guidance/los_guidance/CMakeLists.txt | 2 +- .../include/los_guidance/lib/adaptive_los.hpp | 2 +- .../include/los_guidance/lib/types.hpp | 3 +- .../los_guidance/lib/vector_field_los.hpp | 2 +- .../include/los_guidance/los_guidance.hpp | 77 ------------ .../include/los_guidance/los_guidance_ros.hpp | 5 + .../los_guidance/src/lib/vector_field_los.cpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 21 ++++ guidance/los_guidance/test/CMakeLists.txt | 1 + .../test/vector_field_los_test.cpp | 111 ++++++++++++++++++ 10 files changed, 144 insertions(+), 82 deletions(-) delete mode 100644 guidance/los_guidance/include/los_guidance/los_guidance.hpp create mode 100644 guidance/los_guidance/test/vector_field_los_test.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 00182bc1c..9d8fca42b 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -29,6 +29,7 @@ add_library(${LIB_NAME} SHARED src/lib/integral_los.cpp src/lib/adaptive_los.cpp src/los_guidance_ros.cpp + src/lib/vector_field_los.cpp ) ament_target_dependencies(${LIB_NAME} @@ -84,7 +85,6 @@ install(DIRECTORY ) if(BUILD_TESTING) - include(CTest) add_subdirectory(test) endif() diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index d82684469..f874db35f 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -18,7 +18,7 @@ struct AdaptiveLosParams { double lookahead_distance_v{}; double gamma_h{}; double gamma_v{}; - double time_step{}; + double time_step{}; }; class AdaptiveLOSGuidance { diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index 5207855cc..ca2492d10 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -49,7 +49,8 @@ struct Inputs { enum class ActiveLosMethod { PROPORTIONAL, // 0 INTEGRAL, // 1 - ADAPTIVE // 2 + ADAPTIVE, // 2 + VECTOR_FIELD // 3 }; } // namespace vortex::guidance::los::types diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index a0a171089..9d3210ffd 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { diff --git a/guidance/los_guidance/include/los_guidance/los_guidance.hpp b/guidance/los_guidance/include/los_guidance/los_guidance.hpp deleted file mode 100644 index 71ddfdb11..000000000 --- a/guidance/los_guidance/include/los_guidance/los_guidance.hpp +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef LOS_GUIDANCE__LOS_GUIDANCE_HPP_ -#define LOS_GUIDANCE__LOS_GUIDANCE_HPP_ - -#include - -namespace vortex::guidance { - -namespace LOS { - -struct Point { - double x{}; - double y{}; - double z{}; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } -}; - -struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } -}; - -struct Params { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; -}; - -} // namespace LOS - -/** - * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 - * in "Fossen 2024 Lecture on 2D and 3D path-following control". - */ -class AdaptiveLOSGuidance { - public: - explicit AdaptiveLOSGuidance(const LOS::Params& params); - ~AdaptiveLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - - double calculate_psi_d(const double& y_e) const; - - double calculate_theta_d(const double& z_e) const; - - void update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error); - - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; -}; - -} // namespace vortex::guidance - -#endif // LOS_GUIDANCE__LOS_GUIDANCE_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 68ef71e49..fdeb06a9d 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -15,6 +15,7 @@ #include "los_guidance/lib/adaptive_los.hpp" #include "los_guidance/lib/integral_los.hpp" #include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/vector_field_los.hpp" #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { @@ -42,6 +43,9 @@ class LosGuidanceNode : public rclcpp::Node { // @brief Set the integral LOS guidance parameters void set_integral_los_guidance(YAML::Node config); + // @brief Set the vector field LOS guidance parameters + void set_vector_field_guidance(YAML::Node config); + // @brief Callback for the waypoint topic // @param msg The reference message void waypoint_callback( @@ -124,6 +128,7 @@ class LosGuidanceNode : public rclcpp::Node { std::unique_ptr adaptive_los_{}; std::unique_ptr integral_los_{}; std::unique_ptr proportional_los_{}; + std::unique_ptr vector_field_los_{}; types::ActiveLosMethod method_{}; }; diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp index e008bfe57..52b4f1644 100644 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -21,7 +21,7 @@ types::CrossTrackError VectorFieldLOSGuidance::calculate_crosstrack_error( const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * + const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * rotation_z_.toRotationMatrix().transpose() * diff_vec; diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 04facf075..220009fc2 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -34,6 +34,7 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { set_adaptive_los_guidance(config); set_proportional_los_guidance(config); set_integral_los_guidance(config); + set_vector_field_guidance(config); spdlog::info(start_message); } @@ -150,6 +151,23 @@ void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { integral_los_ = std::make_unique(params); } +void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { + auto vector_field_config = config["vector_field_los"]; + auto params = VectorFieldLosParams{}; + params.max_approach_angle_h = + vector_field_config["max_approach_angle_h"].as(); + params.max_approach_angle_v = + vector_field_config["max_approach_angle_v"].as(); + params.k_p_h = + vector_field_config["k_p_h"].as(); + params.k_p_v = + vector_field_config["k_p_v"].as(); + params.time_step = + static_cast(time_step_.count()) / 1000.0; + + vector_field_los_ = std::make_unique(params); +} + void LosGuidanceNode::waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { path_inputs_.prev_point = path_inputs_.current_position; @@ -280,6 +298,9 @@ void LosGuidanceNode::execute( case types::ActiveLosMethod::INTEGRAL: outputs = integral_los_->calculate_outputs(path_inputs_); break; + case types::ActiveLosMethod::VECTOR_FIELD: + outputs = vector_field_los_->calculate_outputs(path_inputs_); + break; default: spdlog::error("Invalid LOS method selected"); result->success = false; diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index 8f5842651..c0cdc1ae7 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -10,6 +10,7 @@ add_executable( adaptive_los_test.cpp proportional_los_test.cpp integral_los_test.cpp + vector_field_los_test.cpp ) diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp new file mode 100644 index 000000000..527b33694 --- /dev/null +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -0,0 +1,111 @@ +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/vector_field_los.hpp" +#include + +namespace vortex::guidance::los { + +class VectorFieldLosTest : public ::testing::Test { + protected: + VectorFieldLosTest() : Vflos_{get_params()} {} + + VectorFieldLosParams get_params() { + VectorFieldLosParams params; + params.max_approach_angle_h = 30.0 * M_PI / 180.0; // 30 degrees in rad + params.max_approach_angle_v = 20.0 * M_PI / 180.0; // 20 degrees in rad + params.k_p_h = 0.1; // needs tuning + params.k_p_v = 0.1; // needs tuning + params.time_step = 0.01; + return params; + } + + VectorFieldLOSGuidance Vflos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(VectorFieldLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(VectorFieldLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track +TEST_F(VectorFieldLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los From edc7185445a2eb55a346202851613caa192212e3 Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 8 Jan 2026 14:07:12 +0100 Subject: [PATCH 23/87] Start quto-euler for graphing --- .../los_guidance/config/guidance_params.yaml | 4 +- .../include/los_guidance/los_guidance_ros.hpp | 8 +++ .../launch/guidance_test.launch.py | 50 +++++++++++++++++++ .../los_guidance/src/lib/adaptive_los.cpp | 2 + .../los_guidance/src/los_guidance_ros.cpp | 11 ++++ 5 files changed, 74 insertions(+), 1 deletion(-) create mode 100644 guidance/los_guidance/launch/guidance_test.launch.py diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 5e56f6908..ccdcd1aad 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -24,9 +24,11 @@ vector_field_los: k_p_h: 0.5 k_p_v: 0.5 - common: active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive u_desired: 0.3 goal_reached_tol: 1.0 +debug: + enable_debug: false + debug_topic_name: /los_guidance_debug \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index fdeb06a9d..2a4b528b4 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -17,6 +17,8 @@ #include "los_guidance/lib/proportional_los.hpp" #include "los_guidance/lib/vector_field_los.hpp" #include "los_guidance/lib/types.hpp" +#include +//#include namespace vortex::guidance::los { @@ -99,6 +101,12 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr reference_pub_; + bool enable_debug_; + + std::string debug_topic_name_; + + rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Subscription::SharedPtr waypoint_sub_; diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py new file mode 100644 index 000000000..f982f669f --- /dev/null +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -0,0 +1,50 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + stonefish_dir = get_package_share_directory('stonefish_sim') + los_guidance_dir = get_package_share_directory('los_guidance') + velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') + + stonefish_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') + ), + launch_arguments={ + 'scenario': 'tacc', + 'rendering': 'false', + }.items(), + ) + + los_guidance_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(los_guidance_dir, 'launch', 'los_guidance.launch.py') + ) + ) + + velocity_controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py') + ) + ) + + orca_sim = TimerAction( + period=12.0, + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'orca_sim.launch.py') + ) + ) + ] + ) + + return LaunchDescription([ + stonefish_sim, + los_guidance_launch, + velocity_controller_launch, + orca_sim, + ]) \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 4a34ccb79..f35d9a053 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -61,4 +61,6 @@ types::Outputs AdaptiveLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } + + } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 220009fc2..34cb0b631 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -43,6 +43,8 @@ void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.waypoint"); + this->declare_parameter("debug.debug_topic_name"); + this->declare_parameter("debug.enable_debug"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string guidance_topic = @@ -50,6 +52,9 @@ void LosGuidanceNode::set_subscribers_and_publisher() { std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); + debug_topic_name_ = this->get_parameter("debug.debug_topic_name").as_string(); + enable_debug_ = this->get_parameter("debug.enable_debug").as_bool(); + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); reference_pub_ = this->create_publisher( @@ -65,6 +70,12 @@ void LosGuidanceNode::set_subscribers_and_publisher() { pose_topic, qos_sensor_data, std::bind(&LosGuidanceNode::pose_callback, this, std::placeholders::_1)); + + + if (enable_debug_) { + debug_pub_ = this->create_publisher( + debug_topic_name_, qos_sensor_data); + } } void LosGuidanceNode::set_action_server() { From 763f784042a3732c89309ad5b6ffc1cfd21573b8 Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 15 Jan 2026 11:58:55 +0100 Subject: [PATCH 24/87] Fix emailaddress error for git --- guidance/los_guidance/CMakeLists.txt | 2 + .../los_guidance/config/guidance_params.yaml | 6 +- .../include/los_guidance/los_guidance_ros.hpp | 18 +++-- guidance/los_guidance/package.xml | 1 + .../los_guidance/src/los_guidance_ros.cpp | 68 ++++++++++++++++--- 5 files changed, 78 insertions(+), 17 deletions(-) diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 9d8fca42b..1dbdbe0d9 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(rclcpp_action REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) @@ -36,6 +37,7 @@ ament_target_dependencies(${LIB_NAME} rclcpp rclcpp_action geometry_msgs + nav_msgs vortex_msgs vortex_utils_ros Eigen3 diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index ccdcd1aad..5d5d0b754 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -25,10 +25,10 @@ vector_field_los: k_p_v: 0.5 common: - active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive + active_los_method: 0 # 0: Proportional, 1: Integral, 2: Adaptive u_desired: 0.3 goal_reached_tol: 1.0 debug: - enable_debug: false - debug_topic_name: /los_guidance_debug \ No newline at end of file + enable_debug: true + debug_topic_name: "/los_guidance_debug" \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 2a4b528b4..5f79d75b5 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -18,7 +19,8 @@ #include "los_guidance/lib/vector_field_los.hpp" #include "los_guidance/lib/types.hpp" #include -//#include +#include +#include namespace vortex::guidance::los { @@ -28,7 +30,7 @@ class LosGuidanceNode : public rclcpp::Node { private: // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); + void set_subscribers_and_publisher(YAML::Node config); // @brief Set the action server void set_action_server(); @@ -58,6 +60,9 @@ class LosGuidanceNode : public rclcpp::Node { void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg); + // @brief Handle the goal request // @param uuid The goal UUID // @param goal The goal message @@ -88,6 +93,9 @@ class LosGuidanceNode : public rclcpp::Node { const std::shared_ptr request, std::shared_ptr response); + void publish_state_debug(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose); + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); YAML::Node get_los_config(std::string yaml_file_path); @@ -105,11 +113,13 @@ class LosGuidanceNode : public rclcpp::Node { std::string debug_topic_name_; - rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Publisher::SharedPtr debug_pub_; rclcpp::Subscription::SharedPtr waypoint_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; @@ -141,5 +151,5 @@ class LosGuidanceNode : public rclcpp::Node { }; } // namespace vortex::guidance::los - + #endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 5ff6218fe..8fef0da18 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -13,6 +13,7 @@ rclcpp_action geometry_msgs vortex_msgs + nav_msgs vortex_utils_ros eigen diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 34cb0b631..320eeea7d 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,7 +1,7 @@ #include "los_guidance/los_guidance_ros.hpp" +#include #include #include -#include #include #include "los_guidance/lib/types.hpp" @@ -10,7 +10,7 @@ const auto start_message = R"( | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ | | | | | \___ \ | | _| | | | |/ _` |/ _` | '_ \ / __/ _ \ | |__| |_| |___) | | |_| | |_| | | (_| | (_| | | | | (_| __/ - |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| + |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| )"; @@ -28,7 +28,7 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { YAML::Node config = get_los_config(yaml_path); parse_common_config(config["common"]); - set_subscribers_and_publisher(); + set_subscribers_and_publisher(config); set_action_server(); set_service_server(); set_adaptive_los_guidance(config); @@ -39,21 +39,20 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { spdlog::info(start_message); } -void LosGuidanceNode::set_subscribers_and_publisher() { +void LosGuidanceNode::set_subscribers_and_publisher(YAML::Node config) { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.waypoint"); - this->declare_parameter("debug.debug_topic_name"); - this->declare_parameter("debug.enable_debug"); + this->declare_parameter("nav_topics.odometry"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string guidance_topic = this->get_parameter("topics.guidance.los").as_string(); std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); - - debug_topic_name_ = this->get_parameter("debug.debug_topic_name").as_string(); - enable_debug_ = this->get_parameter("debug.enable_debug").as_bool(); + + std::string odom_topic = + this->get_parameter("nav_topics.odometry").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); @@ -71,9 +70,17 @@ void LosGuidanceNode::set_subscribers_and_publisher() { std::bind(&LosGuidanceNode::pose_callback, this, std::placeholders::_1)); + odom_sub_ = this->create_subscription( + odom_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::odom_callback, this, + std::placeholders::_1)); + + auto debug_config = config["debug"]; + enable_debug_ = debug_config["enable_debug"].as(); + debug_topic_name_ = debug_config["debug_topic_name"].as(); if (enable_debug_) { - debug_pub_ = this->create_publisher( + debug_pub_ = this->create_publisher( debug_topic_name_, qos_sensor_data); } } @@ -192,6 +199,8 @@ void LosGuidanceNode::pose_callback( current_pose) { path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); + + publish_state_debug(current_pose); } rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( @@ -235,6 +244,7 @@ void LosGuidanceNode::set_los_mode( } vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs) { + vortex_msgs::msg::LOSGuidance reference_msg; reference_msg.pitch = outputs.theta_d; reference_msg.yaw = outputs.psi_d; @@ -243,6 +253,39 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( return reference_msg; } +void LosGuidanceNode::publish_state_debug(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose) { + if (!enable_debug_) { + return; + } + + const auto &q_msg = current_pose->pose.pose.orientation; + + Eigen::Quaterniond quat( + q_msg.w, + q_msg.x, + q_msg.y, + q_msg.z + ); + + + Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(quat); + + vortex_msgs::msg::PoseEulerStamped debug_msg; + debug_msg.header.stamp = this->now(); + debug_msg.header.frame_id = "los_guidance_debug"; + + debug_msg.x = current_pose->pose.pose.position.x; + debug_msg.y = current_pose->pose.pose.position.y; + debug_msg.z = current_pose->pose.pose.position.z; + debug_msg.roll = euler.x(); + debug_msg.pitch = euler.y(); + debug_msg.yaw = euler.z(); + + debug_pub_->publish(debug_msg); + +} + YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { YAML::Node config = YAML::LoadFile(yaml_file_path); return config; @@ -256,6 +299,11 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { common_config["active_los_method"].as()); } +void LosGuidanceNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + +} + void LosGuidanceNode::execute( const std::shared_ptr< From f073936fe3b91531c8649b435d43b46a7b0aa7f2 Mon Sep 17 00:00:00 2001 From: Anbit Date: Fri, 16 Jan 2026 14:35:57 +0100 Subject: [PATCH 25/87] Fix building error --- guidance/los_guidance/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 8fef0da18..5f696a725 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -16,7 +16,7 @@ nav_msgs vortex_utils_ros eigen - + yaml-cpp ament_cmake From 70d0bb33c2dca0c2c9839fc2c82a3fab49924f29 Mon Sep 17 00:00:00 2001 From: Anbit Date: Fri, 23 Jan 2026 16:47:20 +0100 Subject: [PATCH 26/87] Clean the code --- auv_setup/config/robots/orca.yaml | 1 + .../los_guidance/config/guidance_params.yaml | 6 +- .../include/los_guidance/lib/integral_los.hpp | 2 - .../los_guidance/lib/proportional_los.hpp | 6 +- .../include/los_guidance/los_guidance_ros.hpp | 19 ++-- .../launch/guidance_test.launch.py | 21 +++- .../los_guidance/src/lib/adaptive_los.cpp | 2 +- .../los_guidance/src/lib/integral_los.cpp | 2 +- .../los_guidance/src/lib/proportional_los.cpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 104 +++++++----------- 10 files changed, 75 insertions(+), 90 deletions(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 1fef5f08b..879dcdca5 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -121,6 +121,7 @@ joy: "joy" pose: "pose" twist: "twist" + odom: "odom" operation_mode: "operation_mode" killswitch: "killswitch" aruco_board_pose_camera: "aruco_board_pose_camera" diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 5d5d0b754..496980fa4 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -7,12 +7,8 @@ adaptive_los: prop_los: lookahead_distance_h: 1.5 lookahead_distance_v: 0.6 - k_p_h: 0.5 - k_p_v: 0.5 integer_los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 k_p_h: 0.5 k_p_v: 0.5 k_i_h: 0.1 @@ -25,7 +21,7 @@ vector_field_los: k_p_v: 0.5 common: - active_los_method: 0 # 0: Proportional, 1: Integral, 2: Adaptive + active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 goal_reached_tol: 1.0 diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index 4af1b60ac..0509aaa28 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -9,8 +9,6 @@ namespace vortex::guidance::los { struct IntegralLosParams { - double lookahead_distance_h{}; - double lookahead_distance_v{}; double k_p_h{}; double k_p_v{}; double k_i_h{}; diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index 2a5ea00a1..1dd96654c 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -10,12 +10,9 @@ namespace vortex::guidance::los { struct ProportionalLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; - double k_p_h{}; - double k_p_v{}; - double time_step{}; }; -class ProportionalLOSGuidance { +class ProportionalLOSGuidance { public: ProportionalLOSGuidance(const ProportionalLosParams& params); ~ProportionalLOSGuidance() = default; @@ -28,7 +25,6 @@ class ProportionalLOSGuidance { const types::Inputs& inputs) const; ProportionalLosParams m_params{}; - // again i dont know if i should have them here or just in the functions double pi_h_{0.0}; double pi_v_{0.0}; Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 5f79d75b5..202e38573 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,6 +1,7 @@ #ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#include #include #include #include @@ -20,7 +21,7 @@ #include "los_guidance/lib/types.hpp" #include #include -#include + namespace vortex::guidance::los { @@ -30,7 +31,7 @@ class LosGuidanceNode : public rclcpp::Node { private: // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(YAML::Node config); + void set_subscribers_and_publisher(); // @brief Set the action server void set_action_server(); @@ -109,20 +110,20 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr reference_pub_; - bool enable_debug_; - - std::string debug_topic_name_; + rclcpp::Publisher::SharedPtr los_debug_pub_; - rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Publisher::SharedPtr state_debug_pub_; rclcpp::Subscription::SharedPtr waypoint_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; + rclcpp::Subscription< + nav_msgs::msg::Odometry>::SharedPtr odom_sub_; + + rclcpp::TimerBase::SharedPtr reference_pub_timer_; std::chrono::milliseconds time_step_; @@ -148,6 +149,8 @@ class LosGuidanceNode : public rclcpp::Node { std::unique_ptr proportional_los_{}; std::unique_ptr vector_field_los_{}; types::ActiveLosMethod method_{}; + + nav_msgs::msg::Odometry::SharedPtr debug_current_odom_{}; }; } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index f982f669f..56f3f5b47 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -1,5 +1,5 @@ from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription, TimerAction, ExecuteProcess, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory import os @@ -42,9 +42,28 @@ def generate_launch_description(): ] ) + set_autonomy = TimerAction( + period=12.0, + actions=[ + ExecuteProcess( + cmd=[ + "bash", "-lc", + "for i in {1..5}; do " + " ros2 topic pub --once /orca/killswitch std_msgs/msg/Bool \"{data: false}\"; " + " ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; " + " sleep 1; " + "done" + ], + output="screen", + ), + ], + ) + + return LaunchDescription([ stonefish_sim, los_guidance_launch, velocity_controller_launch, orca_sim, + set_autonomy, ]) \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index f35d9a053..7a1e0d265 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -4,7 +4,7 @@ namespace vortex::guidance::los { AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) - : params_{params} {} + : params_{params} {} void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index b1a4e3962..c0450a62f 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -14,7 +14,7 @@ void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} +} types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) { diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index 0bb65ba01..522de51e9 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -49,6 +49,6 @@ types::Outputs ProportionalLOSGuidance::calculate_outputs( const double theta_d = pi_v_ + std::atan(k_p_v * cross_track_error.z_e); return types::Outputs{psi_d, theta_d}; -} +} } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 320eeea7d..0aa21dcb0 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -21,14 +21,14 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); // auto config = this->declare_parameter("los_config_file"); - + // Do you need yaml path here? Can't you just use ros params directly from the guidance_params.yaml file? const std::string yaml_path = this->declare_parameter("los_config_file"); YAML::Node config = get_los_config(yaml_path); parse_common_config(config["common"]); - set_subscribers_and_publisher(config); + set_subscribers_and_publisher(); set_action_server(); set_service_server(); set_adaptive_los_guidance(config); @@ -39,11 +39,11 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { spdlog::info(start_message); } -void LosGuidanceNode::set_subscribers_and_publisher(YAML::Node config) { +void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.waypoint"); - this->declare_parameter("nav_topics.odometry"); + this->declare_parameter("topics.odom"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string guidance_topic = @@ -52,13 +52,19 @@ void LosGuidanceNode::set_subscribers_and_publisher(YAML::Node config) { this->get_parameter("topics.waypoint").as_string(); std::string odom_topic = - this->get_parameter("nav_topics.odometry").as_string(); + this->get_parameter("topics.odom").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); reference_pub_ = this->create_publisher( guidance_topic, qos_sensor_data); + los_debug_pub_ = this->create_publisher( + "los_debug", qos_sensor_data); + + state_debug_pub_ = this->create_publisher( + "state_debug", qos_sensor_data); + waypoint_sub_ = this->create_subscription( waypoint_topic, qos_sensor_data, std::bind(&LosGuidanceNode::waypoint_callback, this, @@ -73,16 +79,7 @@ void LosGuidanceNode::set_subscribers_and_publisher(YAML::Node config) { odom_sub_ = this->create_subscription( odom_topic, qos_sensor_data, std::bind(&LosGuidanceNode::odom_callback, this, - std::placeholders::_1)); - - auto debug_config = config["debug"]; - enable_debug_ = debug_config["enable_debug"].as(); - debug_topic_name_ = debug_config["debug_topic_name"].as(); - - if (enable_debug_) { - debug_pub_ = this->create_publisher( - debug_topic_name_, qos_sensor_data); - } + std::placeholders::_1)); } void LosGuidanceNode::set_action_server() { @@ -136,15 +133,8 @@ void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { auto params = ProportionalLosParams{}; params.lookahead_distance_h = proportional_los_config["lookahead_distance_h"].as(); - params.lookahead_distance_v = proportional_los_config["lookahead_distance_v"].as(); - params.k_p_h = - proportional_los_config["k_p_h"].as(); - params.k_p_v = - proportional_los_config["k_p_v"].as(); - params.time_step = - static_cast(time_step_.count()) / 1000.0; proportional_los_ = std::make_unique(params); } @@ -152,10 +142,6 @@ void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; auto params = IntegralLosParams{}; - params.lookahead_distance_h = - integral_los_config["lookahead_distance_h"].as(); - params.lookahead_distance_v = - integral_los_config["lookahead_distance_v"].as(); params.k_p_h = integral_los_config["k_p_h"].as(); params.k_p_v = @@ -199,10 +185,14 @@ void LosGuidanceNode::pose_callback( current_pose) { path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); +} - publish_state_debug(current_pose); +void LosGuidanceNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(mutex_); + debug_current_odom_ = msg; } + rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { @@ -253,39 +243,6 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( return reference_msg; } -void LosGuidanceNode::publish_state_debug(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose) { - if (!enable_debug_) { - return; - } - - const auto &q_msg = current_pose->pose.pose.orientation; - - Eigen::Quaterniond quat( - q_msg.w, - q_msg.x, - q_msg.y, - q_msg.z - ); - - - Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(quat); - - vortex_msgs::msg::PoseEulerStamped debug_msg; - debug_msg.header.stamp = this->now(); - debug_msg.header.frame_id = "los_guidance_debug"; - - debug_msg.x = current_pose->pose.pose.position.x; - debug_msg.y = current_pose->pose.pose.position.y; - debug_msg.z = current_pose->pose.pose.position.z; - debug_msg.roll = euler.x(); - debug_msg.pitch = euler.y(); - debug_msg.yaw = euler.z(); - - debug_pub_->publish(debug_msg); - -} - YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { YAML::Node config = YAML::LoadFile(yaml_file_path); return config; @@ -299,16 +256,11 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { common_config["active_los_method"].as()); } -void LosGuidanceNode::odom_callback( - const nav_msgs::msg::Odometry::SharedPtr msg) { - -} - void LosGuidanceNode::execute( - const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { + { std::lock_guard lock(mutex_); this->goal_handle_ = goal_handle; @@ -371,9 +323,29 @@ void LosGuidanceNode::execute( fill_los_reference(outputs); feedback->feedback = reference_msg; + los_debug_pub_->publish(reference_msg); + + double surge = std::sqrt(debug_current_odom_->twist.twist.linear.x + + debug_current_odom_->twist.twist.linear.y + + debug_current_odom_->twist.twist.linear.z); + + vortex_msgs::msg::LOSGuidance state_debug_msg; + Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( + Eigen::Quaterniond( + debug_current_odom_->pose.pose.orientation.w, + debug_current_odom_->pose.pose.orientation.x, + debug_current_odom_->pose.pose.orientation.y, + debug_current_odom_->pose.pose.orientation.z)); + + state_debug_msg.pitch = euler.y(); + state_debug_msg.yaw = euler.z(); + state_debug_msg.surge = surge; + state_debug_pub_->publish(state_debug_msg); + goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); + if ((path_inputs_.current_position - path_inputs_.next_point) .as_vector() .norm() < goal_reached_tol_) { From 47b5121a2c6526c4d61a62ed990958fd43b58a56 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 25 Jan 2026 15:33:34 +0100 Subject: [PATCH 27/87] Fix build error --- guidance/los_guidance/src/los_guidance_ros.cpp | 6 ++++-- guidance/los_guidance/test/integral_los_test.cpp | 2 -- guidance/los_guidance/test/proportional_los_test.cpp | 3 --- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 0aa21dcb0..1cac3aa3b 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -65,7 +65,8 @@ void LosGuidanceNode::set_subscribers_and_publisher() { state_debug_pub_ = this->create_publisher( "state_debug", qos_sensor_data); - waypoint_sub_ = this->create_subscription( + waypoint_sub_ = this->create_subscription< + geometry_msgs::msg::PointStamped>( waypoint_topic, qos_sensor_data, std::bind(&LosGuidanceNode::waypoint_callback, this, std::placeholders::_1)); @@ -76,7 +77,8 @@ void LosGuidanceNode::set_subscribers_and_publisher() { std::bind(&LosGuidanceNode::pose_callback, this, std::placeholders::_1)); - odom_sub_ = this->create_subscription( + odom_sub_ = this->create_subscription< + nav_msgs::msg::Odometry>( odom_topic, qos_sensor_data, std::bind(&LosGuidanceNode::odom_callback, this, std::placeholders::_1)); diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp index f94e93df1..1999abac8 100644 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -10,8 +10,6 @@ class IntegralLosTest : public ::testing::Test { IntegralLosParams get_params() { IntegralLosParams params; - params.lookahead_distance_h = 1.0; - params.lookahead_distance_v = 1.0; params.k_i_h = 0.1; // needs tuning params.k_i_v = 0.1; // needs tuning params.k_p_h = 0.667; // needs tuning diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp index c704dec68..b35a3ba6f 100644 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -11,9 +11,6 @@ class ProportionalLosTest : public ::testing::Test { ProportionalLosParams params; params.lookahead_distance_h = 10.0; params.lookahead_distance_v = 10.0; - params.k_p_h = 0.667; // needs tuning - params.k_p_v = 0.582; // needs tuning - params.time_step = 0.01; return params; } From 6a13ca136e74bb2c0aae3073b21c641674cb2aea Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 4 Feb 2026 11:46:25 +0100 Subject: [PATCH 28/87] Fix surge error --- guidance/los_guidance/src/los_guidance_ros.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 1cac3aa3b..e69c9ed67 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -327,9 +327,8 @@ void LosGuidanceNode::execute( los_debug_pub_->publish(reference_msg); - double surge = std::sqrt(debug_current_odom_->twist.twist.linear.x + - debug_current_odom_->twist.twist.linear.y + - debug_current_odom_->twist.twist.linear.z); + const auto& v = debug_current_odom_->twist.twist.linear; + double surge = std::sqrt(v.x*v.x + v.y*v.y + v.z*v.z); vortex_msgs::msg::LOSGuidance state_debug_msg; Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( From 2bd2c4ea89115e623cc1e8212afe176f5fd01149 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 11 Feb 2026 15:24:54 +0100 Subject: [PATCH 29/87] Add stoping msg publishes when goal reached --- guidance/los_guidance/config/guidance_params.yaml | 4 ++-- guidance/los_guidance/launch/guidance_test.launch.py | 8 ++++++++ guidance/los_guidance/src/los_guidance_ros.cpp | 11 ++++++++--- 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 496980fa4..8e17c67e4 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -21,9 +21,9 @@ vector_field_los: k_p_v: 0.5 common: - active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos + active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 - goal_reached_tol: 1.0 + goal_reached_tol: 0.5 debug: enable_debug: true diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 56f3f5b47..d49971b00 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -6,6 +6,7 @@ def generate_launch_description(): stonefish_dir = get_package_share_directory('stonefish_sim') + vortex_sim_interface_dir = get_package_share_directory('vortex_sim_interface') los_guidance_dir = get_package_share_directory('los_guidance') velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') @@ -19,6 +20,12 @@ def generate_launch_description(): }.items(), ) + vortex_sim_interface = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(vortex_sim_interface_dir, 'launch', 'vortex_sim_interface.launch.py') + ) + ) + los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(los_guidance_dir, 'launch', 'los_guidance.launch.py') @@ -62,6 +69,7 @@ def generate_launch_description(): return LaunchDescription([ stonefish_sim, + vortex_sim_interface, los_guidance_launch, velocity_controller_launch, orca_sim, diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index e69c9ed67..ff5a1b5ce 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -347,9 +347,14 @@ void LosGuidanceNode::execute( reference_pub_->publish(reference_msg); - if ((path_inputs_.current_position - path_inputs_.next_point) - .as_vector() - .norm() < goal_reached_tol_) { + if ((path_inputs_.current_position - path_inputs_.next_point).as_vector().norm() < goal_reached_tol_) { + + auto stop_ref = reference_msg; + stop_ref.surge = 0.0; + stop_ref.pitch = 0.0; + stop_ref.yaw = reference_msg.yaw; + + reference_pub_->publish(stop_ref); result->success = true; goal_handle->succeed(result); spdlog::info("Goal reached"); From 52c919f68a58a4086f8e75f037f8f6a3abf103c7 Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 16 Feb 2026 15:25:39 +0100 Subject: [PATCH 30/87] Add a square test script --- .../los_guidance/config/guidance_params.yaml | 6 +- .../launch/guidance_test.launch.py | 21 +++- guidance/los_guidance/scripts/square_test.py | 100 ++++++++++++++++++ .../los_guidance/src/lib/adaptive_los.cpp | 1 + 4 files changed, 120 insertions(+), 8 deletions(-) create mode 100644 guidance/los_guidance/scripts/square_test.py diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 8e17c67e4..1d11850d0 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -15,15 +15,15 @@ integer_los: k_i_v: 0.1 vector_field_los: - max_approach_angle_h: 1.0 # rad - max_approach_angle_v: 0.6 # rad + max_approach_angle_h: 1.0 + max_approach_angle_v: 0.6 k_p_h: 0.5 k_p_v: 0.5 common: active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 - goal_reached_tol: 0.5 + goal_reached_tol: 1.0 debug: enable_debug: true diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index d49971b00..5ee3886ad 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -1,3 +1,4 @@ +from yaml import Node from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, TimerAction, ExecuteProcess, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -55,17 +56,26 @@ def generate_launch_description(): ExecuteProcess( cmd=[ "bash", "-lc", - "for i in {1..5}; do " - " ros2 topic pub --once /orca/killswitch std_msgs/msg/Bool \"{data: false}\"; " - " ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; " - " sleep 1; " - "done" + "ros2 topic pub --once /orca/killswitch std_msgs/msg/Bool \"{data: false}\"; " + "ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; " ], output="screen", ), ], ) + square_test = TimerAction( + period=14.0, + actions=[ + ExecuteProcess( + cmd=[ + "bash", "-lc", + f"python3 {os.path.join(los_guidance_dir, 'scripts', 'square_test.py')}" + ], + output="screen", + ) + ], + ) return LaunchDescription([ stonefish_sim, @@ -74,4 +84,5 @@ def generate_launch_description(): velocity_controller_launch, orca_sim, set_autonomy, + square_test, ]) \ No newline at end of file diff --git a/guidance/los_guidance/scripts/square_test.py b/guidance/los_guidance/scripts/square_test.py new file mode 100644 index 000000000..c5bd078b0 --- /dev/null +++ b/guidance/los_guidance/scripts/square_test.py @@ -0,0 +1,100 @@ +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient + +from vortex_msgs.action import LOSGuidance +from geometry_msgs.msg import Point +from std_msgs.msg import Header + + +class SquareTest(Node): + + def __init__(self): + super().__init__('square_test_client') + + self._action_client = ActionClient( + self, + LOSGuidance, + '/orca/los_guidance' + ) + + self.depth = 2.5 + self.size = 10.0 + + self.waypoints = [ + (self.size, 0.0, self.depth), + (self.size, self.size, self.depth), + (0.0, self.size, self.depth), + (0.0, 0.0, self.depth), + ] + + self.current_index = 0 + + self.send_next_goal() + + def send_next_goal(self): + + if self.current_index >= len(self.waypoints): + self.get_logger().info("Square test completed!") + rclpy.shutdown() + return + + self._action_client.wait_for_server() + + goal_msg = LOSGuidance.Goal() + + header = Header() + header.frame_id = "world_ned" + + goal_msg.goal.header = header + + x, y, z = self.waypoints[self.current_index] + + goal_msg.goal.point.x = x + goal_msg.goal.point.y = y + goal_msg.goal.point.z = z + + self.get_logger().info( + f"Sending waypoint {self.current_index + 1}: " + f"x={x}, y={y}, z={z}" + ) + + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback + ) + + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + + goal_handle = future.result() + + if not goal_handle.accepted: + self.get_logger().info('Goal rejected') + return + + self.get_logger().info('Goal accepted') + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.result_callback) + + def result_callback(self, future): + + self.get_logger().info("Waypoint reached") + + self.current_index += 1 + self.send_next_goal() + + def feedback_callback(self, feedback_msg): + pass + + +def main(args=None): + rclpy.init(args=args) + node = SquareTest() + rclpy.spin(node) + + +if __name__ == '__main__': + main() diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 7a1e0d265..3303589f8 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -58,6 +58,7 @@ types::Outputs AdaptiveLOSGuidance::calculate_outputs( const double theta_d = pi_v_ + alpha_c_hat_ + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); + return types::Outputs{psi_d, theta_d}; } From 11e8d60dcb53b8999a8bc22f21eb89d06777ea0e Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 16 Feb 2026 15:59:55 +0100 Subject: [PATCH 31/87] Fix Cmake problome for squaretest --- guidance/los_guidance/CMakeLists.txt | 6 ++++++ guidance/los_guidance/launch/guidance_test.launch.py | 2 +- guidance/los_guidance/scripts/square_test.py | 3 +++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 1dbdbe0d9..a943a4f24 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -86,8 +86,14 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +install(PROGRAMS + scripts/square_test.py + DESTINATION share/${PROJECT_NAME}/scripts +) + if(BUILD_TESTING) add_subdirectory(test) endif() ament_package() + diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 5ee3886ad..862e39b50 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -65,7 +65,7 @@ def generate_launch_description(): ) square_test = TimerAction( - period=14.0, + period=20.0, actions=[ ExecuteProcess( cmd=[ diff --git a/guidance/los_guidance/scripts/square_test.py b/guidance/los_guidance/scripts/square_test.py index c5bd078b0..8fe518e9d 100644 --- a/guidance/los_guidance/scripts/square_test.py +++ b/guidance/los_guidance/scripts/square_test.py @@ -5,6 +5,7 @@ from vortex_msgs.action import LOSGuidance from geometry_msgs.msg import Point from std_msgs.msg import Header +from launch.actions import LogInfo class SquareTest(Node): @@ -12,6 +13,8 @@ class SquareTest(Node): def __init__(self): super().__init__('square_test_client') + self.get_logger().info("Square test started") + self._action_client = ActionClient( self, LOSGuidance, From 30568ab65cd8711f1f6d6328db4b99bd666cf926 Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 16 Feb 2026 19:32:36 +0100 Subject: [PATCH 32/87] Fix adaptive param lag --- guidance/los_guidance/config/guidance_params.yaml | 2 +- .../include/los_guidance/lib/adaptive_los.hpp | 1 + guidance/los_guidance/src/lib/adaptive_los.cpp | 8 +++++++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 1d11850d0..7a87ae308 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -23,7 +23,7 @@ vector_field_los: common: active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 - goal_reached_tol: 1.0 + goal_reached_tol: 0.5 debug: enable_debug: true diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index f874db35f..5e4956061 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -34,6 +34,7 @@ class AdaptiveLOSGuidance { const types::Inputs& inputs); void update_adaptive_estimates( const types::CrossTrackError& cross_track_error); + void reset_adaptive_params(); AdaptiveLosParams params_{}; Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 3303589f8..367ccb431 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -28,7 +28,7 @@ const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( return types::CrossTrackError::from_vector(cross_track_error); } - + void AdaptiveLOSGuidance::update_adaptive_estimates( const types::CrossTrackError& cross_track_error) { const double denom_h = std::sqrt(params_.lookahead_distance_h * @@ -47,10 +47,16 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( alpha_c_hat_ += alpha_dot * params_.time_step; } +void AdaptiveLOSGuidance::reset_adaptive_params(){ + beta_c_hat_= 0; + alpha_c_hat_ = 0; +} + types::Outputs AdaptiveLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + void reset_adaptive_params(); update_adaptive_estimates(cross_track_error); const double psi_d = From 73b460c25e3ab6f5493a37c3003be0b555b117d0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 22 Feb 2026 18:32:38 +0100 Subject: [PATCH 33/87] Add tuning parameters --- .../velocity_controller_lqr/__init__.py | 0 .../los_guidance/config/guidance_params.yaml | 32 ++++++++--------- .../include/los_guidance/lib/adaptive_los.hpp | 5 ++- .../include/los_guidance/los_guidance_ros.hpp | 2 ++ .../los_guidance/src/lib/adaptive_los.cpp | 6 ---- .../los_guidance/src/lib/proportional_los.cpp | 9 ----- .../los_guidance/src/los_guidance_ros.cpp | 34 +++++++++++++++---- 7 files changed, 47 insertions(+), 41 deletions(-) delete mode 100644 control/velocity_controller_lqr/velocity_controller_lqr/__init__.py diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 7a87ae308..66bd0c2e3 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,29 +1,29 @@ adaptive_los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 + lookahead_distance_h: 0.9 #Done + lookahead_distance_v: 1.4 #Done + gamma_h: 0.03 #Done + gamma_v: 0.02 #Done prop_los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 + lookahead_distance_h: 0.74 #Done + lookahead_distance_v: 0.8 #Done integer_los: - k_p_h: 0.5 - k_p_v: 0.5 - k_i_h: 0.1 - k_i_v: 0.1 + k_p_h: 0.5 #Done + k_p_v: 0.5 #Done + k_i_h: 0.1 #Done + k_i_v: 0.1 #Done vector_field_los: max_approach_angle_h: 1.0 - max_approach_angle_v: 0.6 - k_p_h: 0.5 - k_p_v: 0.5 + max_approach_angle_v: 0.6 + k_p_h: 1.5 + k_p_v: 0.3 common: - active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos - u_desired: 0.3 - goal_reached_tol: 0.5 + active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos + u_desired: 0.3 # Done + goal_reached_tol: 0.35 # Done debug: enable_debug: true diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 5e4956061..cca8b3553 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -34,15 +34,14 @@ class AdaptiveLOSGuidance { const types::Inputs& inputs); void update_adaptive_estimates( const types::CrossTrackError& cross_track_error); - void reset_adaptive_params(); AdaptiveLosParams params_{}; Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); double pi_h_{}; double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; + double beta_c_hat_ = 0.0; + double alpha_c_hat_ = 0.0; }; // namespace vortex::guidance::los diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 202e38573..a3ba2f764 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -87,6 +87,8 @@ class LosGuidanceNode : public rclcpp::Node { // @brief Execute the goal // @param goal_handle The goal handle + + bool has_active_segment_{false}; void execute(const std::shared_ptr> goal_handle); diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 367ccb431..52607bb4e 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -47,16 +47,10 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( alpha_c_hat_ += alpha_dot * params_.time_step; } -void AdaptiveLOSGuidance::reset_adaptive_params(){ - beta_c_hat_= 0; - alpha_c_hat_ = 0; -} - types::Outputs AdaptiveLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); - void reset_adaptive_params(); update_adaptive_estimates(cross_track_error); const double psi_d = diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index 522de51e9..ae7aeff4e 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -4,15 +4,6 @@ namespace vortex::guidance::los { ProportionalLOSGuidance::ProportionalLOSGuidance( const ProportionalLosParams& params) : m_params{params} { - - if (m_params.lookahead_distance_h <= 0.0) { - m_params.lookahead_distance_h = 1e-9; - } - - if (m_params.lookahead_distance_v <= 0.0) { - m_params.lookahead_distance_v = 1e-9; - } - } void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index ff5a1b5ce..45ecd9dde 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -175,13 +175,25 @@ void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { } void LosGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = types::Point::point_from_ros(los_waypoint->point); - spdlog::info( - "Received waypoint"); // remember to print waypoint that you get + const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) +{ + const auto new_wp = types::Point::point_from_ros(wp_msg->point); + + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } + + spdlog::info("Received waypoint: ({}, {}, {})", + new_wp.x, new_wp.y, new_wp.z); } + + void LosGuidanceNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { @@ -273,8 +285,16 @@ void LosGuidanceNode::execute( const geometry_msgs::msg::PointStamped los_waypoint = goal_handle->get_goal()->goal; - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = types::Point::point_from_ros(los_waypoint.point); + const auto new_wp = types::Point::point_from_ros(los_waypoint.point); + + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } auto feedback = std::make_shared(); From 29a57097bb383bffa7adee26b305359a6171cb1c Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 22 Feb 2026 18:39:16 +0100 Subject: [PATCH 34/87] merge main updates into this branch --- auv_setup/config/robots/orca.yaml | 2 -- 1 file changed, 2 deletions(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 49fd3f273..77c827647 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -142,8 +142,6 @@ set_killswitch: "set_killswitch" toggle_killswitch: "toggle_killswitch" get_operation_mode: "get_operation_mode" - - services: # Maybe not the right place for this? los_mode: "set_los_mode" fsm: From 76f803e2924495404cc29cacf669dedb23064ba8 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Feb 2026 15:30:30 +0100 Subject: [PATCH 35/87] Add tuned parameters --- guidance/los_guidance/config/guidance_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 66bd0c2e3..62164a9a1 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -21,7 +21,7 @@ vector_field_los: k_p_v: 0.3 common: - active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos + active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 # Done goal_reached_tol: 0.35 # Done From 6ecdc9fa8b4e138ab98848049de43b87fae2c253 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Feb 2026 17:13:52 +0100 Subject: [PATCH 36/87] Fix vector feild simulation issues --- .../los_guidance/config/guidance_params.yaml | 2 +- guidance/los_guidance/src/los_guidance_ros.cpp | 17 ++++++++++++----- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 62164a9a1..66bd0c2e3 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -21,7 +21,7 @@ vector_field_los: k_p_v: 0.3 common: - active_los_method: 2 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos + active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos u_desired: 0.3 # Done goal_reached_tol: 0.35 # Done diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 45ecd9dde..ba816d4d9 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -177,6 +177,8 @@ void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { void LosGuidanceNode::waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { + std::lock_guard lock(mutex_); + const auto new_wp = types::Point::point_from_ros(wp_msg->point); if (!has_active_segment_) { @@ -184,19 +186,18 @@ void LosGuidanceNode::waypoint_callback( path_inputs_.next_point = new_wp; has_active_segment_ = true; } else { - path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.prev_point = path_inputs_.next_point; path_inputs_.next_point = new_wp; } - spdlog::info("Received waypoint: ({}, {}, {})", - new_wp.x, new_wp.y, new_wp.z); + spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, new_wp.z); } void LosGuidanceNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose) { + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { + std::lock_guard lock(mutex_); path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); } @@ -319,6 +320,12 @@ void LosGuidanceNode::execute( return; } + types::Inputs inputs_copy; + { + std::lock_guard lock(mutex_); + inputs_copy = path_inputs_; + } + types::Outputs outputs; switch (method_) { From 17d89c98ccfca092d9071c756aee3e5fd08ea7ad Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 15:30:10 +0100 Subject: [PATCH 37/87] Clean code and readMe draft --- guidance/los_guidance/README.md | 260 ++++++++++++++++-- .../los_guidance/config/guidance_params.yaml | 38 +-- .../include/los_guidance/lib/adaptive_los.hpp | 28 +- .../include/los_guidance/lib/integral_los.hpp | 21 +- .../los_guidance/lib/proportional_los.hpp | 18 +- .../include/los_guidance/lib/types.hpp | 21 +- .../los_guidance/lib/vector_field_los.hpp | 24 +- .../include/los_guidance/los_guidance_ros.hpp | 120 ++++---- .../launch/guidance_test.launch.py | 2 +- .../launch/los_guidance.launch.py | 2 - .../los_guidance/src/lib/adaptive_los.cpp | 45 +-- .../los_guidance/src/lib/integral_los.cpp | 27 +- .../los_guidance/src/lib/proportional_los.cpp | 23 +- .../los_guidance/src/lib/vector_field_los.cpp | 28 +- .../los_guidance/src/los_guidance_node.cpp | 4 +- .../los_guidance/src/los_guidance_ros.cpp | 109 ++++---- 16 files changed, 537 insertions(+), 233 deletions(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 33c616c59..2afe923c1 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -1,22 +1,60 @@ -## ALSO Guidance Law for 3D Path Following +# 3D LOS Guidance Library (Read me draft) -The guidance law gives calculates the desired heading angle $\psi_d$ and desired pitch angle $\theta_d$. The crab angles $\beta_c$ and $\alpha_c$ are estimated adaptively. The guidance law looks like +This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. +The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ so a vehicle can follow a path between waypoints. + +The vehicle surge speed is kept **constant** during path following and is defined in the configuration file using the parameter + +``` +u_desired +``` + +This value represents the desired forward velocity of the vehicle. + +--- + +# Implemented LOS Methods + +The library supports four LOS guidance algorithms. + +| Mode | Method | +|-----|------| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +The guidance method can be **changed during runtime** using a ROS service. + +--- + +# Adaptive LOS (ALOS) + +Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. ```math \psi_d = \pi_h - \hat{\beta}_c - \tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) - ``` ```math -\dot{\hat{\beta}}_c = \gamma_h \frac{\Delta_h}{\sqrt{\Delta_h^2 + (y_e^p)^2}} y_e^p +\dot{\hat{\beta}}_c = +\gamma_h +\frac{\Delta_h}{\sqrt{\Delta_h^2 + (y_e^p)^2}} +y_e^p ``` ```math -\theta_d = \pi_v + \hat{\alpha}_c + \tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) +\theta_d = +\pi_v + +\hat{\alpha}_c + +\tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` ```math -\dot{\hat{\alpha}}_c = \gamma_v \frac{\Delta_v}{\sqrt{\Delta_v^2 + (z_e^p)^2}} z_e^p +\dot{\hat{\alpha}}_c = +\gamma_v +\frac{\Delta_v}{\sqrt{\Delta_v^2 + (z_e^p)^2}} +z_e^p ``` where @@ -27,27 +65,217 @@ where - $y_e^p$ is the cross-track error - $z_e^p$ is the vertical-track error -The azimuth angle $\pi_v$ and the elevation angle $\pi_h$ can be found by +Adaptive LOS is generally the **most robust method** and works well for: + +- curved trajectories +- long paths +- environments with disturbances. + +--- + +# Proportional LOS (PLOS) + +Proportional LOS is the simplest LOS guidance law. + +```math +\psi_d = +\pi_h - +\tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) +``` + +```math +\theta_d = +\pi_v + +\tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) +``` + +It is best suited for + +- simple waypoint following +- calm environments with little disturbance. + +However, steady-state tracking error may occur if disturbances are present. + +--- + +# Integral LOS (ILOS) + +Integral LOS adds integral action to remove steady-state error. ```math -\pi_h = \text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n, - x_i^n) +u_h = +k_{p,h}y_e^p + +k_{i,h}\int y_e^p dt ``` + ```math -\pi_v = \text{atan2}(-(z_{i+1}^n - z_i^n), \sqrt{(x_{i+1}^n - x_i^n)^2 + (y_{i+1}^n - y_i^n)^2}) +u_v = +k_{p,v}z_e^p + +k_{i,v}\int z_e^p dt ``` -where $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down frame and $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint in north-east-down frame. +```math +\psi_d = +\pi_h - +\tan^{-1}(u_h) +``` + +```math +\theta_d = +\pi_v + +\tan^{-1}(u_v) +``` + +Integral LOS performs well when there are **constant disturbances**, such as steady ocean currents or wind. + +--- + +# Vector Field LOS (VF-LOS) + +Vector Field LOS generates a **bounded approach angle** toward the path. + +```math +\psi_d = \pi_h - \chi_h +``` + +```math +\chi_h = +\psi_{max} +\frac{2}{\pi} +\tan^{-1}(k_p y_e^p) +``` + +This method is best suited for + +- long straight path following +- corridor tracking. + +However it performs worse when the path contains **sharp turns**. + +--- + +# Path Geometry + +The path between two waypoints defines the reference angles used by the guidance law. + +```math +\pi_h = +\text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n - x_i^n) +``` + +```math +\pi_v = +\text{atan2} +\left( +-(z_{i+1}^n - z_i^n), +\sqrt{(x_{i+1}^n - x_i^n)^2 + +(y_{i+1}^n - y_i^n)^2} +\right) +``` + +where + +- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down frame +- $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. + +--- + +# Path Frame Errors The along-, cross- and vertical-track errors in the path-tangential frame are found by ```math \begin{bmatrix} -x_e^p \\ y_e^p \\ z_e^p -\end{bmatrix} = \mathbf{R}_{y, \pi_v}^\top \mathbf{R}_{z, \pi_h}^\top \left( \begin{bmatrix} -x^n \\ y^n \\ z^n -\end{bmatrix} - \begin{bmatrix} -x_i^n \\ y_i^n \\ z_i^n +x_e^p \\ +y_e^p \\ +z_e^p +\end{bmatrix} += +\mathbf{R}_{y,\pi_v}^\top +\mathbf{R}_{z,\pi_h}^\top +\left( +\begin{bmatrix} +x^n \\ +y^n \\ +z^n +\end{bmatrix} +- +\begin{bmatrix} +x_i^n \\ +y_i^n \\ +z_i^n \end{bmatrix} \right) ``` -where $P^n = (x^n, y^n, z^n)$ is the current position of the drone. + +where + +- $x_e^p$ is the along-track error +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error + +and + +- $P^n = (x^n, y^n, z^n)$ is the current position of the vehicle. + +--- + +# Sending a LOS Goal + +A waypoint can be sent to the guidance node using the action interface: + +``` +ros2 action send_goal /orca/los_guidance \ +vortex_msgs/action/LOSGuidance \ +"{goal: {header: {frame_id: world_ned}, point: {x: 10.0, y: 5.0, z: -2.0}}}" +``` + +This command instructs the guidance node to start following a path toward the waypoint. + +--- + +# Switching LOS Method + +The active LOS guidance method can be changed during runtime. + +``` +ros2 service call /orca/set_los_mode \ +vortex_msgs/srv/SetLosMode "{mode: X}" +``` + +Where + +| X | Method | +|---|---| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +Example: + +``` +ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" +``` + +This switches the guidance system to **Adaptive LOS**. + +--- + +# ROS Topics + +### Subscribed Topics + +| Topic | Message | +|------|------| +| `/orca/pose` | `geometry_msgs/PoseWithCovarianceStamped` | +| `/orca/odom` | `nav_msgs/Odometry` | +| `/orca/waypoint` | `geometry_msgs/PointStamped` | + +### Published Topics + +| Topic | Message | +|------|------| +| `/orca/guidance/los` | `vortex_msgs/LOSGuidance` | +| `/los_debug` | `vortex_msgs/LOSGuidance` | +| `/state_debug` | `vortex_msgs/LOSGuidance` | \ No newline at end of file diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 66bd0c2e3..8ba560c1a 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,30 +1,36 @@ +# Adaptive LOS Parameters adaptive_los: - lookahead_distance_h: 0.9 #Done - lookahead_distance_v: 1.4 #Done - gamma_h: 0.03 #Done - gamma_v: 0.02 #Done + lookahead_distance_h: 0.9 + lookahead_distance_v: 1.4 + gamma_h: 0.03 + gamma_v: 0.02 +# Proportional LOS Parameters prop_los: - lookahead_distance_h: 0.74 #Done - lookahead_distance_v: 0.8 #Done + lookahead_distance_h: 0.74 + lookahead_distance_v: 0.8 +# Integral LOS Parameters integer_los: - k_p_h: 0.5 #Done - k_p_v: 0.5 #Done - k_i_h: 0.1 #Done - k_i_v: 0.1 #Done + k_p_h: 0.5 + k_p_v: 0.5 + k_i_h: 0.1 + k_i_v: 0.1 +# Vector Field LOS Parameters vector_field_los: - max_approach_angle_h: 1.0 - max_approach_angle_v: 0.6 + max_approach_angle_h: 1.0 + max_approach_angle_v: 1.0 k_p_h: 1.5 - k_p_v: 0.3 + k_p_v: 0.9 +# Common Guidance Parameters common: - active_los_method: 3 # 0: Proportional, 1: Integral, 2: Adaptive, 3: VFLos - u_desired: 0.3 # Done - goal_reached_tol: 0.35 # Done + active_los_method: 2 + u_desired: 0.3 + goal_reached_tol: 0.35 +# Debug Settings debug: enable_debug: true debug_topic_name: "/los_guidance_debug" \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index cca8b3553..f7174a89b 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -4,46 +4,56 @@ #include #include #include -#include "los_guidance/lib/types.hpp" -/** - * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 - * in "Fossen 2024 Lecture on 2D and 3D path-following control". - */ +#include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { +// Parameter Structure struct AdaptiveLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; double gamma_h{}; double gamma_v{}; - double time_step{}; + double time_step{}; }; +// Adaptive LOS Guidance Class class AdaptiveLOSGuidance { public: + + // Constructor / Destructor AdaptiveLOSGuidance(const AdaptiveLosParams& params); ~AdaptiveLOSGuidance() = default; + // Main Output Calculation types::Outputs calculate_outputs(const types::Inputs& inputs); private: + + // Internal Update Functions void update_angles(const types::Inputs& inputs); const types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs); void update_adaptive_estimates( const types::CrossTrackError& cross_track_error); + // Parameters AdaptiveLosParams params_{}; + + // Rotation Matrices Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + + // Path Angles double pi_h_{}; double pi_v_{}; + + // Adaptive Estimates double beta_c_hat_ = 0.0; double alpha_c_hat_ = 0.0; - -}; // namespace vortex::guidance::los +}; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE_HPP + +#endif // ADAPTIVE_LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index 0509aaa28..83a528461 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -4,40 +4,53 @@ #include #include #include + #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { -struct IntegralLosParams { +// Parameter Structure +struct IntegralLosParams { double k_p_h{}; double k_p_v{}; double k_i_h{}; double k_i_v{}; double time_step{}; -}; +}; +// Integral LOS Guidance Class class IntegralLOSGuidance { public: + + // Constructor / Destructor IntegralLOSGuidance(const IntegralLosParams& params); ~IntegralLOSGuidance() = default; + // Main Output Calculation types::Outputs calculate_outputs(const types::Inputs& inputs); private: + // Internal Update Functions void update_angles(const types::Inputs& inputs); types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs); + // Parameters IntegralLosParams m_params{}; + // Integral States double int_h{}; double int_v{}; - // again i dont know if i should have them here or just in the functions + + // Path Angles double pi_h_{}; double pi_v_{}; + + // Rotation Representation Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; + } // namespace vortex::guidance::los -#endif // INTEGRAL_LOS_GUIDANCE_HPP +#endif // INTEGRAL_LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index 1dd96654c..a56a5efae 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -4,33 +4,47 @@ #include #include #include + #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { + +// Parameter Structure struct ProportionalLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; }; -class ProportionalLOSGuidance { +// Proportional LOS Guidance Class +class ProportionalLOSGuidance { public: + + // Constructor / Destructor ProportionalLOSGuidance(const ProportionalLosParams& params); ~ProportionalLOSGuidance() = default; + // Main Output Calculation types::Outputs calculate_outputs(const types::Inputs& inputs); private: + + // Internal Update Functions void update_angles(const types::Inputs& inputs); types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs) const; + // Parameters ProportionalLosParams m_params{}; + + // Path Angles double pi_h_{0.0}; double pi_v_{0.0}; + + // Rotation Representation Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } // namespace vortex::guidance::los -#endif // PROPORTIONAL_LOS_GUIDANCE_HPP +#endif // PROPORTIONAL_LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index ca2492d10..a12aa5078 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -9,22 +9,28 @@ namespace vortex::guidance::los::types { +// Point Representation struct Point { double x{}; double y{}; double z{}; + // Point Operations Point operator-(const Point& other) const { return Point{x - other.x, y - other.y, z - other.z}; } - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + // Conversion Functions + Eigen::Vector3d as_vector() const { + return Eigen::Vector3d(x, y, z); + } static Point point_from_ros(const geometry_msgs::msg::Point& msg) { return Point{msg.x, msg.y, msg.z}; } }; +// Cross Track Error struct CrossTrackError { double x_e{}; double y_e{}; @@ -35,24 +41,27 @@ struct CrossTrackError { } }; +// Guidance Outputs struct Outputs { double psi_d{}; double theta_d{}; }; +// Guidance Inputs struct Inputs { Point prev_point{}; Point next_point{}; Point current_position{}; }; +// Active LOS Method enum class ActiveLosMethod { - PROPORTIONAL, // 0 - INTEGRAL, // 1 - ADAPTIVE, // 2 - VECTOR_FIELD // 3 + PROPORTIONAL, + INTEGRAL, + ADAPTIVE, + VECTOR_FIELD }; } // namespace vortex::guidance::los::types -#endif +#endif // TYPES_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index 9d3210ffd..3348b0f74 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -3,39 +3,51 @@ #include #include -#include +#include + #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { +// Parameter Structure struct VectorFieldLosParams { - double max_approach_angle_h{}; - double max_approach_angle_v{}; - double k_p_h{}; - double k_p_v{}; + double max_approach_angle_h{}; + double max_approach_angle_v{}; + double k_p_h{}; + double k_p_v{}; double time_step{}; }; +// Vector Field LOS Guidance Class class VectorFieldLOSGuidance { public: + + // Constructor / Destructor VectorFieldLOSGuidance(const VectorFieldLosParams& params); ~VectorFieldLOSGuidance() = default; + // Main Output Calculation types::Outputs calculate_outputs(const types::Inputs& inputs); private: + + // Internal Update Functions void update_angles(const types::Inputs& inputs); types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs) const; + // Parameters VectorFieldLosParams m_params{}; + // Path Angles double pi_h_{0.0}; double pi_v_{0.0}; + + // Rotation Representation Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } // namespace vortex::guidance::los -#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP +#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index a3ba2f764..e7bbc1482 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,160 +1,134 @@ #ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ -#include -#include #include #include #include #include -#include #include #include +#include #include +#include #include #include #include +#include + +#include +#include + #include "los_guidance/lib/adaptive_los.hpp" #include "los_guidance/lib/integral_los.hpp" -#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" #include "los_guidance/lib/vector_field_los.hpp" -#include "los_guidance/lib/types.hpp" -#include -#include - namespace vortex::guidance::los { +// LOS Guidance ROS Node class LosGuidanceNode : public rclcpp::Node { public: + + // Constructor LosGuidanceNode(); private: - // @brief Set the subscribers and publishers - void set_subscribers_and_publisher(); - // @brief Set the action server + // Setup Functions + void set_subscribers_and_publisher(); void set_action_server(); - - // @brief Determine the LOS mode service void set_service_server(); - // @brief Set the adaptive LOS guidance parameters + // Configuration Functions void set_adaptive_los_guidance(YAML::Node config); - - // @brief Set the proportional LOS guidance parameters void set_proportional_los_guidance(YAML::Node config); - - // @brief Set the integral LOS guidance parameters void set_integral_los_guidance(YAML::Node config); - - // @brief Set the vector field LOS guidance parameters void set_vector_field_guidance(YAML::Node config); + YAML::Node get_los_config(std::string yaml_file_path); + void parse_common_config(YAML::Node common_config); - // @brief Callback for the waypoint topic - // @param msg The reference message + // Callback Functions void waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr msg); - - // @brief Callback for the pose topic - // @param msg The pose message void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - void odom_callback( const nav_msgs::msg::Odometry::SharedPtr msg); - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response + // Action Server Functions rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); - - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle); + void handle_accepted( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle); + void execute( + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle); - // @brief Handle the accepted request - // @param goal_handle The goal handle - void handle_accepted(const std::shared_ptr> goal_handle); - - // @brief Execute the goal - // @param goal_handle The goal handle - - bool has_active_segment_{false}; - void execute(const std::shared_ptr> goal_handle); - + // Service Functions void set_los_mode( const std::shared_ptr request, std::shared_ptr response); - void publish_state_debug(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose); - + // Publish Functions + void publish_state_debug( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose); vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); - YAML::Node get_los_config(std::string yaml_file_path); - - void parse_common_config(YAML::Node common_config); + // State Flags + bool has_active_segment_{false}; + // ROS Interfaces rclcpp_action::Server::SharedPtr action_server_; - rclcpp::Service::SharedPtr los_mode_service_; - rclcpp::Publisher::SharedPtr reference_pub_; - rclcpp::Publisher::SharedPtr los_debug_pub_; - - rclcpp::Publisher::SharedPtr state_debug_pub_; - + rclcpp::Publisher::SharedPtr + state_debug_pub_; rclcpp::Subscription::SharedPtr waypoint_sub_; - rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - - rclcpp::Subscription< - nav_msgs::msg::Odometry>::SharedPtr odom_sub_; - - + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::TimerBase::SharedPtr reference_pub_timer_; + rclcpp::CallbackGroup::SharedPtr cb_group_; + // Timing and Synchronization std::chrono::milliseconds time_step_; - std::mutex mutex_; + // Action State rclcpp_action::GoalUUID preempted_goal_id_; - std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle_; - rclcpp::CallbackGroup::SharedPtr cb_group_; - + // Guidance State types::Inputs path_inputs_{}; - double u_desired_{}; - double goal_reached_tol_{}; + types::ActiveLosMethod method_{}; + // Guidance Modules std::unique_ptr adaptive_los_{}; std::unique_ptr integral_los_{}; std::unique_ptr proportional_los_{}; std::unique_ptr vector_field_los_{}; - types::ActiveLosMethod method_{}; + // Debug Data nav_msgs::msg::Odometry::SharedPtr debug_current_odom_{}; }; } // namespace vortex::guidance::los - -#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ + +#endif // LOS_GUIDANCE_ROS_HPP \ No newline at end of file diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 862e39b50..2bc63f2f9 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -17,7 +17,7 @@ def generate_launch_description(): ), launch_arguments={ 'scenario': 'tacc', - 'rendering': 'false', + 'rendering': 'true', }.items(), ) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 859c0a255..2a677d5c8 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -33,5 +33,3 @@ def generate_launch_description(): return LaunchDescription([los_guidance_node]) -# remember to make them able to swich in the middle of a mission and if you swirch method the parameters shouldn't be reloaded -# unless its a new section diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 52607bb4e..a4acee79f 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -1,11 +1,14 @@ #include + #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { +// Constructor AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) - : params_{params} {} + : params_{params} {} +// Angle Update void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; const double dy = inputs.next_point.y - inputs.prev_point.y; @@ -18,6 +21,7 @@ void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); } +// Cross-Track Error Calculation const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) { const types::Point difference = inputs.current_position - inputs.prev_point; @@ -28,40 +32,47 @@ const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( return types::CrossTrackError::from_vector(cross_track_error); } - + +// Adaptive Estimate Update void AdaptiveLOSGuidance::update_adaptive_estimates( const types::CrossTrackError& cross_track_error) { - const double denom_h = std::sqrt(params_.lookahead_distance_h * - params_.lookahead_distance_h + - cross_track_error.y_e * cross_track_error.y_e); - const double denom_v = std::sqrt(params_.lookahead_distance_v * - params_.lookahead_distance_v + - cross_track_error.z_e * cross_track_error.z_e); + const double denom_h = + std::sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + + cross_track_error.y_e * cross_track_error.y_e); + const double denom_v = + std::sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + + cross_track_error.z_e * cross_track_error.z_e); const double beta_dot = - params_.gamma_h * (params_.lookahead_distance_h / denom_h) * cross_track_error.y_e; + params_.gamma_h * (params_.lookahead_distance_h / denom_h) * + cross_track_error.y_e; const double alpha_dot = - params_.gamma_v * (params_.lookahead_distance_v / denom_v) * cross_track_error.z_e; + params_.gamma_v * (params_.lookahead_distance_v / denom_v) * + cross_track_error.z_e; beta_c_hat_ += beta_dot * params_.time_step; alpha_c_hat_ += alpha_dot * params_.time_step; } +// Output Calculation types::Outputs AdaptiveLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); + update_adaptive_estimates(cross_track_error); const double psi_d = - pi_h_ - beta_c_hat_ - std::atan(cross_track_error.y_e / params_.lookahead_distance_h); + pi_h_ - beta_c_hat_ - + std::atan(cross_track_error.y_e / params_.lookahead_distance_h); + const double theta_d = - pi_v_ + alpha_c_hat_ + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); + pi_v_ + alpha_c_hat_ + + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); - return types::Outputs{psi_d, theta_d}; } - - -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index c0450a62f..3da6deb5a 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -2,9 +2,11 @@ namespace vortex::guidance::los { +// Constructor IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) : m_params{params} {} +// Angle Update void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; @@ -13,30 +15,37 @@ void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { difference.y * difference.y)); rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} +// Cross-Track Error Calculation types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * - rotation_z_.toRotationMatrix().transpose() * - diff_vec; + + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; return types::CrossTrackError::from_vector(path_frame_error); } +// Output Calculation types::Outputs IntegralLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); int_h += cross_track_error.y_e * m_params.time_step; int_v += cross_track_error.z_e * m_params.time_step; - const double u_h = m_params.k_p_h * cross_track_error.y_e + m_params.k_i_h * int_h; - const double u_v = m_params.k_p_v * cross_track_error.z_e + m_params.k_i_v * int_v; + const double u_h = + m_params.k_p_h * cross_track_error.y_e + m_params.k_i_h * int_h; + const double u_v = + m_params.k_p_v * cross_track_error.z_e + m_params.k_i_v * int_v; const double psi_d = pi_h_ - std::atan(u_h); const double theta_d = pi_v_ + std::atan(u_v); @@ -44,4 +53,4 @@ types::Outputs IntegralLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index ae7aeff4e..33aeaa5d6 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -2,10 +2,12 @@ namespace vortex::guidance::los { +// Constructor ProportionalLOSGuidance::ProportionalLOSGuidance( - const ProportionalLosParams& params) : m_params{params} { - } + const ProportionalLosParams& params) + : m_params{params} {} +// Angle Update void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; @@ -17,21 +19,26 @@ void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); } +// Cross-Track Error Calculation types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) const { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * - rotation_z_.toRotationMatrix().transpose() * - diff_vec; + + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; return types::CrossTrackError::from_vector(path_frame_error); } +// Output Calculation types::Outputs ProportionalLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); const double k_p_h = 1.0 / m_params.lookahead_distance_h; const double k_p_v = 1.0 / m_params.lookahead_distance_v; @@ -40,6 +47,6 @@ types::Outputs ProportionalLOSGuidance::calculate_outputs( const double theta_d = pi_v_ + std::atan(k_p_v * cross_track_error.z_e); return types::Outputs{psi_d, theta_d}; -} +} -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp index 52b4f1644..52eb51ead 100644 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -2,9 +2,12 @@ namespace vortex::guidance::los { -VectorFieldLOSGuidance::VectorFieldLOSGuidance(const VectorFieldLosParams& params) +// Constructor +VectorFieldLOSGuidance::VectorFieldLOSGuidance( + const VectorFieldLosParams& params) : m_params{params} {} +// Angle Update void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; @@ -16,34 +19,39 @@ void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); } +// Cross-Track Error Calculation types::CrossTrackError VectorFieldLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) const { const Eigen::Vector3d diff_vec = (inputs.current_position - inputs.prev_point).as_vector(); - const Eigen::Vector3d path_frame_error = rotation_y_.toRotationMatrix().transpose() * - rotation_z_.toRotationMatrix().transpose() * - diff_vec; + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; return types::CrossTrackError::from_vector(path_frame_error); } +// Output Calculation types::Outputs VectorFieldLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); - const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); const double approach_h = - m_params.max_approach_angle_h * (2.0 / M_PI) * std::atan(m_params.k_p_h * cross_track_error.y_e); + m_params.max_approach_angle_h * (2.0 / M_PI) * + std::atan(m_params.k_p_h * cross_track_error.y_e); const double approach_v = - m_params.max_approach_angle_v * (2.0 / M_PI) * std::atan(m_params.k_p_v * cross_track_error.z_e); + m_params.max_approach_angle_v * (2.0 / M_PI) * + std::atan(m_params.k_p_v * cross_track_error.z_e); const double psi_d = pi_h_ - approach_h; - - const double theta_d = pi_v_ + approach_v; + const double theta_d = pi_v_ - approach_v; return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los \ No newline at end of file diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp index 439730a0c..29a19da13 100644 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ b/guidance/los_guidance/src/los_guidance_node.cpp @@ -1,6 +1,8 @@ #include + #include "los_guidance/los_guidance_ros.hpp" +// Main Entry Point int main(int argc, char** argv) { rclcpp::init(argc, argv); @@ -11,4 +13,4 @@ int main(int argc, char** argv) { executor.spin(); return 0; -} +} \ No newline at end of file diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index ba816d4d9..9bc88875e 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,8 +1,10 @@ #include "los_guidance/los_guidance_ros.hpp" + #include #include -#include #include +#include + #include "los_guidance/lib/types.hpp" const auto start_message = R"( @@ -10,18 +12,18 @@ const auto start_message = R"( | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ | | | | | \___ \ | | _| | | | |/ _` |/ _` | '_ \ / __/ _ \ | |__| |_| |___) | | |_| | |_| | | (_| | (_| | | | | (_| __/ - |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| + |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| )"; namespace vortex::guidance::los { +// Constructor LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { double time_step_s = this->declare_parameter("time_step"); time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); - // auto config = this->declare_parameter("los_config_file"); - // Do you need yaml path here? Can't you just use ros params directly from the guidance_params.yaml file? + const std::string yaml_path = this->declare_parameter("los_config_file"); @@ -34,11 +36,12 @@ LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { set_adaptive_los_guidance(config); set_proportional_los_guidance(config); set_integral_los_guidance(config); - set_vector_field_guidance(config); + set_vector_field_guidance(config); spdlog::info(start_message); } +// ROS Setup void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); @@ -50,7 +53,6 @@ void LosGuidanceNode::set_subscribers_and_publisher() { this->get_parameter("topics.guidance.los").as_string(); std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); - std::string odom_topic = this->get_parameter("topics.odom").as_string(); @@ -65,11 +67,11 @@ void LosGuidanceNode::set_subscribers_and_publisher() { state_debug_pub_ = this->create_publisher( "state_debug", qos_sensor_data); - waypoint_sub_ = this->create_subscription< - geometry_msgs::msg::PointStamped>( - waypoint_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::waypoint_callback, this, - std::placeholders::_1)); + waypoint_sub_ = + this->create_subscription( + waypoint_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::waypoint_callback, this, + std::placeholders::_1)); pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( @@ -77,17 +79,17 @@ void LosGuidanceNode::set_subscribers_and_publisher() { std::bind(&LosGuidanceNode::pose_callback, this, std::placeholders::_1)); - odom_sub_ = this->create_subscription< - nav_msgs::msg::Odometry>( + odom_sub_ = this->create_subscription( odom_topic, qos_sensor_data, std::bind(&LosGuidanceNode::odom_callback, this, - std::placeholders::_1)); + std::placeholders::_1)); } void LosGuidanceNode::set_action_server() { this->declare_parameter("action_servers.los"); std::string action_server_name = this->get_parameter("action_servers.los").as_string(); + cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); @@ -113,19 +115,18 @@ void LosGuidanceNode::set_service_server() { std::placeholders::_1, std::placeholders::_2)); } +// Guidance Configuration void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { auto adaptive_los_config = config["adaptive_los"]; auto params = AdaptiveLosParams{}; + params.lookahead_distance_h = adaptive_los_config["lookahead_distance_h"].as(); params.lookahead_distance_v = adaptive_los_config["lookahead_distance_v"].as(); - params.gamma_h = - adaptive_los_config["gamma_h"].as(); - params.gamma_v = - adaptive_los_config["gamma_v"].as(); - params.time_step = - static_cast(time_step_.count()) / 1000.0; + params.gamma_h = adaptive_los_config["gamma_h"].as(); + params.gamma_v = adaptive_los_config["gamma_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; adaptive_los_ = std::make_unique(params); } @@ -133,6 +134,7 @@ void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { auto proportional_los_config = config["prop_los"]; auto params = ProportionalLosParams{}; + params.lookahead_distance_h = proportional_los_config["lookahead_distance_h"].as(); params.lookahead_distance_v = @@ -144,39 +146,34 @@ void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; auto params = IntegralLosParams{}; - params.k_p_h = - integral_los_config["k_p_h"].as(); - params.k_p_v = - integral_los_config["k_p_v"].as(); - params.k_i_h = - integral_los_config["k_i_h"].as(); - params.k_i_v = - integral_los_config["k_i_v"].as(); - params.time_step = - static_cast(time_step_.count()) / 1000.0; + + params.k_p_h = integral_los_config["k_p_h"].as(); + params.k_p_v = integral_los_config["k_p_v"].as(); + params.k_i_h = integral_los_config["k_i_h"].as(); + params.k_i_v = integral_los_config["k_i_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + integral_los_ = std::make_unique(params); } void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { auto vector_field_config = config["vector_field_los"]; auto params = VectorFieldLosParams{}; + params.max_approach_angle_h = vector_field_config["max_approach_angle_h"].as(); params.max_approach_angle_v = vector_field_config["max_approach_angle_v"].as(); - params.k_p_h = - vector_field_config["k_p_h"].as(); - params.k_p_v = - vector_field_config["k_p_v"].as(); - params.time_step = - static_cast(time_step_.count()) / 1000.0; + params.k_p_h = vector_field_config["k_p_h"].as(); + params.k_p_v = vector_field_config["k_p_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; vector_field_los_ = std::make_unique(params); } +// Topic Callbacks void LosGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) -{ + const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { std::lock_guard lock(mutex_); const auto new_wp = types::Point::point_from_ros(wp_msg->point); @@ -190,28 +187,30 @@ void LosGuidanceNode::waypoint_callback( path_inputs_.next_point = new_wp; } - spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, new_wp.z); + spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, + new_wp.z); } - - void LosGuidanceNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose) { std::lock_guard lock(mutex_); path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); } -void LosGuidanceNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { +void LosGuidanceNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { std::lock_guard lock(mutex_); debug_current_odom_ = msg; } - +// Action Server Callbacks rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { (void)goal; + { std::lock_guard lock(mutex_); if (goal_handle_) { @@ -221,6 +220,7 @@ rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( } } } + spdlog::info("Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -240,6 +240,8 @@ void LosGuidanceNode::handle_accepted( goal_handle) { execute(goal_handle); } + +// Service Callback void LosGuidanceNode::set_los_mode( const std::shared_ptr request, std::shared_ptr response) { @@ -247,9 +249,10 @@ void LosGuidanceNode::set_los_mode( spdlog::info("LOS mode set to {}", static_cast(method_)); response->success = true; } + +// Message Helpers vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs) { - vortex_msgs::msg::LOSGuidance reference_msg; reference_msg.pitch = outputs.theta_d; reference_msg.yaw = outputs.psi_d; @@ -271,11 +274,11 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { common_config["active_los_method"].as()); } +// Goal Execution void LosGuidanceNode::execute( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { - { std::lock_guard lock(mutex_); this->goal_handle_ = goal_handle; @@ -355,7 +358,7 @@ void LosGuidanceNode::execute( los_debug_pub_->publish(reference_msg); const auto& v = debug_current_odom_->twist.twist.linear; - double surge = std::sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); vortex_msgs::msg::LOSGuidance state_debug_msg; Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( @@ -368,15 +371,15 @@ void LosGuidanceNode::execute( state_debug_msg.pitch = euler.y(); state_debug_msg.yaw = euler.z(); state_debug_msg.surge = surge; - state_debug_pub_->publish(state_debug_msg); + state_debug_pub_->publish(state_debug_msg); goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); - - if ((path_inputs_.current_position - path_inputs_.next_point).as_vector().norm() < goal_reached_tol_) { - - auto stop_ref = reference_msg; + if ((path_inputs_.current_position - path_inputs_.next_point) + .as_vector() + .norm() < goal_reached_tol_) { + auto stop_ref = reference_msg; stop_ref.surge = 0.0; stop_ref.pitch = 0.0; stop_ref.yaw = reference_msg.yaw; @@ -392,4 +395,4 @@ void LosGuidanceNode::execute( } } -} // namespace vortex::guidance::los +} // namespace vortex::guidance::los \ No newline at end of file From 374230a34a204ab81ef0167c46e52b99ab802b8b Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 15:33:29 +0100 Subject: [PATCH 38/87] ReadMe draft --- guidance/los_guidance/README.md | 92 ++++++++++++++++----------------- 1 file changed, 45 insertions(+), 47 deletions(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 2afe923c1..216bd6df9 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -3,11 +3,7 @@ This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ so a vehicle can follow a path between waypoints. -The vehicle surge speed is kept **constant** during path following and is defined in the configuration file using the parameter - -``` -u_desired -``` +The vehicle surge speed is kept **constant** during path following and is defined in the configuration file using the parameter **u_desired** This value represents the desired forward velocity of the vehicle. @@ -28,6 +24,50 @@ The guidance method can be **changed during runtime** using a ROS service. --- + +# Sending a LOS Goal + +A waypoint can be sent to the guidance node using the action interface: + +``` +ros2 action send_goal /orca/los_guidance \ +vortex_msgs/action/LOSGuidance \ +"{goal: {header: {frame_id: world_ned}, point: {x: 10.0, y: 5.0, z: -2.0}}}" +``` + +This command instructs the guidance node to start following a path toward the waypoint. + +--- + +# Switching LOS Method + +The active LOS guidance method can be changed during runtime. + +``` +ros2 service call /orca/set_los_mode \ +vortex_msgs/srv/SetLosMode "{mode: X}" +``` + +Where + +| X | Method | +|---|---| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +Example: + +``` +ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" +``` + +This switches the guidance system to **Adaptive LOS**. + +--- + + # Adaptive LOS (ALOS) Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. @@ -220,48 +260,6 @@ and --- -# Sending a LOS Goal - -A waypoint can be sent to the guidance node using the action interface: - -``` -ros2 action send_goal /orca/los_guidance \ -vortex_msgs/action/LOSGuidance \ -"{goal: {header: {frame_id: world_ned}, point: {x: 10.0, y: 5.0, z: -2.0}}}" -``` - -This command instructs the guidance node to start following a path toward the waypoint. - ---- - -# Switching LOS Method - -The active LOS guidance method can be changed during runtime. - -``` -ros2 service call /orca/set_los_mode \ -vortex_msgs/srv/SetLosMode "{mode: X}" -``` - -Where - -| X | Method | -|---|---| -| 0 | Proportional LOS | -| 1 | Integral LOS | -| 2 | Adaptive LOS | -| 3 | Vector Field LOS | - -Example: - -``` -ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" -``` - -This switches the guidance system to **Adaptive LOS**. - ---- - # ROS Topics ### Subscribed Topics From 853741d77fdc49e11101edcaa87dee766a3863b1 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 16:36:42 +0100 Subject: [PATCH 39/87] add changes to readMe file --- guidance/los_guidance/README.md | 237 +++++++++++--------------------- 1 file changed, 79 insertions(+), 158 deletions(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 216bd6df9..8bce11d2d 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -1,74 +1,37 @@ -# 3D LOS Guidance Library (Read me draft) +## Path Geometry -This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. -The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ so a vehicle can follow a path between waypoints. +The path between two waypoints defines the **path direction angles** used by the guidance law. -The vehicle surge speed is kept **constant** during path following and is defined in the configuration file using the parameter **u_desired** - -This value represents the desired forward velocity of the vehicle. - ---- - -# Implemented LOS Methods - -The library supports four LOS guidance algorithms. - -| Mode | Method | -|-----|------| -| 0 | Proportional LOS | -| 1 | Integral LOS | -| 2 | Adaptive LOS | -| 3 | Vector Field LOS | - -The guidance method can be **changed during runtime** using a ROS service. - ---- - - -# Sending a LOS Goal - -A waypoint can be sent to the guidance node using the action interface: - -``` -ros2 action send_goal /orca/los_guidance \ -vortex_msgs/action/LOSGuidance \ -"{goal: {header: {frame_id: world_ned}, point: {x: 10.0, y: 5.0, z: -2.0}}}" +```math +\pi_h = +\text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n - x_i^n) ``` -This command instructs the guidance node to start following a path toward the waypoint. - ---- - -# Switching LOS Method - -The active LOS guidance method can be changed during runtime. - -``` -ros2 service call /orca/set_los_mode \ -vortex_msgs/srv/SetLosMode "{mode: X}" +```math +\pi_v = +\text{atan2} +\left( +-(z_{i+1}^n - z_i^n), +\sqrt{(x_{i+1}^n - x_i^n)^2 + +(y_{i+1}^n - y_i^n)^2} +\right) ``` -Where +where -| X | Method | -|---|---| -| 0 | Proportional LOS | -| 1 | Integral LOS | -| 2 | Adaptive LOS | -| 3 | Vector Field LOS | +- $\pi_h$ is the **horizontal path angle (azimuth angle)** +- $\pi_v$ is the **vertical path angle (elevation angle)** -Example: +These angles define the **direction of the current path segment** between two waypoints. -``` -ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" -``` +Additionally -This switches the guidance system to **Adaptive LOS**. +- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down (NED) frame +- $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. --- - -# Adaptive LOS (ALOS) +## Adaptive LOS (ALOS) Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. @@ -99,13 +62,21 @@ z_e^p where -- $\Delta_h$ is the horizontal lookahead distance -- $\Delta_v$ is the vertical lookahead distance -- $\gamma_h$ and $\gamma_v$ are the adaptive gains -- $y_e^p$ is the cross-track error -- $z_e^p$ is the vertical-track error +- $\Delta_h$ is the horizontal lookahead distance +- $\Delta_v$ is the vertical lookahead distance +- $\gamma_h$ and $\gamma_v$ are the adaptive gains +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error + +The terms -Adaptive LOS is generally the **most robust method** and works well for: +- $\hat{\beta}_c$ is the **estimated horizontal crab angle** +- $\hat{\alpha}_c$ is the **estimated vertical crab angle** + +These angles represent the **estimated disturbance-induced deviation** between the vehicle heading and the actual direction of motion. +They allow the guidance system to **compensate for disturbances such as currents or wind**. + +Adaptive LOS is generally the **most robust method** and works well for - curved trajectories - long paths @@ -113,7 +84,7 @@ Adaptive LOS is generally the **most robust method** and works well for: --- -# Proportional LOS (PLOS) +## Proportional LOS (PLOS) Proportional LOS is the simplest LOS guidance law. @@ -129,16 +100,28 @@ Proportional LOS is the simplest LOS guidance law. \tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` -It is best suited for +### Parameters + +- $\Delta_h$ — horizontal lookahead distance +- $\Delta_v$ — vertical lookahead distance -- simple waypoint following -- calm environments with little disturbance. +The lookahead distances determine **how aggressively the vehicle corrects path errors**. -However, steady-state tracking error may occur if disturbances are present. +- small values → aggressive corrections +- large values → smoother but slower convergence + +### Use case + +PLOS works best for + +- simple waypoint following +- environments with minimal disturbances. + +However, it may suffer from **steady-state tracking error** when disturbances are present. --- -# Integral LOS (ILOS) +## Integral LOS (ILOS) Integral LOS adds integral action to remove steady-state error. @@ -166,11 +149,25 @@ k_{i,v}\int z_e^p dt \tan^{-1}(u_v) ``` -Integral LOS performs well when there are **constant disturbances**, such as steady ocean currents or wind. +### Parameters + +- $k_{p,h}$ — horizontal proportional gain +- $k_{p,v}$ — vertical proportional gain +- $k_{i,h}$ — horizontal integral gain +- $k_{i,v}$ — vertical integral gain + +The integral term allows the controller to **eliminate steady-state cross-track errors** caused by constant disturbances. + +### Use case + +ILOS works well when there are **persistent disturbances**, such as + +- steady ocean currents +- constant wind disturbances. --- -# Vector Field LOS (VF-LOS) +## Vector Field LOS (VF-LOS) Vector Field LOS generates a **bounded approach angle** toward the path. @@ -185,95 +182,19 @@ Vector Field LOS generates a **bounded approach angle** toward the path. \tan^{-1}(k_p y_e^p) ``` -This method is best suited for - -- long straight path following -- corridor tracking. - -However it performs worse when the path contains **sharp turns**. - ---- - -# Path Geometry - -The path between two waypoints defines the reference angles used by the guidance law. - -```math -\pi_h = -\text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n - x_i^n) -``` +### Parameters -```math -\pi_v = -\text{atan2} -\left( --(z_{i+1}^n - z_i^n), -\sqrt{(x_{i+1}^n - x_i^n)^2 + -(y_{i+1}^n - y_i^n)^2} -\right) -``` - -where - -- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down frame -- $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. - ---- - -# Path Frame Errors - -The along-, cross- and vertical-track errors in the path-tangential frame are found by - -```math -\begin{bmatrix} -x_e^p \\ -y_e^p \\ -z_e^p -\end{bmatrix} -= -\mathbf{R}_{y,\pi_v}^\top -\mathbf{R}_{z,\pi_h}^\top -\left( -\begin{bmatrix} -x^n \\ -y^n \\ -z^n -\end{bmatrix} -- -\begin{bmatrix} -x_i^n \\ -y_i^n \\ -z_i^n -\end{bmatrix} -\right) -``` - -where - -- $x_e^p$ is the along-track error -- $y_e^p$ is the cross-track error -- $z_e^p$ is the vertical-track error - -and - -- $P^n = (x^n, y^n, z^n)$ is the current position of the vehicle. - ---- +- $\psi_{max}$ — maximum allowed approach angle +- $k_p$ — proportional gain controlling path convergence -# ROS Topics +The bounded approach angle prevents excessively aggressive heading changes. -### Subscribed Topics +### Use case -| Topic | Message | -|------|------| -| `/orca/pose` | `geometry_msgs/PoseWithCovarianceStamped` | -| `/orca/odom` | `nav_msgs/Odometry` | -| `/orca/waypoint` | `geometry_msgs/PointStamped` | +VF-LOS works best for -### Published Topics +- long straight path following +- corridor tracking +- inspection missions along pipelines or cables. -| Topic | Message | -|------|------| -| `/orca/guidance/los` | `vortex_msgs/LOSGuidance` | -| `/los_debug` | `vortex_msgs/LOSGuidance` | -| `/state_debug` | `vortex_msgs/LOSGuidance` | \ No newline at end of file +However it performs worse when the path contains **sharp turns or rapidly changing path segments**. \ No newline at end of file From 2476aceec03520ff2792836cdd1e58336bac9747 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 17:02:36 +0100 Subject: [PATCH 40/87] Add more stuff in readme-file --- guidance/los_guidance/README.md | 180 +++++++++++++++++++++++++++++++- 1 file changed, 179 insertions(+), 1 deletion(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 8bce11d2d..10f796b69 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -1,3 +1,81 @@ +# 3D LOS Guidance Library + +This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. + +The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ that allow a vehicle to follow a path between waypoints. + +The node receives + +- vehicle pose +- waypoint information + +and computes guidance references based on the selected LOS algorithm. The resulting guidance commands consist of + +- desired yaw +- desired pitch +- desired surge velocity. + +The vehicle surge speed is kept **constant during path following** and is defined in the configuration file using the parameter `u_desired`. + +--- + +# Implemented LOS Methods + +The library supports four LOS guidance algorithms. + +| Mode | Method | +|-----|------| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +The guidance method can be **changed during runtime** using a ROS service. + +--- + + +# Sending a LOS Goal + +A waypoint can be sent to the guidance node using the action interface: + +``` +ros2 action send_goal /orca/los_guidance \ +vortex_msgs/action/LOSGuidance \ +"{goal: {header: {frame_id: world_ned}, point: {x: 0.0, y: 0.0, z: 0.0}}}" +``` + +This command instructs the guidance node to start following a path toward the waypoint. + +--- + +# Switching LOS Method + +The active LOS guidance method can be changed during runtime. + +``` +ros2 service call /orca/set_los_mode \ +vortex_msgs/srv/SetLosMode "{mode: X}" +``` + +Where + +| X | Method | +|---|---| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +Example: + +``` +ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" +``` + +This switches the guidance system to **Adaptive LOS**. + +--- ## Path Geometry The path between two waypoints defines the **path direction angles** used by the guidance law. @@ -31,6 +109,46 @@ Additionally --- +## Path Frame Errors + +The along-, cross- and vertical-track errors in the path-tangential frame are found by + +```math +\begin{bmatrix} +x_e^p \\ +y_e^p \\ +z_e^p +\end{bmatrix} += +\mathbf{R}_{y,\pi_v}^\top +\mathbf{R}_{z,\pi_h}^\top +\left( +\begin{bmatrix} +x^n \\ +y^n \\ +z^n +\end{bmatrix} +- +\begin{bmatrix} +x_i^n \\ +y_i^n \\ +z_i^n +\end{bmatrix} +\right) +``` + +where + +- $x_e^p$ is the along-track error +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error + +and + +- $P^n = (x^n, y^n, z^n)$ is the current position of the vehicle. + +--- + ## Adaptive LOS (ALOS) Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. @@ -197,4 +315,64 @@ VF-LOS works best for - corridor tracking - inspection missions along pipelines or cables. -However it performs worse when the path contains **sharp turns or rapidly changing path segments**. \ No newline at end of file +However it performs worse when the path contains **sharp turns or rapidly changing path segments**. + +--- + +## ROS Interfaces + +| Interface | Name | Type | Message-Type | +|----------|------|------|---------| +| Action Server | `/orca/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | +| Subscriber | `/orca/waypoint` | Waypoint input | `geometry_msgs/PointStamped` | +| Subscriber | `/orca/pose` | Vehicle pose | `geometry_msgs/PoseWithCovarianceStamped` | +| Subscriber | `/orca/odom` | Vehicle velocity | `nav_msgs/Odometry` | +| Publisher | `/orca/guidance/los` | Guidance reference (yaw, pitch, surge) | `vortex_msgs/LOSGuidance` | +| Publisher | `/los_debug` | LOS debug output | `vortex_msgs/LOSGuidance` | +| Publisher | `/state_debug` | Vehicle state debug | `vortex_msgs/LOSGuidance` | + +--- + +## Guidance Node Architecture +The LOS guidance node computes reference commands for the vehicle based on +the current vehicle state and the active waypoint. + +The process works as follows + +1. The node receives the **vehicle pose**. +2. The current **path segment** is defined between two waypoints. +3. The **path geometry** is computed to determine the path direction. +4. The **cross-track and vertical-track errors** are calculated in the path frame. +5. The selected **LOS guidance algorithm** computes desired yaw and pitch. +6. The node publishes the resulting **guidance reference**. + +### Data Flow + +``` +Vehicle Pose + Waypoint + │ + ▼ + Path Geometry + (π_h , π_v angles) + │ + ▼ + Path Frame Errors + (x_e^p , y_e^p , z_e^p) + │ + ▼ + LOS Algorithm + (PLOS / ILOS / ALOS / VF-LOS) + │ + ▼ + Guidance Output + (ψ_d , θ_d , u_desired) +``` + +The resulting guidance command is published as a `vortex_msgs/LOSGuidance` +message containing + +- desired yaw +- desired pitch +- desired surge velocity. + +--- \ No newline at end of file From 0f4094af8d0dcf9a2f601343f4d70645ffe8119b Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 17:08:22 +0100 Subject: [PATCH 41/87] fix pre-commit issues --- guidance/los_guidance/CMakeLists.txt | 1 - guidance/los_guidance/README.md | 60 +++++++++---------- .../los_guidance/config/guidance_params.yaml | 2 +- .../include/los_guidance/lib/adaptive_los.hpp | 4 +- .../include/los_guidance/lib/integral_los.hpp | 3 +- .../los_guidance/lib/proportional_los.hpp | 4 +- .../include/los_guidance/lib/types.hpp | 13 +--- .../los_guidance/lib/vector_field_los.hpp | 4 +- .../include/los_guidance/los_guidance_ros.hpp | 21 +++---- .../launch/guidance_test.launch.py | 55 ++++++++++------- .../launch/los_guidance.launch.py | 2 - guidance/los_guidance/scripts/square_test.py | 25 ++------ .../los_guidance/src/lib/adaptive_los.cpp | 14 ++--- .../los_guidance/src/lib/integral_los.cpp | 2 +- .../los_guidance/src/lib/proportional_los.cpp | 2 +- .../los_guidance/src/lib/vector_field_los.cpp | 12 ++-- .../los_guidance/src/los_guidance_node.cpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 25 ++++---- .../test/vector_field_los_test.cpp | 6 +- 19 files changed, 114 insertions(+), 143 deletions(-) diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index a943a4f24..d6bdb26bf 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -96,4 +96,3 @@ if(BUILD_TESTING) endif() ament_package() - diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 10f796b69..af54fdb90 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -97,14 +97,14 @@ The path between two waypoints defines the **path direction angles** used by the where -- $\pi_h$ is the **horizontal path angle (azimuth angle)** -- $\pi_v$ is the **vertical path angle (elevation angle)** +- $\pi_h$ is the **horizontal path angle (azimuth angle)** +- $\pi_v$ is the **vertical path angle (elevation angle)** These angles define the **direction of the current path segment** between two waypoints. Additionally -- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down (NED) frame +- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down (NED) frame - $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. --- @@ -139,9 +139,9 @@ z_i^n where -- $x_e^p$ is the along-track error -- $y_e^p$ is the cross-track error -- $z_e^p$ is the vertical-track error +- $x_e^p$ is the along-track error +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error and @@ -149,7 +149,7 @@ and --- -## Adaptive LOS (ALOS) +## Adaptive LOS (ALSO) Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. @@ -180,24 +180,24 @@ z_e^p where -- $\Delta_h$ is the horizontal lookahead distance -- $\Delta_v$ is the vertical lookahead distance -- $\gamma_h$ and $\gamma_v$ are the adaptive gains -- $y_e^p$ is the cross-track error -- $z_e^p$ is the vertical-track error +- $\Delta_h$ is the horizontal lookahead distance +- $\Delta_v$ is the vertical lookahead distance +- $\gamma_h$ and $\gamma_v$ are the adaptive gains +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error The terms - $\hat{\beta}_c$ is the **estimated horizontal crab angle** - $\hat{\alpha}_c$ is the **estimated vertical crab angle** -These angles represent the **estimated disturbance-induced deviation** between the vehicle heading and the actual direction of motion. +These angles represent the **estimated disturbance-induced deviation** between the vehicle heading and the actual direction of motion. They allow the guidance system to **compensate for disturbances such as currents or wind**. Adaptive LOS is generally the **most robust method** and works well for -- curved trajectories -- long paths +- curved trajectories +- long paths - environments with disturbances. --- @@ -220,19 +220,19 @@ Proportional LOS is the simplest LOS guidance law. ### Parameters -- $\Delta_h$ — horizontal lookahead distance -- $\Delta_v$ — vertical lookahead distance +- $\Delta_h$ — horizontal lookahead distance +- $\Delta_v$ — vertical lookahead distance The lookahead distances determine **how aggressively the vehicle corrects path errors**. -- small values → aggressive corrections +- small values → aggressive corrections - large values → smoother but slower convergence ### Use case PLOS works best for -- simple waypoint following +- simple waypoint following - environments with minimal disturbances. However, it may suffer from **steady-state tracking error** when disturbances are present. @@ -269,10 +269,10 @@ k_{i,v}\int z_e^p dt ### Parameters -- $k_{p,h}$ — horizontal proportional gain -- $k_{p,v}$ — vertical proportional gain -- $k_{i,h}$ — horizontal integral gain -- $k_{i,v}$ — vertical integral gain +- $k_{p,h}$ — horizontal proportional gain +- $k_{p,v}$ — vertical proportional gain +- $k_{i,h}$ — horizontal integral gain +- $k_{i,v}$ — vertical integral gain The integral term allows the controller to **eliminate steady-state cross-track errors** caused by constant disturbances. @@ -280,7 +280,7 @@ The integral term allows the controller to **eliminate steady-state cross-track ILOS works well when there are **persistent disturbances**, such as -- steady ocean currents +- steady ocean currents - constant wind disturbances. --- @@ -302,7 +302,7 @@ Vector Field LOS generates a **bounded approach angle** toward the path. ### Parameters -- $\psi_{max}$ — maximum allowed approach angle +- $\psi_{max}$ — maximum allowed approach angle - $k_p$ — proportional gain controlling path convergence The bounded approach angle prevents excessively aggressive heading changes. @@ -311,8 +311,8 @@ The bounded approach angle prevents excessively aggressive heading changes. VF-LOS works best for -- long straight path following -- corridor tracking +- long straight path following +- corridor tracking - inspection missions along pipelines or cables. However it performs worse when the path contains **sharp turns or rapidly changing path segments**. @@ -339,7 +339,7 @@ the current vehicle state and the active waypoint. The process works as follows -1. The node receives the **vehicle pose**. +1. The node receives the **vehicle pose**. 2. The current **path segment** is defined between two waypoints. 3. The **path geometry** is computed to determine the path direction. 4. The **cross-track and vertical-track errors** are calculated in the path frame. @@ -361,7 +361,7 @@ Vehicle Pose + Waypoint │ ▼ LOS Algorithm - (PLOS / ILOS / ALOS / VF-LOS) + (PLOS / ILOS / ALSO / VF-LOS) │ ▼ Guidance Output @@ -375,4 +375,4 @@ message containing - desired pitch - desired surge velocity. ---- \ No newline at end of file +--- diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 8ba560c1a..35ac01df6 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -33,4 +33,4 @@ common: # Debug Settings debug: enable_debug: true - debug_topic_name: "/los_guidance_debug" \ No newline at end of file + debug_topic_name: "/los_guidance_debug" diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index f7174a89b..f63bdfe57 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -21,7 +21,6 @@ struct AdaptiveLosParams { // Adaptive LOS Guidance Class class AdaptiveLOSGuidance { public: - // Constructor / Destructor AdaptiveLOSGuidance(const AdaptiveLosParams& params); ~AdaptiveLOSGuidance() = default; @@ -30,7 +29,6 @@ class AdaptiveLOSGuidance { types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions void update_angles(const types::Inputs& inputs); const types::CrossTrackError calculate_crosstrack_error( @@ -56,4 +54,4 @@ class AdaptiveLOSGuidance { } // namespace vortex::guidance::los -#endif // ADAPTIVE_LOS_GUIDANCE_HPP \ No newline at end of file +#endif // ADAPTIVE_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index 83a528461..a8983d8a3 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -21,7 +21,6 @@ struct IntegralLosParams { // Integral LOS Guidance Class class IntegralLOSGuidance { public: - // Constructor / Destructor IntegralLOSGuidance(const IntegralLosParams& params); ~IntegralLOSGuidance() = default; @@ -53,4 +52,4 @@ class IntegralLOSGuidance { } // namespace vortex::guidance::los -#endif // INTEGRAL_LOS_GUIDANCE_HPP \ No newline at end of file +#endif // INTEGRAL_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index a56a5efae..7290839e6 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -18,7 +18,6 @@ struct ProportionalLosParams { // Proportional LOS Guidance Class class ProportionalLOSGuidance { public: - // Constructor / Destructor ProportionalLOSGuidance(const ProportionalLosParams& params); ~ProportionalLOSGuidance() = default; @@ -27,7 +26,6 @@ class ProportionalLOSGuidance { types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions void update_angles(const types::Inputs& inputs); types::CrossTrackError calculate_crosstrack_error( @@ -47,4 +45,4 @@ class ProportionalLOSGuidance { } // namespace vortex::guidance::los -#endif // PROPORTIONAL_LOS_GUIDANCE_HPP \ No newline at end of file +#endif // PROPORTIONAL_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index a12aa5078..6de4e0360 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -21,9 +21,7 @@ struct Point { } // Conversion Functions - Eigen::Vector3d as_vector() const { - return Eigen::Vector3d(x, y, z); - } + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } static Point point_from_ros(const geometry_msgs::msg::Point& msg) { return Point{msg.x, msg.y, msg.z}; @@ -55,13 +53,8 @@ struct Inputs { }; // Active LOS Method -enum class ActiveLosMethod { - PROPORTIONAL, - INTEGRAL, - ADAPTIVE, - VECTOR_FIELD -}; +enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; } // namespace vortex::guidance::los::types -#endif // TYPES_HPP \ No newline at end of file +#endif // TYPES_HPP diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index 3348b0f74..545013203 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -21,7 +21,6 @@ struct VectorFieldLosParams { // Vector Field LOS Guidance Class class VectorFieldLOSGuidance { public: - // Constructor / Destructor VectorFieldLOSGuidance(const VectorFieldLosParams& params); ~VectorFieldLOSGuidance() = default; @@ -30,7 +29,6 @@ class VectorFieldLOSGuidance { types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions void update_angles(const types::Inputs& inputs); types::CrossTrackError calculate_crosstrack_error( @@ -50,4 +48,4 @@ class VectorFieldLOSGuidance { } // namespace vortex::guidance::los -#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP \ No newline at end of file +#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index e7bbc1482..73ebc7b99 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,6 +1,7 @@ #ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#include #include #include #include @@ -13,7 +14,6 @@ #include #include #include -#include #include #include @@ -29,12 +29,10 @@ namespace vortex::guidance::los { // LOS Guidance ROS Node class LosGuidanceNode : public rclcpp::Node { public: - // Constructor LosGuidanceNode(); private: - // Setup Functions void set_subscribers_and_publisher(); void set_action_server(); @@ -53,8 +51,7 @@ class LosGuidanceNode : public rclcpp::Node { const geometry_msgs::msg::PointStamped::SharedPtr msg); void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - void odom_callback( - const nav_msgs::msg::Odometry::SharedPtr msg); + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); // Action Server Functions rclcpp_action::GoalResponse handle_goal( @@ -64,14 +61,10 @@ class LosGuidanceNode : public rclcpp::Node { const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle); - void handle_accepted( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle); - void execute( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle); + void handle_accepted(const std::shared_ptr> goal_handle); + void execute(const std::shared_ptr> goal_handle); // Service Functions void set_los_mode( @@ -131,4 +124,4 @@ class LosGuidanceNode : public rclcpp::Node { } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE_ROS_HPP \ No newline at end of file +#endif // LOS_GUIDANCE_ROS_HPP diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 2bc63f2f9..8dd953671 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -1,9 +1,14 @@ -from yaml import Node +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction, ExecuteProcess, SetEnvironmentVariable +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + TimerAction, +) from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory -import os + def generate_launch_description(): stonefish_dir = get_package_share_directory('stonefish_sim') @@ -23,7 +28,9 @@ def generate_launch_description(): vortex_sim_interface = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(vortex_sim_interface_dir, 'launch', 'vortex_sim_interface.launch.py') + os.path.join( + vortex_sim_interface_dir, 'launch', 'vortex_sim_interface.launch.py' + ) ) ) @@ -35,7 +42,9 @@ def generate_launch_description(): velocity_controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py') + os.path.join( + velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py' + ) ) ) @@ -47,17 +56,18 @@ def generate_launch_description(): os.path.join(stonefish_dir, 'launch', 'orca_sim.launch.py') ) ) - ] + ], ) set_autonomy = TimerAction( - period=12.0, + period=12.0, actions=[ ExecuteProcess( cmd=[ - "bash", "-lc", + "bash", + "-lc", "ros2 topic pub --once /orca/killswitch std_msgs/msg/Bool \"{data: false}\"; " - "ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; " + "ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; ", ], output="screen", ), @@ -69,20 +79,23 @@ def generate_launch_description(): actions=[ ExecuteProcess( cmd=[ - "bash", "-lc", - f"python3 {os.path.join(los_guidance_dir, 'scripts', 'square_test.py')}" + "bash", + "-lc", + f"python3 {os.path.join(los_guidance_dir, 'scripts', 'square_test.py')}", ], output="screen", ) ], ) - return LaunchDescription([ - stonefish_sim, - vortex_sim_interface, - los_guidance_launch, - velocity_controller_launch, - orca_sim, - set_autonomy, - square_test, - ]) \ No newline at end of file + return LaunchDescription( + [ + stonefish_sim, + vortex_sim_interface, + los_guidance_launch, + velocity_controller_launch, + orca_sim, + set_autonomy, + square_test, + ] + ) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 2a677d5c8..f726983f0 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -31,5 +31,3 @@ def generate_launch_description(): output="screen", ) return LaunchDescription([los_guidance_node]) - - diff --git a/guidance/los_guidance/scripts/square_test.py b/guidance/los_guidance/scripts/square_test.py index 8fe518e9d..954530849 100644 --- a/guidance/los_guidance/scripts/square_test.py +++ b/guidance/los_guidance/scripts/square_test.py @@ -1,27 +1,19 @@ import rclpy -from rclpy.node import Node from rclpy.action import ActionClient - -from vortex_msgs.action import LOSGuidance -from geometry_msgs.msg import Point +from rclpy.node import Node from std_msgs.msg import Header -from launch.actions import LogInfo +from vortex_msgs.action import LOSGuidance class SquareTest(Node): - def __init__(self): super().__init__('square_test_client') self.get_logger().info("Square test started") - self._action_client = ActionClient( - self, - LOSGuidance, - '/orca/los_guidance' - ) + self._action_client = ActionClient(self, LOSGuidance, '/orca/los_guidance') - self.depth = 2.5 + self.depth = 2.5 self.size = 10.0 self.waypoints = [ @@ -36,7 +28,6 @@ def __init__(self): self.send_next_goal() def send_next_goal(self): - if self.current_index >= len(self.waypoints): self.get_logger().info("Square test completed!") rclpy.shutdown() @@ -58,19 +49,16 @@ def send_next_goal(self): goal_msg.goal.point.z = z self.get_logger().info( - f"Sending waypoint {self.current_index + 1}: " - f"x={x}, y={y}, z={z}" + f"Sending waypoint {self.current_index + 1}: x={x}, y={y}, z={z}" ) self._send_goal_future = self._action_client.send_goal_async( - goal_msg, - feedback_callback=self.feedback_callback + goal_msg, feedback_callback=self.feedback_callback ) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): - goal_handle = future.result() if not goal_handle.accepted: @@ -83,7 +71,6 @@ def goal_response_callback(self, future): self._get_result_future.add_done_callback(self.result_callback) def result_callback(self, future): - self.get_logger().info("Waypoint reached") self.current_index += 1 diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index a4acee79f..db6177b1c 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -43,12 +43,12 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( std::sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + cross_track_error.z_e * cross_track_error.z_e); - const double beta_dot = - params_.gamma_h * (params_.lookahead_distance_h / denom_h) * - cross_track_error.y_e; - const double alpha_dot = - params_.gamma_v * (params_.lookahead_distance_v / denom_v) * - cross_track_error.z_e; + const double beta_dot = params_.gamma_h * + (params_.lookahead_distance_h / denom_h) * + cross_track_error.y_e; + const double alpha_dot = params_.gamma_v * + (params_.lookahead_distance_v / denom_v) * + cross_track_error.z_e; beta_c_hat_ += beta_dot * params_.time_step; alpha_c_hat_ += alpha_dot * params_.time_step; @@ -75,4 +75,4 @@ types::Outputs AdaptiveLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index 3da6deb5a..c8f0b4d59 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -53,4 +53,4 @@ types::Outputs IntegralLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index 33aeaa5d6..f76df385c 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -49,4 +49,4 @@ types::Outputs ProportionalLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp index 52eb51ead..edf352a6e 100644 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -40,13 +40,11 @@ types::Outputs VectorFieldLOSGuidance::calculate_outputs( const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); - const double approach_h = - m_params.max_approach_angle_h * (2.0 / M_PI) * - std::atan(m_params.k_p_h * cross_track_error.y_e); + const double approach_h = m_params.max_approach_angle_h * (2.0 / M_PI) * + std::atan(m_params.k_p_h * cross_track_error.y_e); - const double approach_v = - m_params.max_approach_angle_v * (2.0 / M_PI) * - std::atan(m_params.k_p_v * cross_track_error.z_e); + const double approach_v = m_params.max_approach_angle_v * (2.0 / M_PI) * + std::atan(m_params.k_p_v * cross_track_error.z_e); const double psi_d = pi_h_ - approach_h; const double theta_d = pi_v_ - approach_v; @@ -54,4 +52,4 @@ types::Outputs VectorFieldLOSGuidance::calculate_outputs( return types::Outputs{psi_d, theta_d}; } -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp index 29a19da13..31ad2eb74 100644 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ b/guidance/los_guidance/src/los_guidance_node.cpp @@ -13,4 +13,4 @@ int main(int argc, char** argv) { executor.spin(); return 0; -} \ No newline at end of file +} diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 9bc88875e..40210dae6 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -2,8 +2,8 @@ #include #include -#include #include +#include #include "los_guidance/lib/types.hpp" @@ -53,8 +53,7 @@ void LosGuidanceNode::set_subscribers_and_publisher() { this->get_parameter("topics.guidance.los").as_string(); std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); - std::string odom_topic = - this->get_parameter("topics.odom").as_string(); + std::string odom_topic = this->get_parameter("topics.odom").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); @@ -67,11 +66,10 @@ void LosGuidanceNode::set_subscribers_and_publisher() { state_debug_pub_ = this->create_publisher( "state_debug", qos_sensor_data); - waypoint_sub_ = - this->create_subscription( - waypoint_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::waypoint_callback, this, - std::placeholders::_1)); + waypoint_sub_ = this->create_subscription( + waypoint_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::waypoint_callback, this, + std::placeholders::_1)); pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( @@ -362,11 +360,10 @@ void LosGuidanceNode::execute( vortex_msgs::msg::LOSGuidance state_debug_msg; Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( - Eigen::Quaterniond( - debug_current_odom_->pose.pose.orientation.w, - debug_current_odom_->pose.pose.orientation.x, - debug_current_odom_->pose.pose.orientation.y, - debug_current_odom_->pose.pose.orientation.z)); + Eigen::Quaterniond(debug_current_odom_->pose.pose.orientation.w, + debug_current_odom_->pose.pose.orientation.x, + debug_current_odom_->pose.pose.orientation.y, + debug_current_odom_->pose.pose.orientation.z)); state_debug_msg.pitch = euler.y(); state_debug_msg.yaw = euler.z(); @@ -395,4 +392,4 @@ void LosGuidanceNode::execute( } } -} // namespace vortex::guidance::los \ No newline at end of file +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp index 527b33694..956e9b5f0 100644 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -1,6 +1,6 @@ -#include "los_guidance/lib/proportional_los.hpp" #include "los_guidance/lib/vector_field_los.hpp" #include +#include "los_guidance/lib/proportional_los.hpp" namespace vortex::guidance::los { @@ -12,8 +12,8 @@ class VectorFieldLosTest : public ::testing::Test { VectorFieldLosParams params; params.max_approach_angle_h = 30.0 * M_PI / 180.0; // 30 degrees in rad params.max_approach_angle_v = 20.0 * M_PI / 180.0; // 20 degrees in rad - params.k_p_h = 0.1; // needs tuning - params.k_p_v = 0.1; // needs tuning + params.k_p_h = 0.1; // needs tuning + params.k_p_v = 0.1; // needs tuning params.time_step = 0.01; return params; } From a8bf5b4c1f6c15122c4157926ee6526468b4ece2 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 17:28:32 +0100 Subject: [PATCH 42/87] Fix pre-commit header issues --- .../include/los_guidance/lib/adaptive_los.hpp | 8 ++++---- .../include/los_guidance/lib/integral_los.hpp | 8 ++++---- .../include/los_guidance/lib/proportional_los.hpp | 8 ++++---- guidance/los_guidance/include/los_guidance/lib/types.hpp | 6 +++--- .../include/los_guidance/lib/vector_field_los.hpp | 8 ++++---- .../include/los_guidance/los_guidance_ros.hpp | 2 +- 6 files changed, 20 insertions(+), 20 deletions(-) diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index f63bdfe57..1f9784178 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -1,5 +1,5 @@ -#ifndef ADAPTIVE_LOS_GUIDANCE_HPP -#define ADAPTIVE_LOS_GUIDANCE_HPP +#ifndef LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ +#define LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ #include #include @@ -22,7 +22,7 @@ struct AdaptiveLosParams { class AdaptiveLOSGuidance { public: // Constructor / Destructor - AdaptiveLOSGuidance(const AdaptiveLosParams& params); + explicit AdaptiveLOSGuidance(const AdaptiveLosParams& params); ~AdaptiveLOSGuidance() = default; // Main Output Calculation @@ -54,4 +54,4 @@ class AdaptiveLOSGuidance { } // namespace vortex::guidance::los -#endif // ADAPTIVE_LOS_GUIDANCE_HPP +#endif // LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index a8983d8a3..bde7d0d5f 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -1,5 +1,5 @@ -#ifndef INTEGRAL_LOS_GUIDANCE_HPP -#define INTEGRAL_LOS_GUIDANCE_HPP +#ifndef LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ +#define LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ #include #include @@ -22,7 +22,7 @@ struct IntegralLosParams { class IntegralLOSGuidance { public: // Constructor / Destructor - IntegralLOSGuidance(const IntegralLosParams& params); + explicit IntegralLOSGuidance(const IntegralLosParams& params); ~IntegralLOSGuidance() = default; // Main Output Calculation @@ -52,4 +52,4 @@ class IntegralLOSGuidance { } // namespace vortex::guidance::los -#endif // INTEGRAL_LOS_GUIDANCE_HPP +#endif // LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index 7290839e6..a2e9663e1 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -1,5 +1,5 @@ -#ifndef PROPORTIONAL_LOS_GUIDANCE_HPP -#define PROPORTIONAL_LOS_GUIDANCE_HPP +#ifndef LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ +#define LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ #include #include @@ -19,7 +19,7 @@ struct ProportionalLosParams { class ProportionalLOSGuidance { public: // Constructor / Destructor - ProportionalLOSGuidance(const ProportionalLosParams& params); + explicit ProportionalLOSGuidance(const ProportionalLosParams& params); ~ProportionalLOSGuidance() = default; // Main Output Calculation @@ -45,4 +45,4 @@ class ProportionalLOSGuidance { } // namespace vortex::guidance::los -#endif // PROPORTIONAL_LOS_GUIDANCE_HPP +#endif // LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index 6de4e0360..52aab3d48 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -1,5 +1,5 @@ -#ifndef TYPES_HPP -#define TYPES_HPP +#ifndef LOS_GUIDANCE__LIB__TYPES_HPP_ +#define LOS_GUIDANCE__LIB__TYPES_HPP_ #include #include @@ -57,4 +57,4 @@ enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; } // namespace vortex::guidance::los::types -#endif // TYPES_HPP +#endif // LOS_GUIDANCE__LIB__TYPES_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index 545013203..60d332929 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -1,5 +1,5 @@ -#ifndef VECTOR_FIELD_LOS_GUIDANCE_HPP -#define VECTOR_FIELD_LOS_GUIDANCE_HPP +#ifndef LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ +#define LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ #include #include @@ -22,7 +22,7 @@ struct VectorFieldLosParams { class VectorFieldLOSGuidance { public: // Constructor / Destructor - VectorFieldLOSGuidance(const VectorFieldLosParams& params); + explicit VectorFieldLOSGuidance(const VectorFieldLosParams& params); ~VectorFieldLOSGuidance() = default; // Main Output Calculation @@ -48,4 +48,4 @@ class VectorFieldLOSGuidance { } // namespace vortex::guidance::los -#endif // VECTOR_FIELD_LOS_GUIDANCE_HPP +#endif // LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 73ebc7b99..54ff2e24c 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -124,4 +124,4 @@ class LosGuidanceNode : public rclcpp::Node { } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE_ROS_HPP +#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ From bdb00db5e17a3a17ced383dc86561d06fadcb212 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 8 Mar 2026 17:48:41 +0100 Subject: [PATCH 43/87] fix build error --- auv_setup/config/robots/orca.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 302e414e4..938cb1bfc 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -126,7 +126,6 @@ killswitch: "killswitch" reference_pose: "reference_pose" landmarks: "landmarks" - odom: "odom" waypoint: "waypoint" waypoint_list: "waypoint_list" guidance: From c8a1da7a6b364130ab6aef4ce87b99a23d9200c0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 9 Mar 2026 14:20:35 +0100 Subject: [PATCH 44/87] Fix issues with new operation_manager --- .../los_guidance/config/guidance_params.yaml | 4 ++-- .../launch/guidance_test.launch.py | 24 ++++++++++++++++--- 2 files changed, 23 insertions(+), 5 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 35ac01df6..63ab43b8f 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -26,8 +26,8 @@ vector_field_los: # Common Guidance Parameters common: - active_los_method: 2 - u_desired: 0.3 + active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS + u_desired: 0.3 goal_reached_tol: 0.35 # Debug Settings diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 8dd953671..03d404982 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): ), launch_arguments={ 'scenario': 'tacc', - 'rendering': 'true', + 'rendering': 'false', }.items(), ) @@ -34,6 +34,16 @@ def generate_launch_description(): ) ) + operation_mode_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("operation_mode_manager"), + "launch", + "operation_mode_manager.launch.py", + ) + ) +) + los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(los_guidance_dir, 'launch', 'los_guidance.launch.py') @@ -66,8 +76,15 @@ def generate_launch_description(): cmd=[ "bash", "-lc", - "ros2 topic pub --once /orca/killswitch std_msgs/msg/Bool \"{data: false}\"; " - "ros2 topic pub --once /orca/operation_mode std_msgs/msg/String \"{data: 'autonomous mode'}\"; ", + ( + "ros2 service call /orca/set_killswitch " + "vortex_msgs/srv/SetKillswitch " + "\"{killswitch_on: false}\" " + "&& " + "ros2 service call /orca/set_operation_mode " + "vortex_msgs/srv/SetOperationMode " + "\"{requested_operation_mode: {operation_mode: 1}}\"" + ), ], output="screen", ), @@ -92,6 +109,7 @@ def generate_launch_description(): [ stonefish_sim, vortex_sim_interface, + operation_mode_launch, los_guidance_launch, velocity_controller_launch, orca_sim, From a2927addaf9cfa7a3dbf1b34e80bae64dc0b235d Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 9 Mar 2026 15:06:08 +0100 Subject: [PATCH 45/87] Add clamp for pitch --- guidance/los_guidance/config/guidance_params.yaml | 3 ++- .../include/los_guidance/los_guidance_ros.hpp | 1 + guidance/los_guidance/launch/guidance_test.launch.py | 12 ++++++------ guidance/los_guidance/src/los_guidance_ros.cpp | 5 ++++- 4 files changed, 13 insertions(+), 8 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 63ab43b8f..6dd71b6a4 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -27,8 +27,9 @@ vector_field_los: # Common Guidance Parameters common: active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS - u_desired: 0.3 + u_desired: 0.3 goal_reached_tol: 0.35 + max_pitch_angle: 0.785 # 45 degrees # Debug Settings debug: diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 54ff2e24c..39e4edca8 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -110,6 +110,7 @@ class LosGuidanceNode : public rclcpp::Node { types::Inputs path_inputs_{}; double u_desired_{}; double goal_reached_tol_{}; + double max_pitch_angle_{}; types::ActiveLosMethod method_{}; // Guidance Modules diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 03d404982..38a50e97c 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -35,14 +35,14 @@ def generate_launch_description(): ) operation_mode_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory("operation_mode_manager"), - "launch", - "operation_mode_manager.launch.py", + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("operation_mode_manager"), + "launch", + "operation_mode_manager.launch.py", + ) ) ) -) los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 40210dae6..372152025 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -252,7 +252,9 @@ void LosGuidanceNode::set_los_mode( vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs) { vortex_msgs::msg::LOSGuidance reference_msg; - reference_msg.pitch = outputs.theta_d; + const double clamped_pitch = + std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); + reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; reference_msg.surge = u_desired_; @@ -267,6 +269,7 @@ YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { void LosGuidanceNode::parse_common_config(YAML::Node common_config) { std::lock_guard lock(mutex_); u_desired_ = common_config["u_desired"].as(); + max_pitch_angle_ = common_config["max_pitch_angle"].as(); goal_reached_tol_ = common_config["goal_reached_tol"].as(); method_ = static_cast( common_config["active_los_method"].as()); From 7f7fdded7ae295fa87014103ff623ef5f37c4aef Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 9 Mar 2026 15:17:11 +0100 Subject: [PATCH 46/87] Fix some duplicate issues --- guidance/los_guidance/launch/guidance_test.launch.py | 4 ++-- guidance/los_guidance/src/los_guidance_ros.cpp | 11 +++++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 38a50e97c..3a85dfc7d 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -21,8 +21,8 @@ def generate_launch_description(): os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), launch_arguments={ - 'scenario': 'tacc', - 'rendering': 'false', + 'scenario': 'default', + 'rendering': 'true', }.items(), ) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 372152025..de851d950 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -334,16 +334,16 @@ void LosGuidanceNode::execute( switch (method_) { case types::ActiveLosMethod::ADAPTIVE: - outputs = adaptive_los_->calculate_outputs(path_inputs_); + outputs = adaptive_los_->calculate_outputs(inputs_copy); break; case types::ActiveLosMethod::PROPORTIONAL: - outputs = proportional_los_->calculate_outputs(path_inputs_); + outputs = proportional_los_->calculate_outputs(inputs_copy); break; case types::ActiveLosMethod::INTEGRAL: - outputs = integral_los_->calculate_outputs(path_inputs_); + outputs = integral_los_->calculate_outputs(inputs_copy); break; case types::ActiveLosMethod::VECTOR_FIELD: - outputs = vector_field_los_->calculate_outputs(path_inputs_); + outputs = vector_field_los_->calculate_outputs(inputs_copy); break; default: spdlog::error("Invalid LOS method selected"); @@ -375,8 +375,7 @@ void LosGuidanceNode::execute( state_debug_pub_->publish(state_debug_msg); goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); - - if ((path_inputs_.current_position - path_inputs_.next_point) + if ((inputs_copy.current_position - inputs_copy.next_point) .as_vector() .norm() < goal_reached_tol_) { auto stop_ref = reference_msg; From f64e566ebe06a60fc58fc9d1c63c31b447f338ec Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 9 Mar 2026 15:44:40 +0100 Subject: [PATCH 47/87] Fix max_pitch to 55 degres --- guidance/los_guidance/config/guidance_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 6dd71b6a4..0914cd9ff 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -29,7 +29,7 @@ common: active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS u_desired: 0.3 goal_reached_tol: 0.35 - max_pitch_angle: 0.785 # 45 degrees + max_pitch_angle: 0.96 # 55 degrees # Debug Settings debug: From 7a39ece3d4571a45643e6505dbe83a65d64400ec Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 12 Mar 2026 16:09:42 +0100 Subject: [PATCH 48/87] Fix: merge issiue --- .../launch/los_guidance.launch.py | 62 ++++++++++++------- 1 file changed, 40 insertions(+), 22 deletions(-) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index bf653b641..5ee9a3d1d 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -5,29 +5,47 @@ from launch.actions import OpaqueFunction from launch_ros.actions import Node -adapt_params = path.join( - get_package_share_directory("los_guidance"), - "config", - "guidance_params.yaml", -) -orca_params = path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - "orca.yaml", +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, ) -def generate_launch_description(): - los_guidance_node = Node( - package="los_guidance", - executable="los_guidance_node", - name="los_guidance_node", - namespace="orca", - parameters=[ - orca_params, - adapt_params, - ], - output="screen", +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + guidance_config = os.path.join( + get_package_share_directory("los_guidance"), + "config", + "guidance_params.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", ) - return LaunchDescription([los_guidance_node]) + + return [ + Node( + package="los_guidance", + executable="los_guidance_node", + name="los_guidance_node", + namespace=namespace, + parameters=[ + drone_params, + { + "los_config_file": guidance_config, + "time_step": 0.1, + }, + ], + output="screen", + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) \ No newline at end of file From 0431417b0da2021f77e7b2088f60890f2251b63f Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 16 Mar 2026 13:13:55 +0100 Subject: [PATCH 49/87] Start: Test libraary for los --- guidance/los_guidance/CMakeLists.txt | 2 +- .../launch/guidance_test.launch.py | 103 +++++++----- guidance/los_guidance/scripts/square_test.py | 90 ----------- .../los_guidance/scripts/test_scenarios.py | 150 ++++++++++++++++++ 4 files changed, 217 insertions(+), 128 deletions(-) delete mode 100644 guidance/los_guidance/scripts/square_test.py create mode 100644 guidance/los_guidance/scripts/test_scenarios.py diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index d6bdb26bf..00c00a391 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -87,7 +87,7 @@ install(DIRECTORY ) install(PROGRAMS - scripts/square_test.py + scripts/test_scenarios.py DESTINATION share/${PROJECT_NAME}/scripts ) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 3a85dfc7d..fa78df486 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -3,59 +3,74 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( + DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, TimerAction, ) from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) -def generate_launch_description(): - stonefish_dir = get_package_share_directory('stonefish_sim') - vortex_sim_interface_dir = get_package_share_directory('vortex_sim_interface') - los_guidance_dir = get_package_share_directory('los_guidance') - velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + test_scenario = LaunchConfiguration("test_scenario").perform(context) + + stonefish_dir = get_package_share_directory("stonefish_sim") + vortex_sim_interface_dir = get_package_share_directory("vortex_sim_interface") + los_guidance_dir = get_package_share_directory("los_guidance") + operation_mode_manager_dir = get_package_share_directory("operation_mode_manager") stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') + os.path.join(stonefish_dir, "launch", "simulation.launch.py") ), launch_arguments={ - 'scenario': 'default', - 'rendering': 'true', + "scenario": "default", + "rendering": "false", }.items(), ) vortex_sim_interface = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( - vortex_sim_interface_dir, 'launch', 'vortex_sim_interface.launch.py' + vortex_sim_interface_dir, "launch", "vortex_sim_interface.launch.py" ) - ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), ) operation_mode_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( - get_package_share_directory("operation_mode_manager"), + operation_mode_manager_dir, "launch", "operation_mode_manager.launch.py", ) - ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), ) los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(los_guidance_dir, 'launch', 'los_guidance.launch.py') - ) - ) - - velocity_controller_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py' - ) - ) + os.path.join(los_guidance_dir, "launch", "los_guidance.launch.py") + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), ) orca_sim = TimerAction( @@ -63,7 +78,7 @@ def generate_launch_description(): actions=[ IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, 'launch', 'orca_sim.launch.py') + os.path.join(stonefish_dir, "launch", "orca_sim.launch.py") ) ) ], @@ -77,11 +92,11 @@ def generate_launch_description(): "bash", "-lc", ( - "ros2 service call /orca/set_killswitch " + f"ros2 service call /{namespace}/set_killswitch " "vortex_msgs/srv/SetKillswitch " "\"{killswitch_on: false}\" " "&& " - "ros2 service call /orca/set_operation_mode " + f"ros2 service call /{namespace}/set_operation_mode " "vortex_msgs/srv/SetOperationMode " "\"{requested_operation_mode: {operation_mode: 1}}\"" ), @@ -91,29 +106,43 @@ def generate_launch_description(): ], ) - square_test = TimerAction( + test_scenarios = TimerAction( period=20.0, actions=[ ExecuteProcess( cmd=[ "bash", "-lc", - f"python3 {os.path.join(los_guidance_dir, 'scripts', 'square_test.py')}", + ( + f"python3 {os.path.join(los_guidance_dir, 'scripts', 'test_scenario.py')} " + f"--ros-args -p drone:={drone} -p test_scenario:={test_scenario}" + ), ], output="screen", ) ], ) + return [ + stonefish_sim, + vortex_sim_interface, + operation_mode_launch, + los_guidance_launch, + orca_sim, + set_autonomy, + test_scenarios, + ] + + +def generate_launch_description(): return LaunchDescription( - [ - stonefish_sim, - vortex_sim_interface, - operation_mode_launch, - los_guidance_launch, - velocity_controller_launch, - orca_sim, - set_autonomy, - square_test, + declare_drone_and_namespace_args(default_drone="orca") + + [ + DeclareLaunchArgument( + "test_scenario", + default_value="square", + description="Scenario to run: square, circle, test_pitch, opposite_point", + ), + OpaqueFunction(function=launch_setup), ] - ) + ) \ No newline at end of file diff --git a/guidance/los_guidance/scripts/square_test.py b/guidance/los_guidance/scripts/square_test.py deleted file mode 100644 index 954530849..000000000 --- a/guidance/los_guidance/scripts/square_test.py +++ /dev/null @@ -1,90 +0,0 @@ -import rclpy -from rclpy.action import ActionClient -from rclpy.node import Node -from std_msgs.msg import Header -from vortex_msgs.action import LOSGuidance - - -class SquareTest(Node): - def __init__(self): - super().__init__('square_test_client') - - self.get_logger().info("Square test started") - - self._action_client = ActionClient(self, LOSGuidance, '/orca/los_guidance') - - self.depth = 2.5 - self.size = 10.0 - - self.waypoints = [ - (self.size, 0.0, self.depth), - (self.size, self.size, self.depth), - (0.0, self.size, self.depth), - (0.0, 0.0, self.depth), - ] - - self.current_index = 0 - - self.send_next_goal() - - def send_next_goal(self): - if self.current_index >= len(self.waypoints): - self.get_logger().info("Square test completed!") - rclpy.shutdown() - return - - self._action_client.wait_for_server() - - goal_msg = LOSGuidance.Goal() - - header = Header() - header.frame_id = "world_ned" - - goal_msg.goal.header = header - - x, y, z = self.waypoints[self.current_index] - - goal_msg.goal.point.x = x - goal_msg.goal.point.y = y - goal_msg.goal.point.z = z - - self.get_logger().info( - f"Sending waypoint {self.current_index + 1}: x={x}, y={y}, z={z}" - ) - - self._send_goal_future = self._action_client.send_goal_async( - goal_msg, feedback_callback=self.feedback_callback - ) - - self._send_goal_future.add_done_callback(self.goal_response_callback) - - def goal_response_callback(self, future): - goal_handle = future.result() - - if not goal_handle.accepted: - self.get_logger().info('Goal rejected') - return - - self.get_logger().info('Goal accepted') - - self._get_result_future = goal_handle.get_result_async() - self._get_result_future.add_done_callback(self.result_callback) - - def result_callback(self, future): - self.get_logger().info("Waypoint reached") - - self.current_index += 1 - self.send_next_goal() - - def feedback_callback(self, feedback_msg): - pass - - -def main(args=None): - rclpy.init(args=args) - node = SquareTest() - rclpy.spin(node) - - -if __name__ == '__main__': - main() diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py new file mode 100644 index 000000000..0d6a2d328 --- /dev/null +++ b/guidance/los_guidance/scripts/test_scenarios.py @@ -0,0 +1,150 @@ +import math + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from std_msgs.msg import Header +from vortex_msgs.action import LOSGuidance + + +class WaypointTest(Node): + def __init__(self): + super().__init__("waypoint_test_client") + + self.declare_parameter("test_scenario", "square") + self.declare_parameter("drone", "orca") + + self.test_scenario = self.get_parameter("test_scenario").value + self.drone = self.get_parameter("drone").value + + self._action_client = ActionClient( + self, + LOSGuidance, + f"/{self.drone}/los_guidance", + ) + + self.depth = 2.5 + self.square_size = 10.0 + + self.circle_radius = 8.0 + self.circle_points = 16 + self.circle_center_x = 0.0 + self.circle_center_y = 0.0 + + self.waypoints = self.generate_waypoints(self.test_scenario) + self.current_index = 0 + + self.get_logger().info(f"Starting test scenario: {self.test_scenario}") + self.get_logger().info(f"Using drone namespace: {self.drone}") + self.get_logger().info(f"Number of waypoints: {len(self.waypoints)}") + + self.send_next_goal() + + def generate_waypoints(self, test_scenario): + if test_scenario == "square": + s = self.square_size + d = self.depth + return [ + (s, 0.0, d), + (s, s, d), + (0.0, s, d), + (0.0, 0.0, d), + ] + + elif test_scenario == "circle": + d = self.depth + waypoints = [] + + for i in range(self.circle_points): + theta = 2.0 * math.pi * i / self.circle_points + x = self.circle_center_x + self.circle_radius * math.cos(theta) + y = self.circle_center_y + self.circle_radius * math.sin(theta) + waypoints.append((x, y, d)) + + waypoints.append(waypoints[0]) + return waypoints + + elif test_scenario == "test_pitch": + # 0 = water surface, do not go above + # 3 = seabed/ground, do not touch + # Keep all depths safely between these + return [ + (3.0, 0.0, 1.0), # slight up + (6.0, 0.0, 2.0), # slight down + (9.0, 0.0, 1.0), # up again + (12.0, 0.0, 2.0), # down again + ] + + elif test_scenario == "opposite_point": + # Go to one point, then the exact opposite point + return [ + (6.0, 4.0, self.depth), + (-6.0, -4.0, self.depth), + ] + + else: + self.get_logger().warn( + f"Unknown test_scenario '{test_scenario}', defaulting to square" + ) + return self.generate_waypoints("square") + + def send_next_goal(self): + if self.current_index >= len(self.waypoints): + self.get_logger().info(f"{self.test_scenario} test completed!") + rclpy.shutdown() + return + + self._action_client.wait_for_server() + + goal_msg = LOSGuidance.Goal() + + header = Header() + header.frame_id = "world_ned" + goal_msg.goal.header = header + + x, y, z = self.waypoints[self.current_index] + goal_msg.goal.point.x = float(x) + goal_msg.goal.point.y = float(y) + goal_msg.goal.point.z = float(z) + + self.get_logger().info( + f"Sending waypoint {self.current_index + 1}/{len(self.waypoints)}: " + f"x={x:.2f}, y={y:.2f}, z={z:.2f}" + ) + + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback, + ) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + + if not goal_handle.accepted: + self.get_logger().error("Goal rejected") + rclpy.shutdown() + return + + self.get_logger().info("Goal accepted") + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.result_callback) + + def result_callback(self, future): + self.get_logger().info("Waypoint reached") + self.current_index += 1 + self.send_next_goal() + + def feedback_callback(self, feedback_msg): + pass + + +def main(args=None): + rclpy.init(args=args) + node = WaypointTest() + rclpy.spin(node) + + +if __name__ == "__main__": + main() \ No newline at end of file From ef4517efd5bbffb2b7d2030843b9cf8e2a360f69 Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 23 Mar 2026 12:14:30 +0100 Subject: [PATCH 50/87] Add: Diffrent test scenraios for autopilot test --- guidance/los_guidance/config/guidance_params.yaml | 3 ++- .../include/los_guidance/los_guidance_ros.hpp | 4 +++- guidance/los_guidance/launch/guidance_test.launch.py | 8 ++++---- guidance/los_guidance/launch/los_guidance.launch.py | 2 +- guidance/los_guidance/scripts/test_scenarios.py | 8 ++++---- guidance/los_guidance/src/los_guidance_ros.cpp | 11 ++++++++--- 6 files changed, 22 insertions(+), 14 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 0914cd9ff..0a546a4a8 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -28,8 +28,9 @@ vector_field_los: common: active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS u_desired: 0.3 - goal_reached_tol: 0.35 + goal_reached_tol: 0.15 max_pitch_angle: 0.96 # 55 degrees + slow_down_distance: 0.40 # Debug Settings debug: diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 39e4edca8..a996c42b9 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -75,7 +75,8 @@ class LosGuidanceNode : public rclcpp::Node { void publish_state_debug( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose); - vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output, + types::Inputs inputs); // State Flags bool has_active_segment_{false}; @@ -111,6 +112,7 @@ class LosGuidanceNode : public rclcpp::Node { double u_desired_{}; double goal_reached_tol_{}; double max_pitch_angle_{}; + double slow_down_distance_{}; types::ActiveLosMethod method_{}; // Guidance Modules diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index fa78df486..68e739ab7 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -106,7 +106,7 @@ def launch_setup(context, *args, **kwargs): ], ) - test_scenarios = TimerAction( + run_test_scenario = TimerAction( period=20.0, actions=[ ExecuteProcess( @@ -114,7 +114,7 @@ def launch_setup(context, *args, **kwargs): "bash", "-lc", ( - f"python3 {os.path.join(los_guidance_dir, 'scripts', 'test_scenario.py')} " + f"python3 {os.path.join(los_guidance_dir, 'scripts', 'test_scenarios.py')} " f"--ros-args -p drone:={drone} -p test_scenario:={test_scenario}" ), ], @@ -130,7 +130,7 @@ def launch_setup(context, *args, **kwargs): los_guidance_launch, orca_sim, set_autonomy, - test_scenarios, + run_test_scenario, ] @@ -145,4 +145,4 @@ def generate_launch_description(): ), OpaqueFunction(function=launch_setup), ] - ) \ No newline at end of file + ) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 5ee9a3d1d..d616e0853 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -48,4 +48,4 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] - ) \ No newline at end of file + ) diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py index 0d6a2d328..95edef2d6 100644 --- a/guidance/los_guidance/scripts/test_scenarios.py +++ b/guidance/los_guidance/scripts/test_scenarios.py @@ -69,9 +69,9 @@ def generate_waypoints(self, test_scenario): # 3 = seabed/ground, do not touch # Keep all depths safely between these return [ - (3.0, 0.0, 1.0), # slight up - (6.0, 0.0, 2.0), # slight down - (9.0, 0.0, 1.0), # up again + (3.0, 0.0, 1.0), # slight up + (6.0, 0.0, 2.0), # slight down + (9.0, 0.0, 1.0), # up again (12.0, 0.0, 2.0), # down again ] @@ -147,4 +147,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index de851d950..09042ae74 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -250,13 +250,17 @@ void LosGuidanceNode::set_los_mode( // Message Helpers vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( - types::Outputs outputs) { + types::Outputs outputs, + types::Inputs inputs) { vortex_msgs::msg::LOSGuidance reference_msg; const double clamped_pitch = std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; - reference_msg.surge = u_desired_; + if ((inputs.current_position - inputs.next_point).as_vector().norm() <= + slow_down_distance_) { + reference_msg.surge = (u_desired_ / 2.0); + } return reference_msg; } @@ -271,6 +275,7 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { u_desired_ = common_config["u_desired"].as(); max_pitch_angle_ = common_config["max_pitch_angle"].as(); goal_reached_tol_ = common_config["goal_reached_tol"].as(); + slow_down_distance_ = common_config["slow_down_distance"].as(); method_ = static_cast( common_config["active_los_method"].as()); } @@ -353,7 +358,7 @@ void LosGuidanceNode::execute( } vortex_msgs::msg::LOSGuidance reference_msg = - fill_los_reference(outputs); + fill_los_reference(outputs, inputs_copy); feedback->feedback = reference_msg; los_debug_pub_->publish(reference_msg); From 7ec98730afbd477839b1c070b75183e3bda69a06 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Mar 2026 13:20:20 +0100 Subject: [PATCH 51/87] Add: Slowing down while reaching the goal --- .../los_guidance/launch/guidance_test.launch.py | 15 +++++++++++++++ guidance/los_guidance/src/los_guidance_ros.cpp | 14 +++++++++++--- 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 68e739ab7..7b3470932 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -26,6 +26,7 @@ def launch_setup(context, *args, **kwargs): vortex_sim_interface_dir = get_package_share_directory("vortex_sim_interface") los_guidance_dir = get_package_share_directory("los_guidance") operation_mode_manager_dir = get_package_share_directory("operation_mode_manager") + velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -73,6 +74,19 @@ def launch_setup(context, *args, **kwargs): }.items(), ) + velocity_controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py' + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + + ) + orca_sim = TimerAction( period=12.0, actions=[ @@ -128,6 +142,7 @@ def launch_setup(context, *args, **kwargs): vortex_sim_interface, operation_mode_launch, los_guidance_launch, + velocity_controller_launch, orca_sim, set_autonomy, run_test_scenario, diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 09042ae74..976d73980 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -257,9 +257,17 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; - if ((inputs.current_position - inputs.next_point).as_vector().norm() <= - slow_down_distance_) { - reference_msg.surge = (u_desired_ / 2.0); + + const double distance_to_goal = + (inputs.current_position - inputs.next_point).as_vector().norm(); + + const double u_slow_min = 0.2; // bytt ut dette med config + + if (distance_to_goal <= slow_down_distance_) { + const double alpha = distance_to_goal / slow_down_distance_; + reference_msg.surge = u_slow_min + alpha * (u_desired_ - u_slow_min); + } else { + reference_msg.surge = u_desired_; } return reference_msg; From b965ebd5ce9637b71c422d1d49402c86f3f2ae1e Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Mar 2026 15:51:35 +0100 Subject: [PATCH 52/87] Fix: Issues with slowdown function --- .../los_guidance/config/guidance_params.yaml | 6 ++-- .../include/los_guidance/los_guidance_ros.hpp | 3 ++ .../launch/guidance_test.launch.py | 10 +++---- .../los_guidance/src/los_guidance_ros.cpp | 30 ++++++++++++++++--- 4 files changed, 38 insertions(+), 11 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 0a546a4a8..7a7b8c091 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -28,9 +28,11 @@ vector_field_los: common: active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS u_desired: 0.3 - goal_reached_tol: 0.15 + goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees - slow_down_distance: 0.40 + slow_down_distance: 1.0 + u_slow_min_ : 0.3 + surge_initialization: false # Debug Settings debug: diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index a996c42b9..a2baf24a2 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -113,6 +113,9 @@ class LosGuidanceNode : public rclcpp::Node { double goal_reached_tol_{}; double max_pitch_angle_{}; double slow_down_distance_{}; + bool surge_initialized_{}; + double u_slow_min_{}; + double commanded_surge_{}; types::ActiveLosMethod method_{}; // Guidance Modules diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 7b3470932..79d3a1c94 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -26,7 +26,7 @@ def launch_setup(context, *args, **kwargs): vortex_sim_interface_dir = get_package_share_directory("vortex_sim_interface") los_guidance_dir = get_package_share_directory("los_guidance") operation_mode_manager_dir = get_package_share_directory("operation_mode_manager") - velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') + velocity_controller_dir = get_package_share_directory('velocity_controller') stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -77,7 +77,7 @@ def launch_setup(context, *args, **kwargs): velocity_controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( - velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py' + velocity_controller_dir, 'launch', 'velocity_controller.launch.py' ) ), launch_arguments={ @@ -92,7 +92,7 @@ def launch_setup(context, *args, **kwargs): actions=[ IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, "launch", "orca_sim.launch.py") + os.path.join(stonefish_dir, "launch", "drone_sim.launch.py") ) ) ], @@ -139,8 +139,8 @@ def launch_setup(context, *args, **kwargs): return [ stonefish_sim, - vortex_sim_interface, - operation_mode_launch, + #vortex_sim_interface, + #operation_mode_launch, los_guidance_launch, velocity_controller_launch, orca_sim, diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 976d73980..b39d141fb 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -261,15 +261,34 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( const double distance_to_goal = (inputs.current_position - inputs.next_point).as_vector().norm(); - const double u_slow_min = 0.2; // bytt ut dette med config + double target_surge = u_desired_; + double surge_rate_limit_ = 0.3; if (distance_to_goal <= slow_down_distance_) { const double alpha = distance_to_goal / slow_down_distance_; - reference_msg.surge = u_slow_min + alpha * (u_desired_ - u_slow_min); - } else { - reference_msg.surge = u_desired_; + reference_msg.surge = u_slow_min_ + alpha * (u_desired_ - u_slow_min_); + } + + if (!surge_initialized_) { + commanded_surge_ = target_surge; + surge_initialized_ = true; } + else { + const double dt = static_cast(time_step_.count()) / 1000.0; + const double max_step = surge_rate_limit_ * dt; + const double delta = target_surge - commanded_surge_; + + if (delta > max_step) { + commanded_surge_ += max_step; + } else if (delta < -max_step) { + commanded_surge_ -= max_step; + } else { + commanded_surge_ = target_surge; + } + } + + reference_msg.surge = commanded_surge_; return reference_msg; } @@ -284,6 +303,9 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { max_pitch_angle_ = common_config["max_pitch_angle"].as(); goal_reached_tol_ = common_config["goal_reached_tol"].as(); slow_down_distance_ = common_config["slow_down_distance"].as(); + u_slow_min_ = common_config["u_slow_min_"].as(); + surge_initialized_ = common_config["surge_initialization"].as(); + method_ = static_cast( common_config["active_los_method"].as()); } From bcd49aa475bf2ad93c41a7ba0ae9d6444c972943 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Mar 2026 16:09:20 +0100 Subject: [PATCH 53/87] Merge with main --- .../los_guidance/config/guidance_params.yaml | 3 +- .../include/los_guidance/los_guidance_ros.hpp | 1 + .../launch/guidance_test.launch.py | 32 +------------------ .../los_guidance/src/los_guidance_ros.cpp | 2 +- 4 files changed, 5 insertions(+), 33 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 7a7b8c091..7a8c613d7 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -31,7 +31,8 @@ common: goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees slow_down_distance: 1.0 - u_slow_min_ : 0.3 + u_slow_min_ : 0.1 + surge_rate_limit: 0.1 surge_initialization: false # Debug Settings diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index a2baf24a2..e6f3d3851 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -116,6 +116,7 @@ class LosGuidanceNode : public rclcpp::Node { bool surge_initialized_{}; double u_slow_min_{}; double commanded_surge_{}; + double surge_rate_limit_ {}; types::ActiveLosMethod method_{}; // Guidance Modules diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 79d3a1c94..7a0470421 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -23,9 +23,7 @@ def launch_setup(context, *args, **kwargs): test_scenario = LaunchConfiguration("test_scenario").perform(context) stonefish_dir = get_package_share_directory("stonefish_sim") - vortex_sim_interface_dir = get_package_share_directory("vortex_sim_interface") los_guidance_dir = get_package_share_directory("los_guidance") - operation_mode_manager_dir = get_package_share_directory("operation_mode_manager") velocity_controller_dir = get_package_share_directory('velocity_controller') stonefish_sim = IncludeLaunchDescription( @@ -34,33 +32,7 @@ def launch_setup(context, *args, **kwargs): ), launch_arguments={ "scenario": "default", - "rendering": "false", - }.items(), - ) - - vortex_sim_interface = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - vortex_sim_interface_dir, "launch", "vortex_sim_interface.launch.py" - ) - ), - launch_arguments={ - "drone": drone, - "namespace": namespace, - }.items(), - ) - - operation_mode_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - operation_mode_manager_dir, - "launch", - "operation_mode_manager.launch.py", - ) - ), - launch_arguments={ - "drone": drone, - "namespace": namespace, + "rendering": "true", }.items(), ) @@ -139,8 +111,6 @@ def launch_setup(context, *args, **kwargs): return [ stonefish_sim, - #vortex_sim_interface, - #operation_mode_launch, los_guidance_launch, velocity_controller_launch, orca_sim, diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index b39d141fb..2598257ae 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -262,7 +262,6 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( (inputs.current_position - inputs.next_point).as_vector().norm(); double target_surge = u_desired_; - double surge_rate_limit_ = 0.3; if (distance_to_goal <= slow_down_distance_) { const double alpha = distance_to_goal / slow_down_distance_; @@ -305,6 +304,7 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { slow_down_distance_ = common_config["slow_down_distance"].as(); u_slow_min_ = common_config["u_slow_min_"].as(); surge_initialized_ = common_config["surge_initialization"].as(); + surge_rate_limit_ = common_config["surge_rate_limit"].as(); method_ = static_cast( common_config["active_los_method"].as()); From e4db24b02847d7a214c30d2b2fd099bca26b55d0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Mar 2026 19:05:19 +0100 Subject: [PATCH 54/87] fix slowing issue --- .../velocity_controller_lqr/__init__.py | 0 guidance/los_guidance/config/guidance_params.yaml | 2 +- guidance/los_guidance/src/los_guidance_ros.cpp | 11 ++++++----- 3 files changed, 7 insertions(+), 6 deletions(-) create mode 100644 control/velocity_controller_lqr/velocity_controller_lqr/__init__.py diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py b/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 7a8c613d7..ca9e93dcf 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -30,7 +30,7 @@ common: u_desired: 0.3 goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees - slow_down_distance: 1.0 + slow_down_distance: 1.5 u_slow_min_ : 0.1 surge_rate_limit: 0.1 surge_initialization: false diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 2598257ae..241b8347f 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -253,6 +253,7 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs, types::Inputs inputs) { vortex_msgs::msg::LOSGuidance reference_msg; + const double clamped_pitch = std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); reference_msg.pitch = clamped_pitch; @@ -264,16 +265,16 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( double target_surge = u_desired_; if (distance_to_goal <= slow_down_distance_) { - const double alpha = distance_to_goal / slow_down_distance_; - reference_msg.surge = u_slow_min_ + alpha * (u_desired_ - u_slow_min_); + const double alpha = std::clamp( + distance_to_goal / slow_down_distance_, 0.0, 1.0); + target_surge = + u_slow_min_ + alpha * (u_desired_ - u_slow_min_); } if (!surge_initialized_) { commanded_surge_ = target_surge; surge_initialized_ = true; - } - - else { + } else { const double dt = static_cast(time_step_.count()) / 1000.0; const double max_step = surge_rate_limit_ * dt; const double delta = target_surge - commanded_surge_; From 5d766b2f68e0893069e73d3e27c36a52c22dfb25 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 24 Mar 2026 19:17:29 +0100 Subject: [PATCH 55/87] Pre-commit check and changes --- .../los_guidance/config/guidance_params.yaml | 3 +- .../include/los_guidance/los_guidance_ros.hpp | 3 +- .../launch/guidance_test.launch.py | 3 +- .../los_guidance/src/los_guidance_ros.cpp | 50 ++++++++++--------- 4 files changed, 32 insertions(+), 27 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index ca9e93dcf..7c89a1b35 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -30,8 +30,9 @@ common: u_desired: 0.3 goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees + slow_approach: false slow_down_distance: 1.5 - u_slow_min_ : 0.1 + u_slow_min: 0.1 surge_rate_limit: 0.1 surge_initialization: false diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index e6f3d3851..fa0b878e2 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -112,11 +112,12 @@ class LosGuidanceNode : public rclcpp::Node { double u_desired_{}; double goal_reached_tol_{}; double max_pitch_angle_{}; + bool slow_approach_{}; double slow_down_distance_{}; bool surge_initialized_{}; double u_slow_min_{}; double commanded_surge_{}; - double surge_rate_limit_ {}; + double surge_rate_limit_{}; types::ActiveLosMethod method_{}; // Guidance Modules diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 7a0470421..772474673 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -52,11 +52,10 @@ def launch_setup(context, *args, **kwargs): velocity_controller_dir, 'launch', 'velocity_controller.launch.py' ) ), - launch_arguments={ + launch_arguments={ "drone": drone, "namespace": namespace, }.items(), - ) orca_sim = TimerAction( diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 241b8347f..5c0f8dc3a 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -259,33 +259,36 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; - const double distance_to_goal = - (inputs.current_position - inputs.next_point).as_vector().norm(); + if (slow_approach_) { + const double distance_to_goal = + (inputs.current_position - inputs.next_point).as_vector().norm(); - double target_surge = u_desired_; + double target_surge = u_desired_; - if (distance_to_goal <= slow_down_distance_) { - const double alpha = std::clamp( - distance_to_goal / slow_down_distance_, 0.0, 1.0); - target_surge = - u_slow_min_ + alpha * (u_desired_ - u_slow_min_); - } + if (distance_to_goal <= slow_down_distance_) { + const double alpha = + std::clamp(distance_to_goal / slow_down_distance_, 0.0, 1.0); + target_surge = u_slow_min_ + alpha * (u_desired_ - u_slow_min_); + } - if (!surge_initialized_) { - commanded_surge_ = target_surge; - surge_initialized_ = true; - } else { - const double dt = static_cast(time_step_.count()) / 1000.0; - const double max_step = surge_rate_limit_ * dt; - const double delta = target_surge - commanded_surge_; - - if (delta > max_step) { - commanded_surge_ += max_step; - } else if (delta < -max_step) { - commanded_surge_ -= max_step; - } else { + if (!surge_initialized_) { commanded_surge_ = target_surge; + surge_initialized_ = true; + } else { + const double dt = static_cast(time_step_.count()) / 1000.0; + const double max_step = surge_rate_limit_ * dt; + const double delta = target_surge - commanded_surge_; + + if (delta > max_step) { + commanded_surge_ += max_step; + } else if (delta < -max_step) { + commanded_surge_ -= max_step; + } else { + commanded_surge_ = target_surge; + } } + } else { + commanded_surge_ = u_desired_; } reference_msg.surge = commanded_surge_; @@ -302,8 +305,9 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { u_desired_ = common_config["u_desired"].as(); max_pitch_angle_ = common_config["max_pitch_angle"].as(); goal_reached_tol_ = common_config["goal_reached_tol"].as(); + slow_approach_ = common_config["slow_approach"].as(); slow_down_distance_ = common_config["slow_down_distance"].as(); - u_slow_min_ = common_config["u_slow_min_"].as(); + u_slow_min_ = common_config["u_slow_min"].as(); surge_initialized_ = common_config["surge_initialization"].as(); surge_rate_limit_ = common_config["surge_rate_limit"].as(); From 8d2707a3ce7c30b0e623b3ed6d1c103df441c846 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 12:17:15 +0100 Subject: [PATCH 56/87] Edit in read me --- guidance/los_guidance/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index af54fdb90..20dd8f38e 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -324,7 +324,6 @@ However it performs worse when the path contains **sharp turns or rapidly changi | Interface | Name | Type | Message-Type | |----------|------|------|---------| | Action Server | `/orca/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | -| Subscriber | `/orca/waypoint` | Waypoint input | `geometry_msgs/PointStamped` | | Subscriber | `/orca/pose` | Vehicle pose | `geometry_msgs/PoseWithCovarianceStamped` | | Subscriber | `/orca/odom` | Vehicle velocity | `nav_msgs/Odometry` | | Publisher | `/orca/guidance/los` | Guidance reference (yaw, pitch, surge) | `vortex_msgs/LOSGuidance` | From a74fd950390d8ab9fa3b7cf3afc1fe20f0d440ce Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 15:27:44 +0100 Subject: [PATCH 57/87] Final changes before PR --- guidance/los_guidance/README.md | 19 ++++++ .../los_guidance/config/guidance_params.yaml | 8 +-- .../include/los_guidance/los_guidance_ros.hpp | 2 +- .../los_guidance/src/los_guidance_ros.cpp | 32 +++++---- .../los_guidance/test/adaptive_los_test.cpp | 66 ------------------- 5 files changed, 39 insertions(+), 88 deletions(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 20dd8f38e..10a3a6341 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -319,6 +319,25 @@ However it performs worse when the path contains **sharp turns or rapidly changi --- +## Testing with `guidance_test.launch.py` + +To test the entire los system from just one place a simple, low-effort launch script has been implemented. The file is named `los_guidance/launch/guidance_test.launch.py` It launches the necessary nodes for simulation and autopilot and lets you choose a test scenario through the `test_scenario` argument.The sceraios that have been implemented are: + +| Scenario | Use | +|----------|------| +| Circle | ends the drone in a circular path | +| Square |sends the drone through a square waypoint pattern| +| opposite_point | sends the drone toward a waypoint and then toward a waypoint in the opposite direction (180 degrees) | + +Example of use: + +``` +ros2 launch los_guidance guidance_test.launch.py test_scenario:=circle +``` + +--- + + ## ROS Interfaces | Interface | Name | Type | Message-Type | diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 7c89a1b35..1a5368dbb 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -26,7 +26,7 @@ vector_field_los: # Common Guidance Parameters common: - active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS + active_los_method: 2 # 0: Proportional LOS, 1: Integral LOS, 2: Adaptive LOS, 3: Vector Field LOS u_desired: 0.3 goal_reached_tol: 0.20 max_pitch_angle: 0.96 # 55 degrees @@ -34,9 +34,3 @@ common: slow_down_distance: 1.5 u_slow_min: 0.1 surge_rate_limit: 0.1 - surge_initialization: false - -# Debug Settings -debug: - enable_debug: true - debug_topic_name: "/los_guidance_debug" diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index fa0b878e2..50fc6af2c 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -114,7 +114,7 @@ class LosGuidanceNode : public rclcpp::Node { double max_pitch_angle_{}; bool slow_approach_{}; double slow_down_distance_{}; - bool surge_initialized_{}; + bool surge_initialized_{false}; double u_slow_min_{}; double commanded_surge_{}; double surge_rate_limit_{}; diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 5c0f8dc3a..156466aa3 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -236,7 +236,7 @@ void LosGuidanceNode::handle_accepted( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { - execute(goal_handle); + std::thread{[this, goal_handle]() { execute(goal_handle); }}.detach(); } // Service Callback @@ -308,7 +308,6 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { slow_approach_ = common_config["slow_approach"].as(); slow_down_distance_ = common_config["slow_down_distance"].as(); u_slow_min_ = common_config["u_slow_min"].as(); - surge_initialized_ = common_config["surge_initialization"].as(); surge_rate_limit_ = common_config["surge_rate_limit"].as(); method_ = static_cast( @@ -398,23 +397,28 @@ void LosGuidanceNode::execute( los_debug_pub_->publish(reference_msg); - const auto& v = debug_current_odom_->twist.twist.linear; - double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); + if (debug_current_odom_) { + const auto& v = debug_current_odom_->twist.twist.linear; + double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); - vortex_msgs::msg::LOSGuidance state_debug_msg; - Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( - Eigen::Quaterniond(debug_current_odom_->pose.pose.orientation.w, - debug_current_odom_->pose.pose.orientation.x, - debug_current_odom_->pose.pose.orientation.y, - debug_current_odom_->pose.pose.orientation.z)); + vortex_msgs::msg::LOSGuidance state_debug_msg; + Eigen::Vector3d euler = + vortex::utils::math::quat_to_euler(Eigen::Quaterniond( + debug_current_odom_->pose.pose.orientation.w, + debug_current_odom_->pose.pose.orientation.x, + debug_current_odom_->pose.pose.orientation.y, + debug_current_odom_->pose.pose.orientation.z)); - state_debug_msg.pitch = euler.y(); - state_debug_msg.yaw = euler.z(); - state_debug_msg.surge = surge; + state_debug_msg.pitch = euler.y(); + state_debug_msg.yaw = euler.z(); + state_debug_msg.surge = surge; + + state_debug_pub_->publish(state_debug_msg); + } - state_debug_pub_->publish(state_debug_msg); goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); + if ((inputs_copy.current_position - inputs_copy.next_point) .as_vector() .norm() < goal_reached_tol_) { diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp index 5fd71027a..dbf63eb04 100644 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -20,72 +20,6 @@ class AdaptiveLosTest : public ::testing::Test { AdaptiveLOSGuidance los_; const double tol = 1e-9; }; -/* -TEST_F(AdaptiveLosTest, T01_test_cross_track_error_on_track){ - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(AdaptiveLosTests, T02_test_cross_track_error_right_off_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.5, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(AdaptiveLosTests, T03_test_cross_track_error_left_off_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, -0.5, 0.0}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, -0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(AdaptiveLosTests, T04_test_cross_track_error_under_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, 0.5}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.5, tol); -} - -TEST_F(AdaptiveLosTests, T05_test_cross_track_error_over_track) { - types::Inputs inputs; - inputs.prev_point = types::Point{0.0, 0.0, 0.0}; - inputs.next_point = types::Point{1.0, 0.0, 0.0}; - inputs.current_position = types::Point{0.0, 0.0, -0.5}; - - const types::Output O = los_.calculate_outputs(inputs); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, -0.5, tol); -} -*/ // Test commanded angles when drone is to the right of the track TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { From 86c6f3bf742d1c2f28ac63792096bd943f13a4b3 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 15:53:08 +0100 Subject: [PATCH 58/87] Add: New simulation test --- tests/simulator_tests/los_test/send_goal.py | 118 +++++++++++++++----- 1 file changed, 89 insertions(+), 29 deletions(-) diff --git a/tests/simulator_tests/los_test/send_goal.py b/tests/simulator_tests/los_test/send_goal.py index d9ad9770f..c07f9e53b 100644 --- a/tests/simulator_tests/los_test/send_goal.py +++ b/tests/simulator_tests/los_test/send_goal.py @@ -1,61 +1,121 @@ import rclpy from rclpy.action import ActionClient from rclpy.node import Node +from std_msgs.msg import Header from vortex_msgs.action import LOSGuidance class LOSGuidanceClient(Node): def __init__(self): - super().__init__('los_guidance_client') - # Create the action client - self._action_client = ActionClient(self, LOSGuidance, '/orca/los_guidance') + super().__init__("los_guidance_client") + + self.declare_parameter("drone", "orca") + self.declare_parameter("x", 20.0) + self.declare_parameter("y", 20.0) + self.declare_parameter("z", 2.5) + + self.drone = self.get_parameter("drone").value + self.goal_x = float(self.get_parameter("x").value) + self.goal_y = float(self.get_parameter("y").value) + self.goal_z = float(self.get_parameter("z").value) + + self._action_client = ActionClient( + self, + LOSGuidance, + f"/{self.drone}/los_guidance", + ) + + self.get_logger().info(f"Using drone namespace: {self.drone}") self.send_goal() def send_goal(self): + self.get_logger().info("Waiting for action server...") + if not self._action_client.wait_for_server(timeout_sec=10.0): + self.get_logger().error("Action server not available") + self.shutdown_with_code(1) + return + goal_msg = LOSGuidance.Goal() - # Create a message with the goal - goal_msg.goal.point.x = 20.0 - goal_msg.goal.point.y = 20.0 - goal_msg.goal.point.z = 5.0 + header = Header() + header.frame_id = "world_ned" + header.stamp = self.get_clock().now().to_msg() + goal_msg.goal.header = header + + goal_msg.goal.point.x = self.goal_x + goal_msg.goal.point.y = self.goal_y + goal_msg.goal.point.z = self.goal_z + + self.get_logger().info( + f"Sending goal: x={self.goal_x:.2f}, y={self.goal_y:.2f}, z={self.goal_z:.2f}" + ) - # Send the goal asynchronously - self._action_client.wait_for_server(timeout_sec=10.0) - self.get_logger().info('Sending goal...') - self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback, + ) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): goal_handle = future.result() + + if goal_handle is None: + self.get_logger().error("Failed to send goal") + self.shutdown_with_code(1) + return + if not goal_handle.accepted: - self.get_logger().info('Goal rejected :(') + self.get_logger().error("Goal rejected") + self.shutdown_with_code(1) return - self.get_logger().info('Goal accepted :)') + self.get_logger().info("Goal accepted") self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def get_result_callback(self, future): - result = future.result().result.success - self.get_logger().info(f'Goal result: {result}') - if result: - self.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - exit(0) + result_msg = future.result() + + if result_msg is None: + self.get_logger().error("Did not receive result") + self.shutdown_with_code(1) + return + + result = result_msg.result + status = result_msg.status + + self.get_logger().info(f"Result status: {status}") + self.get_logger().info(f"Goal success: {result.success}") + + if result.success: + self.get_logger().info("Goal reached successfully") + self.shutdown_with_code(0) else: - self.get_logger().info('Goal failed :(') - self.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - exit(1) + self.get_logger().error("Goal failed") + self.shutdown_with_code(1) + + def feedback_callback(self, feedback_msg): + self.get_logger().debug("Received feedback") + + def shutdown_with_code(self, code): + self.destroy_node() + if rclpy.ok(): + rclpy.shutdown() def main(args=None): rclpy.init(args=args) - action_client = LOSGuidanceClient() - rclpy.spin(action_client) + node = LOSGuidanceClient() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info("Interrupted by user") + finally: + if rclpy.ok(): + node.destroy_node() + rclpy.shutdown() -if __name__ == '__main__': - main() +if __name__ == "__main__": + main() \ No newline at end of file From bf60cb3eaaa77edb189f621b32ecf259e8178826 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 16:21:56 +0100 Subject: [PATCH 59/87] Final Final changes for PR --- tests/simulator_tests/los_test/send_goal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/simulator_tests/los_test/send_goal.py b/tests/simulator_tests/los_test/send_goal.py index c07f9e53b..934ba93ab 100644 --- a/tests/simulator_tests/los_test/send_goal.py +++ b/tests/simulator_tests/los_test/send_goal.py @@ -118,4 +118,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + main() From 8ac7b2b441abc0bab74f2489c2896c687b66b79c Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 16:51:27 +0100 Subject: [PATCH 60/87] FInal FInal Final change before PR --- guidance/los_guidance/src/los_guidance_ros.cpp | 1 - .../los_guidance/test/vector_field_los_test.cpp | 14 +++++++------- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 156466aa3..b04d7121d 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,5 +1,4 @@ #include "los_guidance/los_guidance_ros.hpp" - #include #include #include diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp index 956e9b5f0..34bbf1e6a 100644 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -12,8 +12,8 @@ class VectorFieldLosTest : public ::testing::Test { VectorFieldLosParams params; params.max_approach_angle_h = 30.0 * M_PI / 180.0; // 30 degrees in rad params.max_approach_angle_v = 20.0 * M_PI / 180.0; // 20 degrees in rad - params.k_p_h = 0.1; // needs tuning - params.k_p_v = 0.1; // needs tuning + params.k_p_h = 1.5; // needs tuning + params.k_p_v = 0.9; // needs tuning params.time_step = 0.01; return params; } @@ -69,8 +69,8 @@ TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { EXPECT_NEAR(O.psi_d, 0.0, tol); // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(O.theta_d, 0.0); - EXPECT_LT(O.theta_d, 1.57); + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); } // Test commanded angles when drone is over the track @@ -86,8 +86,8 @@ TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { EXPECT_NEAR(O.psi_d, 0.0, tol); // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); } // Test commanded angles when drone is over and to the right of the track @@ -105,7 +105,7 @@ TEST_F(VectorFieldLosTest, T10_test_commanded_angles) { // Pitch cmd should be between -pi/2 and 0 EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, -1.57); + EXPECT_GT(O.theta_d, 1.57); } } // namespace vortex::guidance::los From a0cc0754bcab5292b0d7e995fabdd4793b1d8ce9 Mon Sep 17 00:00:00 2001 From: Anbit Date: Wed, 25 Mar 2026 17:19:12 +0100 Subject: [PATCH 61/87] Finally Final changes before PR --- guidance/los_guidance/test/vector_field_los_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp index 34bbf1e6a..e11f7d3dd 100644 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -104,8 +104,8 @@ TEST_F(VectorFieldLosTest, T10_test_commanded_angles) { EXPECT_GT(O.psi_d, -1.57); // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(O.theta_d, 0.0); - EXPECT_GT(O.theta_d, 1.57); + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); } } // namespace vortex::guidance::los From 7c9c4f3c49b01ad3a31487e532f2acdb8cc9955c Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 26 Mar 2026 13:58:49 +0100 Subject: [PATCH 62/87] Fix: errror in launch file --- guidance/los_guidance/launch/los_guidance.launch.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index fb1749a3f..6ffefc6df 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -33,7 +33,13 @@ def launch_setup(context, *args, **kwargs): executable="los_guidance_node", name="los_guidance_node", namespace=namespace, - parameters=[drone_params, los_config], + parameters=[ + drone_params, + { + "los_config_file": los_config, + "time_step": 0.1, + }, + ], output="screen", ) ] From 62562fa2a43a5597e795c478d73a20d6cb4346b0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Fri, 27 Mar 2026 12:39:23 +0100 Subject: [PATCH 63/87] Fix: Same msg publisher --- guidance/los_guidance/src/los_guidance_ros.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index b04d7121d..d55af3aa0 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -59,9 +59,6 @@ void LosGuidanceNode::set_subscribers_and_publisher() { reference_pub_ = this->create_publisher( guidance_topic, qos_sensor_data); - los_debug_pub_ = this->create_publisher( - "los_debug", qos_sensor_data); - state_debug_pub_ = this->create_publisher( "state_debug", qos_sensor_data); @@ -394,8 +391,6 @@ void LosGuidanceNode::execute( fill_los_reference(outputs, inputs_copy); feedback->feedback = reference_msg; - los_debug_pub_->publish(reference_msg); - if (debug_current_odom_) { const auto& v = debug_current_odom_->twist.twist.linear; double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); From 935eefc6ad0cfc07d1a088c019b68cb21c2e26e5 Mon Sep 17 00:00:00 2001 From: Anbit Date: Fri, 27 Mar 2026 12:47:55 +0100 Subject: [PATCH 64/87] Remove: feedback for LOS action server --- guidance/los_guidance/src/los_guidance_ros.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index d55af3aa0..a14545425 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -336,8 +336,6 @@ void LosGuidanceNode::execute( path_inputs_.next_point = new_wp; } - auto feedback = - std::make_shared(); auto result = std::make_shared(); rclcpp::Rate loop_rate(1000.0 / time_step_.count()); @@ -389,7 +387,6 @@ void LosGuidanceNode::execute( vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(outputs, inputs_copy); - feedback->feedback = reference_msg; if (debug_current_odom_) { const auto& v = debug_current_odom_->twist.twist.linear; @@ -410,7 +407,6 @@ void LosGuidanceNode::execute( state_debug_pub_->publish(state_debug_msg); } - goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); if ((inputs_copy.current_position - inputs_copy.next_point) From 79afdc0bffc4f3a0e6f749d2be76b69193a4982e Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Fri, 27 Mar 2026 11:59:50 +0100 Subject: [PATCH 65/87] convert los guidance to composable node --- guidance/los_guidance/CMakeLists.txt | 25 ++++++------------- .../include/los_guidance/los_guidance_ros.hpp | 2 +- guidance/los_guidance/package.xml | 1 + .../los_guidance/src/los_guidance_node.cpp | 16 ------------ .../los_guidance/src/los_guidance_ros.cpp | 6 ++++- 5 files changed, 14 insertions(+), 36 deletions(-) delete mode 100644 guidance/los_guidance/src/los_guidance_node.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 00c00a391..f8e7e7b4c 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) @@ -36,6 +37,7 @@ add_library(${LIB_NAME} SHARED ament_target_dependencies(${LIB_NAME} rclcpp rclcpp_action + rclcpp_components geometry_msgs nav_msgs vortex_msgs @@ -45,21 +47,13 @@ ament_target_dependencies(${LIB_NAME} fmt ) -add_executable(los_guidance_node - src/los_guidance_node.cpp -) - -ament_target_dependencies(los_guidance_node - rclcpp - rclcpp_action - geometry_msgs - vortex_msgs - vortex_utils_ros +target_link_libraries(${LIB_NAME} + yaml-cpp ) -target_link_libraries(los_guidance_node - ${LIB_NAME} - yaml-cpp +rclcpp_components_register_node(${LIB_NAME} + PLUGIN "vortex::guidance::los::LosGuidanceNode" + EXECUTABLE los_guidance_node ) install(TARGETS @@ -70,11 +64,6 @@ install(TARGETS RUNTIME DESTINATION bin ) -install(TARGETS - los_guidance_node - DESTINATION lib/${PROJECT_NAME} -) - install( DIRECTORY include/ DESTINATION include/ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 50fc6af2c..55adfa079 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -30,7 +30,7 @@ namespace vortex::guidance::los { class LosGuidanceNode : public rclcpp::Node { public: // Constructor - LosGuidanceNode(); + explicit LosGuidanceNode(const rclcpp::NodeOptions& options); private: // Setup Functions diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 5f696a725..450afeae7 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -11,6 +11,7 @@ rclcpp rclcpp_action + rclcpp_components geometry_msgs vortex_msgs nav_msgs diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp deleted file mode 100644 index 31ad2eb74..000000000 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ /dev/null @@ -1,16 +0,0 @@ -#include - -#include "los_guidance/los_guidance_ros.hpp" - -// Main Entry Point -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - - executor.spin(); - - return 0; -} diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index a14545425..de3183b62 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,5 +1,6 @@ #include "los_guidance/los_guidance_ros.hpp" #include +#include #include #include #include @@ -18,7 +19,8 @@ const auto start_message = R"( namespace vortex::guidance::los { // Constructor -LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { +LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) + : Node("los_guidance_node", options) { double time_step_s = this->declare_parameter("time_step"); time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); @@ -429,3 +431,5 @@ void LosGuidanceNode::execute( } } // namespace vortex::guidance::los + +RCLCPP_COMPONENTS_REGISTER_NODE(vortex::guidance::los::LosGuidanceNode) From 4c42050477c5882445d1ecc2e1da6da4e412c544 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sat, 28 Mar 2026 17:28:33 +0100 Subject: [PATCH 66/87] Start: PR review changes Part1 --- guidance/los_guidance/README.md | 42 +++--- .../los_guidance/config/guidance_params.yaml | 52 +++++--- .../include/los_guidance/lib/adaptive_los.hpp | 101 +++++++++++--- .../include/los_guidance/lib/integral_los.hpp | 95 ++++++++++--- .../los_guidance/lib/proportional_los.hpp | 73 ++++++++-- .../include/los_guidance/lib/types.hpp | 53 ++++++-- .../los_guidance/lib/vector_field_los.hpp | 80 ++++++++--- .../include/los_guidance/los_guidance_ros.hpp | 125 +++++++++++++++--- .../launch/guidance_test.launch.py | 6 +- .../los_guidance/scripts/test_scenarios.py | 12 +- .../los_guidance/src/lib/adaptive_los.cpp | 27 ++-- .../los_guidance/src/lib/integral_los.cpp | 24 ++-- .../los_guidance/src/lib/proportional_los.cpp | 16 +-- .../los_guidance/src/lib/vector_field_los.cpp | 20 +-- .../los_guidance/src/los_guidance_ros.cpp | 99 +++++++------- .../los_guidance/test/adaptive_los_test.cpp | 14 +- .../los_guidance/test/integral_los_test.cpp | 14 +- .../test/proportional_los_test.cpp | 10 +- .../test/vector_field_los_test.cpp | 14 +- 19 files changed, 624 insertions(+), 253 deletions(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 10a3a6341..40c0a80c2 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -30,7 +30,7 @@ The library supports four LOS guidance algorithms. | 2 | Adaptive LOS | | 3 | Vector Field LOS | -The guidance method can be **changed during runtime** using a ROS service. +The guidance method can be preconfigured in the package configuration file before startup, and changed during runtime using a ROS service. --- @@ -40,7 +40,7 @@ The guidance method can be **changed during runtime** using a ROS service. A waypoint can be sent to the guidance node using the action interface: ``` -ros2 action send_goal /orca/los_guidance \ +ros2 action send_goal /drone_name/los_guidance \ vortex_msgs/action/LOSGuidance \ "{goal: {header: {frame_id: world_ned}, point: {x: 0.0, y: 0.0, z: 0.0}}}" ``` @@ -51,10 +51,12 @@ This command instructs the guidance node to start following a path toward the wa # Switching LOS Method -The active LOS guidance method can be changed during runtime. +The active LOS guidance method can be configured in advance through the package config file, and it can also be changed during runtime. + +At runtime, the LOS guidance method can be switched with: ``` -ros2 service call /orca/set_los_mode \ +ros2 service call /drone_name/set_los_mode \ vortex_msgs/srv/SetLosMode "{mode: X}" ``` @@ -70,7 +72,7 @@ Where Example: ``` -ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" +ros2 service call /drone_name/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" ``` This switches the guidance system to **Adaptive LOS**. @@ -218,10 +220,10 @@ Proportional LOS is the simplest LOS guidance law. \tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` -### Parameters +where -- $\Delta_h$ — horizontal lookahead distance -- $\Delta_v$ — vertical lookahead distance +- $\Delta_h$ horizontal lookahead distance +- $\Delta_v$ vertical lookahead distance The lookahead distances determine **how aggressively the vehicle corrects path errors**. @@ -267,12 +269,12 @@ k_{i,v}\int z_e^p dt \tan^{-1}(u_v) ``` -### Parameters +where -- $k_{p,h}$ — horizontal proportional gain -- $k_{p,v}$ — vertical proportional gain -- $k_{i,h}$ — horizontal integral gain -- $k_{i,v}$ — vertical integral gain +- $k_{p,h}$ horizontal proportional gain +- $k_{p,v}$ vertical proportional gain +- $k_{i,h}$ horizontal integral gain +- $k_{i,v}$ vertical integral gain The integral term allows the controller to **eliminate steady-state cross-track errors** caused by constant disturbances. @@ -300,10 +302,10 @@ Vector Field LOS generates a **bounded approach angle** toward the path. \tan^{-1}(k_p y_e^p) ``` -### Parameters +where -- $\psi_{max}$ — maximum allowed approach angle -- $k_p$ — proportional gain controlling path convergence +- $\psi_{max}$ maximum allowed approach angle +- $k_p$ proportional gain controlling path convergence The bounded approach angle prevents excessively aggressive heading changes. @@ -342,10 +344,10 @@ ros2 launch los_guidance guidance_test.launch.py test_scenario:=circle | Interface | Name | Type | Message-Type | |----------|------|------|---------| -| Action Server | `/orca/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | -| Subscriber | `/orca/pose` | Vehicle pose | `geometry_msgs/PoseWithCovarianceStamped` | -| Subscriber | `/orca/odom` | Vehicle velocity | `nav_msgs/Odometry` | -| Publisher | `/orca/guidance/los` | Guidance reference (yaw, pitch, surge) | `vortex_msgs/LOSGuidance` | +| Action Server | `/drone_name/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | +| Subscriber | `/drone_name/pose` | Vehicle pose | `geometry_msgs/PoseWithCovarianceStamped` | +| Subscriber | `/drone_name/odom` | Vehicle velocity | `nav_msgs/Odometry` | +| Publisher | `/drone_name/guidance/los` | Guidance reference (yaw, pitch, surge) | `vortex_msgs/LOSGuidance` | | Publisher | `/los_debug` | LOS debug output | `vortex_msgs/LOSGuidance` | | Publisher | `/state_debug` | Vehicle state debug | `vortex_msgs/LOSGuidance` | diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 1a5368dbb..82ddec457 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,36 +1,56 @@ -# Adaptive LOS Parameters +#/** +# * @file guidance_params.yaml +# * @brief Configuration parameters for the LOS guidance node. +# * +# * This file contains the tunable parameters for all supported Line-Of-Sight +# * (LOS) guidance methods, together with common guidance settings shared across +# * the node. +# * +# * Parameter descriptions: +# * - `lookahead_distance_h`: Horizontal lookahead distance used to define the +# * LOS reference point in the horizontal plane. +# * - `lookahead_distance_v`: Vertical lookahead distance used to define the +# * LOS reference point in the vertical plane. +# * - `gamma_h`: Adaptive update gain for horizontal LOS adaptation. +# * - `gamma_v`: Adaptive update gain for vertical LOS adaptation. +# * - `k_p_h`: Proportional gain in the horizontal plane. +# * - `k_p_v`: Proportional gain in the vertical plane. +# * - `k_i_h`: Integral gain in the horizontal plane. +# * - `k_i_v`: Integral gain in the vertical plane. +# * - `max_approach_angle_h`: Maximum allowed horizontal approach angle for +# * vector field LOS guidance. +# * - `max_approach_angle_v`: Maximum allowed vertical approach angle for +# * vector field LOS guidance. +# * - +# */ adaptive_los: lookahead_distance_h: 0.9 lookahead_distance_v: 1.4 - gamma_h: 0.03 - gamma_v: 0.02 + adaptation_gain_h: 0.03 + adaptation_gain_v: 0.02 -# Proportional LOS Parameters prop_los: lookahead_distance_h: 0.74 lookahead_distance_v: 0.8 -# Integral LOS Parameters integer_los: - k_p_h: 0.5 - k_p_v: 0.5 - k_i_h: 0.1 - k_i_v: 0.1 + proportional_gain_h: 0.5 + proportional_gain_v: 0.5 + integral_gain_h: 0.1 + integral_gain_v: 0.1 -# Vector Field LOS Parameters vector_field_los: max_approach_angle_h: 1.0 max_approach_angle_v: 1.0 - k_p_h: 1.5 - k_p_v: 0.9 + proportional_gain_h: 1.5 + proportional_gain_v: 0.9 -# Common Guidance Parameters common: - active_los_method: 2 # 0: Proportional LOS, 1: Integral LOS, 2: Adaptive LOS, 3: Vector Field LOS + active_los_method: 2 u_desired: 0.3 goal_reached_tol: 0.20 - max_pitch_angle: 0.96 # 55 degrees + max_pitch_angle: 0.96 slow_approach: false slow_down_distance: 1.5 u_slow_min: 0.1 - surge_rate_limit: 0.1 + surge_rate_limit: 0.1 \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 1f9784178..5fc8eca57 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -1,57 +1,122 @@ +/** + * @file adaptive_los.hpp + * @brief Defines the AdaptiveLOSGuidance class and its parameter structure for + * the adaptive LOS guidance algorithm. + */ + #ifndef LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ #define LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ #include #include -#include +#include #include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { -// Parameter Structure +/** + * @brief Parameters for the Adaptive LOS guidance algorithm. + * + * This structure stores the tuning parameters used by the adaptive LOS + * controller for horizontal and vertical path-following. + */ struct AdaptiveLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; + double adaptation_gain_h{}; + double adaptation_gain_v{}; + double time_step{}; // in milliseconds }; -// Adaptive LOS Guidance Class +/** + * @brief Implements the adaptive LOS guidance algorithm. + * + * This class computes desired heading and pitch commands based on the current + * vehicle position and the active path segment. + */ class AdaptiveLOSGuidance { public: - // Constructor / Destructor + /** + * @brief Constructs an AdaptiveLOSGuidance object. + * @param params Parameters for the adaptive LOS guidance algorithm. + */ explicit AdaptiveLOSGuidance(const AdaptiveLosParams& params); + + /** + * @brief Destroys the AdaptiveLOSGuidance object. + */ ~AdaptiveLOSGuidance() = default; - // Main Output Calculation + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for adaptive LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions + /** + * @brief Updates the path heading and path pitch angles from the active path + * segment. + * @param inputs Input values containing the previous and next path points. + */ void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference frame. + * @param inputs Input values containing the current vehicle position and path + * information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ const types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs); + + /** + * @brief Updates the adaptive estimates based on the current cross-track + * error. + * @param cross_track_error The current cross-track error in the path frame. + */ void update_adaptive_estimates( const types::CrossTrackError& cross_track_error); - // Parameters + /** + * @brief Parameters used by the adaptive LOS guidance algorithm. + */ AdaptiveLosParams params_{}; - // Rotation Matrices + /** + * @brief Rotation matrix for the path pitch rotation about the y-axis. + */ Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + + /** + * @brief Rotation matrix for the path heading rotation about the z-axis. + */ Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - // Path Angles - double pi_h_{}; - double pi_v_{}; + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{0.0}; + + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{0.0}; + + /** + * @brief Stores the horizontal adaptive estimate. + */ + double beta_c_hat_{0.0}; - // Adaptive Estimates - double beta_c_hat_ = 0.0; - double alpha_c_hat_ = 0.0; + /** + * @brief Stores the vertical adaptive estimate. + */ + double alpha_c_hat_{0.0}; }; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ +#endif // LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index bde7d0d5f..bd27e8137 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -1,3 +1,8 @@ +/** + * @file integral_los.hpp + * @brief Defines the IntegralLOSGuidance class and parameter structure for the + * integral LOS guidance algorithm. + */ #ifndef LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ #define LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ @@ -5,51 +10,101 @@ #include #include -#include "los_guidance/lib/types.hpp" +#include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { -// Parameter Structure +/** + * @brief Parameters for the Integral LOS guidance algorithm. + */ struct IntegralLosParams { - double k_p_h{}; - double k_p_v{}; - double k_i_h{}; - double k_i_v{}; - double time_step{}; + double proportional_gain_h{}; + double proportional_gain_v{}; + double integral_gain_h{}; + double integral_gain_v{}; + double time_step{}; // in milliseconds }; -// Integral LOS Guidance Class +/** + * @brief Implements the integral LOS guidance algorithm. + * + * This class computes desired heading and pitch commands using proportional and + * integral cross-track error feedback. + */ class IntegralLOSGuidance { public: - // Constructor / Destructor + /** + * @brief Constructs an IntegralLOSGuidance object. + * @param params Parameters for the integral LOS guidance algorithm. + */ explicit IntegralLOSGuidance(const IntegralLosParams& params); + + /** + * @brief Destroys the IntegralLOSGuidance object. + */ ~IntegralLOSGuidance() = default; - // Main Output Calculation + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for integral LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions + /** + * @brief Updates the path heading and path pitch angles from the active path + * segment. + * @param inputs Input values containing the previous and next path points. + */ void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference frame. + * @param inputs Input values containing the current vehicle position and path + * information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs); - // Parameters + /** + * @brief Parameters used by the integral LOS guidance algorithm. + */ IntegralLosParams m_params{}; - // Integral States - double int_h{}; - double int_v{}; + /** + * @brief Stores the integral of the horizontal cross-track error. + */ + double integrated_horizontal_error_{}; + + /** + * @brief Stores the integral of the vertical cross-track error. + */ + double integrated_vertical_error_{}; - // Path Angles - double pi_h_{}; - double pi_v_{}; + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{}; - // Rotation Representation + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{}; + + /** + * @brief Rotation representation for the path pitch angle about the y-axis. + */ Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + + /** + * @brief Rotation representation for the path heading angle about the z-axis. + */ Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ +#endif // LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index a2e9663e1..5a4a1a044 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -1,3 +1,8 @@ +/** + * @file proportional_los.hpp + * @brief Defines the ProportionalLOSGuidance class and parameter structure for + * the proportional LOS guidance algorithm. + */ #ifndef LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ #define LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ @@ -7,42 +12,86 @@ #include "los_guidance/lib/types.hpp" -namespace vortex::guidance::los { +namespace vortex::guidance::los { -// Parameter Structure +/** + * @brief Parameters for the proportional LOS guidance algorithm. + */ struct ProportionalLosParams { double lookahead_distance_h{}; double lookahead_distance_v{}; }; -// Proportional LOS Guidance Class +/** + * @brief Implements the proportional LOS guidance algorithm. + * + * This class computes desired heading and pitch commands using proportional + * cross-track error feedback. + */ class ProportionalLOSGuidance { public: - // Constructor / Destructor + /** + * @brief Constructs a ProportionalLOSGuidance object. + * @param params Parameters for the proportional LOS guidance algorithm. + */ explicit ProportionalLOSGuidance(const ProportionalLosParams& params); + + /** + * @brief Destroys the ProportionalLOSGuidance object. + */ ~ProportionalLOSGuidance() = default; - // Main Output Calculation + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for proportional LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions + /** + * @brief Updates the path heading and path pitch angles from the active path + * segment. + * @param inputs Input values containing the previous and next path points. + */ void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference frame. + * @param inputs Input values containing the current vehicle position and path + * information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs) const; - // Parameters + /** + * @brief Parameters used by the proportional LOS guidance algorithm. + */ ProportionalLosParams m_params{}; - // Path Angles - double pi_h_{0.0}; - double pi_v_{0.0}; + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{0.0}; - // Rotation Representation + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{0.0}; + + /** + * @brief Rotation representation for the path pitch angle about the y-axis. + */ Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + + /** + * @brief Rotation representation for the path heading angle about the z-axis. + */ Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ +#endif // LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index 52aab3d48..3088c5045 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -1,3 +1,7 @@ +/** + * @file types.hpp + * @brief Defines shared data types used by the LOS guidance algorithms. + */ #ifndef LOS_GUIDANCE__LIB__TYPES_HPP_ #define LOS_GUIDANCE__LIB__TYPES_HPP_ @@ -9,52 +13,85 @@ namespace vortex::guidance::los::types { -// Point Representation +/** + * @brief Represents a 3D point. + * + * This struct is used to store waypoint positions and vehicle position data + * in Cartesian coordinates. + */ struct Point { double x{}; double y{}; double z{}; - // Point Operations + /** + * @brief Subtracts another point from this point. + * @param other The point to subtract. + * @return Point The resulting difference vector as a point. + */ Point operator-(const Point& other) const { return Point{x - other.x, y - other.y, z - other.z}; } - // Conversion Functions + /** + * @brief Converts the point to an Eigen 3D vector. + * @return Eigen::Vector3d The point represented as an Eigen vector. + */ Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + /** + * @brief Creates a Point from a ROS point message. + * @param msg ROS point message. + * @return Point The converted point. + */ static Point point_from_ros(const geometry_msgs::msg::Point& msg) { return Point{msg.x, msg.y, msg.z}; } }; -// Cross Track Error +/** + * @brief Represents cross-track error in the path-fixed reference frame. + * + * The values describe the position error relative to the active path segment + * in the transformed coordinate frame. + */ struct CrossTrackError { double x_e{}; double y_e{}; double z_e{}; + /** + * @brief Creates a CrossTrackError from an Eigen vector. + * @param vector Eigen vector containing cross-track error components. + * @return CrossTrackError The converted cross-track error. + */ inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { return CrossTrackError{vector.x(), vector.y(), vector.z()}; } }; -// Guidance Outputs +/** + * @brief Stores the LOS guidance outputs. + */ struct Outputs { double psi_d{}; double theta_d{}; }; -// Guidance Inputs +/** + * @brief Stores the inputs required by the LOS guidance algorithms. + */ struct Inputs { Point prev_point{}; Point next_point{}; Point current_position{}; }; -// Active LOS Method +/** + * @brief Enumerates the available LOS guidance methods. + */ enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; } // namespace vortex::guidance::los::types -#endif // LOS_GUIDANCE__LIB__TYPES_HPP_ +#endif // LOS_GUIDANCE__LIB__TYPES_HPP_ \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index 60d332929..4938392e6 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -1,3 +1,8 @@ +/** + * @file vector_field_los.hpp + * @brief Defines the VectorFieldLOSGuidance class and parameter structure for + * the vector field LOS guidance algorithm. + */ #ifndef LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ #define LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ @@ -7,45 +12,90 @@ #include "los_guidance/lib/types.hpp" -namespace vortex::guidance::los { +namespace vortex::guidance::los { -// Parameter Structure +/** + * @brief Parameters for the vector field LOS guidance algorithm. + */ struct VectorFieldLosParams { double max_approach_angle_h{}; double max_approach_angle_v{}; - double k_p_h{}; - double k_p_v{}; - double time_step{}; + double proportional_gain_h{}; + double proportional_gain_v{}; + double time_step{}; // in milliseconds }; -// Vector Field LOS Guidance Class +/** + * @brief Implements the vector field LOS guidance algorithm. + * + * This class computes desired heading and pitch commands using vector field + * path-following logic based on the current vehicle position and active path + * segment. + */ class VectorFieldLOSGuidance { public: - // Constructor / Destructor + /** + * @brief Constructs a VectorFieldLOSGuidance object. + * @param params Parameters for the vector field LOS guidance algorithm. + */ explicit VectorFieldLOSGuidance(const VectorFieldLosParams& params); + + /** + * @brief Destroys the VectorFieldLOSGuidance object. + */ ~VectorFieldLOSGuidance() = default; - // Main Output Calculation + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for vector field LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ types::Outputs calculate_outputs(const types::Inputs& inputs); private: - // Internal Update Functions + /** + * @brief Updates the path heading and path pitch angles from the active path + * segment. + * @param inputs Input values containing the previous and next path points. + */ void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference frame. + * @param inputs Input values containing the current vehicle position and path + * information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ types::CrossTrackError calculate_crosstrack_error( const types::Inputs& inputs) const; - // Parameters + /** + * @brief Parameters used by the vector field LOS guidance algorithm. + */ VectorFieldLosParams m_params{}; - // Path Angles - double pi_h_{0.0}; - double pi_v_{0.0}; + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{0.0}; - // Rotation Representation + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{0.0}; + + /** + * @brief Rotation representation for the path pitch angle about the y-axis. + */ Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + + /** + * @brief Rotation representation for the path heading angle about the z-axis. + */ Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ +#endif // LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ \ No newline at end of file diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 55adfa079..e241cdd1d 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,3 +1,8 @@ +/** + * @file los_guidance_ros.hpp + * @brief The LosGuidanceNode class initializes ROS interfaces, loads + * configuration parameters, and runs the LOS guidance node. + */ #ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ @@ -26,62 +31,153 @@ namespace vortex::guidance::los { -// LOS Guidance ROS Node +/** + * @brief The LosGuidanceNode class initializes ROS interfaces, loads LOS + * guidance parameters, and manages path-following execution. + */ class LosGuidanceNode : public rclcpp::Node { public: - // Constructor + /** + * @brief Constructs a LosGuidanceNode object. + * @param options ROS node options used when creating the node. + */ explicit LosGuidanceNode(const rclcpp::NodeOptions& options); private: - // Setup Functions + /** + * @brief Sets up the ROS subscribers and publishers used by the node. + */ void set_subscribers_and_publisher(); + + /** + * @brief Sets up the LOS guidance action server. + */ void set_action_server(); + + /** + * @brief Sets up the service server used for changing LOS guidance mode. + */ void set_service_server(); - // Configuration Functions + /** + * @brief Initializes the adaptive LOS guidance module from configuration. + * @param config YAML configuration node containing adaptive LOS parameters. + */ void set_adaptive_los_guidance(YAML::Node config); + + /** + * @brief Initializes the proportional LOS guidance module from configuration. + * @param config YAML configuration node containing proportional LOS parameters. + */ void set_proportional_los_guidance(YAML::Node config); + + /** + * @brief Initializes the integral LOS guidance module from configuration. + * @param config YAML configuration node containing integral LOS parameters. + */ void set_integral_los_guidance(YAML::Node config); + + /** + * @brief Initializes the vector field LOS guidance module from configuration. + * @param config YAML configuration node containing vector field LOS parameters. + */ void set_vector_field_guidance(YAML::Node config); + + /** + * @brief Loads the LOS guidance YAML configuration file. + * @param yaml_file_path Path to the YAML configuration file. + * @return YAML::Node Parsed YAML configuration. + */ YAML::Node get_los_config(std::string yaml_file_path); + + /** + * @brief Parses common guidance parameters shared by all LOS methods. + * @param common_config YAML node containing common guidance parameters. + */ void parse_common_config(YAML::Node common_config); - // Callback Functions + /** + * @brief Callback for receiving waypoint updates. + * @param msg Received waypoint message. + */ void waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr msg); + + /** + * @brief Callback for receiving pose updates. + * @param msg Received pose message. + */ void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + /** + * @brief Callback for receiving odometry updates. + * @param msg Received odometry message. + */ void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - // Action Server Functions + /** + * @brief Handles an incoming LOS guidance action goal request. + * @param uuid Unique identifier for the received goal. + * @param goal Requested LOS guidance goal. + * @return rclcpp_action::GoalResponse Response indicating whether the goal is accepted. + */ rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); + + /** + * @brief Handles cancellation of an active LOS guidance goal. + * @param goal_handle Handle to the goal being cancelled. + * @return rclcpp_action::CancelResponse Response indicating whether cancellation is accepted. + */ rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle); + + /** + * @brief Handles an accepted LOS guidance goal. + * @param goal_handle Handle to the accepted goal. + */ void handle_accepted(const std::shared_ptr> goal_handle); + vortex_msgs::action::LOSGuidance>> goal_handle); + + /** + * @brief Executes the LOS guidance action. + * @param goal_handle Handle to the active LOS guidance goal. + */ void execute(const std::shared_ptr> goal_handle); - // Service Functions + /** + * @brief Service callback for changing the active LOS guidance method. + * @param request Service request containing the desired LOS mode. + * @param response Service response indicating whether the mode change succeeded. + */ void set_los_mode( const std::shared_ptr request, std::shared_ptr response); - // Publish Functions + /** + * @brief Publishes debug information about the current vehicle state. + * @param current_pose Current vehicle pose used for debug publishing. + */ void publish_state_debug( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose); + + /** + * @brief Fills a LOS guidance reference message from computed outputs and inputs. + * @param output Calculated LOS guidance outputs. + * @param inputs Current LOS guidance inputs. + * @return vortex_msgs::msg::LOSGuidance Populated LOS guidance reference message. + */ vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output, types::Inputs inputs); - // State Flags bool has_active_segment_{false}; - // ROS Interfaces rclcpp_action::Server::SharedPtr action_server_; rclcpp::Service::SharedPtr los_mode_service_; @@ -97,17 +193,14 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp::TimerBase::SharedPtr reference_pub_timer_; rclcpp::CallbackGroup::SharedPtr cb_group_; - // Timing and Synchronization std::chrono::milliseconds time_step_; std::mutex mutex_; - // Action State rclcpp_action::GoalUUID preempted_goal_id_; std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle_; - // Guidance State types::Inputs path_inputs_{}; double u_desired_{}; double goal_reached_tol_{}; @@ -120,16 +213,14 @@ class LosGuidanceNode : public rclcpp::Node { double surge_rate_limit_{}; types::ActiveLosMethod method_{}; - // Guidance Modules std::unique_ptr adaptive_los_{}; std::unique_ptr integral_los_{}; std::unique_ptr proportional_los_{}; std::unique_ptr vector_field_los_{}; - // Debug Data nav_msgs::msg::Odometry::SharedPtr debug_current_odom_{}; }; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ \ No newline at end of file diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 772474673..0ba8ac78e 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -31,7 +31,7 @@ def launch_setup(context, *args, **kwargs): os.path.join(stonefish_dir, "launch", "simulation.launch.py") ), launch_arguments={ - "scenario": "default", + "scenario": "nautilus_no_gpu", "rendering": "true", }.items(), ) @@ -124,8 +124,8 @@ def generate_launch_description(): + [ DeclareLaunchArgument( "test_scenario", - default_value="square", - description="Scenario to run: square, circle, test_pitch, opposite_point", + default_value="4_corner", + description="Scenario to run: 4_corner, circle, test_pitch, opposite_point", ), OpaqueFunction(function=launch_setup), ] diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py index 95edef2d6..a79e7d99c 100644 --- a/guidance/los_guidance/scripts/test_scenarios.py +++ b/guidance/los_guidance/scripts/test_scenarios.py @@ -11,8 +11,8 @@ class WaypointTest(Node): def __init__(self): super().__init__("waypoint_test_client") - self.declare_parameter("test_scenario", "square") - self.declare_parameter("drone", "orca") + self.declare_parameter("test_scenario", "4_corner") + self.declare_parameter("drone", "nautilus") self.test_scenario = self.get_parameter("test_scenario").value self.drone = self.get_parameter("drone").value @@ -41,7 +41,7 @@ def __init__(self): self.send_next_goal() def generate_waypoints(self, test_scenario): - if test_scenario == "square": + if test_scenario == "4_corner": s = self.square_size d = self.depth return [ @@ -66,7 +66,7 @@ def generate_waypoints(self, test_scenario): elif test_scenario == "test_pitch": # 0 = water surface, do not go above - # 3 = seabed/ground, do not touch + # this test scenario has no seabed, so z can be however we need. # Keep all depths safely between these return [ (3.0, 0.0, 1.0), # slight up @@ -84,9 +84,9 @@ def generate_waypoints(self, test_scenario): else: self.get_logger().warn( - f"Unknown test_scenario '{test_scenario}', defaulting to square" + f"Unknown test_scenario '{test_scenario}', defaulting to 4_corner" ) - return self.generate_waypoints("square") + return self.generate_waypoints("4_corner") def send_next_goal(self): if self.current_index >= len(self.waypoints): diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index db6177b1c..62f179dd0 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -4,24 +4,21 @@ namespace vortex::guidance::los { -// Constructor AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) : params_{params} {} -// Angle Update void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; const double dy = inputs.next_point.y - inputs.prev_point.y; const double dz = inputs.next_point.z - inputs.prev_point.z; - pi_h_ = std::atan2(dy, dx); - pi_v_ = std::atan2(-dz, std::sqrt(dx * dx + dy * dy)); + path_heading_ = std::atan2(dy, dx); + path_pitch_ = std::atan2(-dz, std::sqrt(dx * dx + dy * dy)); - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); } -// Cross-Track Error Calculation const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( const types::Inputs& inputs) { const types::Point difference = inputs.current_position - inputs.prev_point; @@ -33,7 +30,6 @@ const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( return types::CrossTrackError::from_vector(cross_track_error); } -// Adaptive Estimate Update void AdaptiveLOSGuidance::update_adaptive_estimates( const types::CrossTrackError& cross_track_error) { const double denom_h = @@ -43,10 +39,10 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( std::sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + cross_track_error.z_e * cross_track_error.z_e); - const double beta_dot = params_.gamma_h * + const double beta_dot = params_.adaptation_gain_h * (params_.lookahead_distance_h / denom_h) * cross_track_error.y_e; - const double alpha_dot = params_.gamma_v * + const double alpha_dot = params_.adaptation_gain_v * (params_.lookahead_distance_v / denom_v) * cross_track_error.z_e; @@ -54,7 +50,6 @@ void AdaptiveLOSGuidance::update_adaptive_estimates( alpha_c_hat_ += alpha_dot * params_.time_step; } -// Output Calculation types::Outputs AdaptiveLOSGuidance::calculate_outputs( const types::Inputs& inputs) { update_angles(inputs); @@ -64,15 +59,15 @@ types::Outputs AdaptiveLOSGuidance::calculate_outputs( update_adaptive_estimates(cross_track_error); - const double psi_d = - pi_h_ - beta_c_hat_ - + const double desired_yaw = + path_heading_ - beta_c_hat_ - std::atan(cross_track_error.y_e / params_.lookahead_distance_h); - const double theta_d = - pi_v_ + alpha_c_hat_ + + const double desired_pitch = + path_pitch_ + alpha_c_hat_ + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); - return types::Outputs{psi_d, theta_d}; + return types::Outputs{desired_yaw, desired_pitch}; } } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index c8f0b4d59..c6338d7e1 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -5,17 +5,17 @@ namespace vortex::guidance::los { // Constructor IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) : m_params{params} {} - + // Angle Update void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + path_heading_ = std::atan2(difference.y, difference.x); + path_pitch_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); } // Cross-Track Error Calculation @@ -39,18 +39,18 @@ types::Outputs IntegralLOSGuidance::calculate_outputs( const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); - int_h += cross_track_error.y_e * m_params.time_step; - int_v += cross_track_error.z_e * m_params.time_step; + integrated_horizontal_error_ += cross_track_error.y_e * m_params.time_step; + integrated_vertical_error_ += cross_track_error.z_e * m_params.time_step; const double u_h = - m_params.k_p_h * cross_track_error.y_e + m_params.k_i_h * int_h; + m_params.proportional_gain_h * cross_track_error.y_e + m_params.integral_gain_h * integrated_horizontal_error_; const double u_v = - m_params.k_p_v * cross_track_error.z_e + m_params.k_i_v * int_v; + m_params.proportional_gain_v * cross_track_error.z_e + m_params.integral_gain_v * integrated_vertical_error_; - const double psi_d = pi_h_ - std::atan(u_h); - const double theta_d = pi_v_ + std::atan(u_v); + const double desired_yaw = path_heading_ - std::atan(u_h); + const double desired_pitch = path_pitch_ + std::atan(u_v); - return types::Outputs{psi_d, theta_d}; + return types::Outputs{desired_yaw, desired_pitch}; } } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index f76df385c..240137271 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -6,17 +6,17 @@ namespace vortex::guidance::los { ProportionalLOSGuidance::ProportionalLOSGuidance( const ProportionalLosParams& params) : m_params{params} {} - + // Angle Update void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + path_heading_ = std::atan2(difference.y, difference.x); + path_pitch_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); } // Cross-Track Error Calculation @@ -43,10 +43,10 @@ types::Outputs ProportionalLOSGuidance::calculate_outputs( const double k_p_h = 1.0 / m_params.lookahead_distance_h; const double k_p_v = 1.0 / m_params.lookahead_distance_v; - const double psi_d = pi_h_ - std::atan(k_p_h * cross_track_error.y_e); - const double theta_d = pi_v_ + std::atan(k_p_v * cross_track_error.z_e); + const double desired_yaw = path_heading_ - std::atan(k_p_h * cross_track_error.y_e); + const double desired_pitch = path_pitch_ + std::atan(k_p_v * cross_track_error.z_e); - return types::Outputs{psi_d, theta_d}; + return types::Outputs{desired_yaw, desired_pitch}; } } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp index edf352a6e..d2b34355e 100644 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -6,17 +6,17 @@ namespace vortex::guidance::los { VectorFieldLOSGuidance::VectorFieldLOSGuidance( const VectorFieldLosParams& params) : m_params{params} {} - + // Angle Update void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; - pi_h_ = std::atan2(difference.y, difference.x); - pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + path_heading_ = std::atan2(difference.y, difference.x); + path_pitch_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + difference.y * difference.y)); - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); + rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); } // Cross-Track Error Calculation @@ -41,15 +41,15 @@ types::Outputs VectorFieldLOSGuidance::calculate_outputs( calculate_crosstrack_error(inputs); const double approach_h = m_params.max_approach_angle_h * (2.0 / M_PI) * - std::atan(m_params.k_p_h * cross_track_error.y_e); + std::atan(m_params.proportional_gain_h * cross_track_error.y_e); const double approach_v = m_params.max_approach_angle_v * (2.0 / M_PI) * - std::atan(m_params.k_p_v * cross_track_error.z_e); + std::atan(m_params.proportional_gain_v * cross_track_error.z_e); - const double psi_d = pi_h_ - approach_h; - const double theta_d = pi_v_ - approach_v; + const double desired_yaw = path_heading_ - approach_h; + const double desired_pitch = path_pitch_ - approach_v; - return types::Outputs{psi_d, theta_d}; + return types::Outputs{desired_yaw, desired_pitch}; } } // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index de3183b62..186337fed 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -7,6 +7,12 @@ #include "los_guidance/lib/types.hpp" +#ifdef NDEBUG +constexpr bool debug = false; +#else +constexpr bool debug = true; +#endif + const auto start_message = R"( _ ___ ____ ____ _ _ | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ @@ -23,7 +29,7 @@ LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) : Node("los_guidance_node", options) { double time_step_s = this->declare_parameter("time_step"); time_step_ = - std::chrono::milliseconds(static_cast(time_step_s * 1000)); + std::chrono::milliseconds(static_cast(time_step_s * 1000)); //in const std::string yaml_path = this->declare_parameter("los_config_file"); @@ -42,7 +48,6 @@ LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) spdlog::info(start_message); } -// ROS Setup void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); @@ -111,7 +116,6 @@ void LosGuidanceNode::set_service_server() { std::placeholders::_1, std::placeholders::_2)); } -// Guidance Configuration void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { auto adaptive_los_config = config["adaptive_los"]; auto params = AdaptiveLosParams{}; @@ -120,8 +124,8 @@ void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { adaptive_los_config["lookahead_distance_h"].as(); params.lookahead_distance_v = adaptive_los_config["lookahead_distance_v"].as(); - params.gamma_h = adaptive_los_config["gamma_h"].as(); - params.gamma_v = adaptive_los_config["gamma_v"].as(); + params.adaptation_gain_h = adaptive_los_config["adaptation_gain_h"].as(); + params.adaptation_gain_v = adaptive_los_config["adaptation_gain_v"].as(); params.time_step = static_cast(time_step_.count()) / 1000.0; adaptive_los_ = std::make_unique(params); @@ -143,10 +147,10 @@ void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; auto params = IntegralLosParams{}; - params.k_p_h = integral_los_config["k_p_h"].as(); - params.k_p_v = integral_los_config["k_p_v"].as(); - params.k_i_h = integral_los_config["k_i_h"].as(); - params.k_i_v = integral_los_config["k_i_v"].as(); + params.proportional_gain_h = integral_los_config["proportional_gain_h"].as(); + params.proportional_gain_v = integral_los_config["proportional_gain_v"].as(); + params.integral_gain_h = integral_los_config["integral_gain_h"].as(); + params.integral_gain_v = integral_los_config["integral_gain_v"].as(); params.time_step = static_cast(time_step_.count()) / 1000.0; integral_los_ = std::make_unique(params); @@ -160,17 +164,17 @@ void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { vector_field_config["max_approach_angle_h"].as(); params.max_approach_angle_v = vector_field_config["max_approach_angle_v"].as(); - params.k_p_h = vector_field_config["k_p_h"].as(); - params.k_p_v = vector_field_config["k_p_v"].as(); + params.proportional_gain_h = vector_field_config["proportional_gain_h"].as(); + params.proportional_gain_v = vector_field_config["proportional_gain_v"].as(); params.time_step = static_cast(time_step_.count()) / 1000.0; vector_field_los_ = std::make_unique(params); } -// Topic Callbacks void LosGuidanceNode::waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { - std::lock_guard lock(mutex_); + + std::unique_lock lock(mutex_); const auto new_wp = types::Point::point_from_ros(wp_msg->point); @@ -183,38 +187,41 @@ void LosGuidanceNode::waypoint_callback( path_inputs_.next_point = new_wp; } - spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, - new_wp.z); + lock.unlock(); + + spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, new_wp.z); } void LosGuidanceNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); path_inputs_.current_position = types::Point::point_from_ros(current_pose->pose.pose.position); + lock.unlock(); } void LosGuidanceNode::odom_callback( const nav_msgs::msg::Odometry::SharedPtr msg) { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); debug_current_odom_ = msg; + lock.unlock(); } -// Action Server Callbacks rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { (void)goal; { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); if (goal_handle_) { if (goal_handle_->is_active()) { spdlog::info("Aborting current goal and accepting new goal"); preempted_goal_id_ = goal_handle_->get_goal_id(); } } + lock.unlock(); } spdlog::info("Accepted goal request"); @@ -237,7 +244,6 @@ void LosGuidanceNode::handle_accepted( std::thread{[this, goal_handle]() { execute(goal_handle); }}.detach(); } -// Service Callback void LosGuidanceNode::set_los_mode( const std::shared_ptr request, std::shared_ptr response) { @@ -246,7 +252,6 @@ void LosGuidanceNode::set_los_mode( response->success = true; } -// Message Helpers vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs, types::Inputs inputs) { @@ -299,7 +304,7 @@ YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { } void LosGuidanceNode::parse_common_config(YAML::Node common_config) { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); u_desired_ = common_config["u_desired"].as(); max_pitch_angle_ = common_config["max_pitch_angle"].as(); goal_reached_tol_ = common_config["goal_reached_tol"].as(); @@ -310,16 +315,18 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { method_ = static_cast( common_config["active_los_method"].as()); + + lock.unlock(); } -// Goal Execution void LosGuidanceNode::execute( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); this->goal_handle_ = goal_handle; + lock.unlock(); } spdlog::info("Executing goal"); @@ -342,14 +349,15 @@ void LosGuidanceNode::execute( rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - while (rclcpp::ok()) { + while (rclcpp::ok()) { { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); if (goal_handle->get_goal_id() == preempted_goal_id_) { result->success = false; goal_handle->abort(result); return; } + lock.unlock(); } if (goal_handle->is_canceling()) { @@ -361,8 +369,9 @@ void LosGuidanceNode::execute( types::Inputs inputs_copy; { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); inputs_copy = path_inputs_; + lock.unlock(); } types::Outputs outputs; @@ -386,11 +395,25 @@ void LosGuidanceNode::execute( goal_handle->abort(result); return; } + auto reference_msg = std::make_unique( + fill_los_reference(outputs, inputs_copy)); + + if ((inputs_copy.current_position - inputs_copy.next_point) + .as_vector() + .norm() < goal_reached_tol_) { + + reference_msg->pitch = 0.0; + reference_msg->surge = 0.0; - vortex_msgs::msg::LOSGuidance reference_msg = - fill_los_reference(outputs, inputs_copy); + result->success = true; + goal_handle->succeed(result); + spdlog::info("Goal reached"); + return; + } + + reference_pub_->publish(std::move(reference_msg)); - if (debug_current_odom_) { + if (debug_current_odom_ && debug) { const auto& v = debug_current_odom_->twist.twist.linear; double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); @@ -409,22 +432,6 @@ void LosGuidanceNode::execute( state_debug_pub_->publish(state_debug_msg); } - reference_pub_->publish(reference_msg); - - if ((inputs_copy.current_position - inputs_copy.next_point) - .as_vector() - .norm() < goal_reached_tol_) { - auto stop_ref = reference_msg; - stop_ref.surge = 0.0; - stop_ref.pitch = 0.0; - stop_ref.yaw = reference_msg.yaw; - - reference_pub_->publish(stop_ref); - result->success = true; - goal_handle->succeed(result); - spdlog::info("Goal reached"); - return; - } loop_rate.sleep(); } diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp index dbf63eb04..b93b5363a 100644 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -1,7 +1,7 @@ #include "los_guidance/lib/adaptive_los.hpp" #include -namespace vortex::guidance::los { // new namespace added los +namespace vortex::guidance::los { class AdaptiveLosTest : public ::testing::Test { protected: @@ -9,10 +9,10 @@ class AdaptiveLosTest : public ::testing::Test { AdaptiveLosParams get_params() { AdaptiveLosParams p; - p.lookahead_distance_h = 1.0; - p.lookahead_distance_v = 1.0; - p.gamma_h = 1.0; - p.gamma_v = 1.0; + p.lookahead_distance_h = 0.9; + p.lookahead_distance_v = 1.4; + p.adaptation_gain_h = 0.03; + p.adaptation_gain_v = 0.02; p.time_step = 0.01; return p; } @@ -70,7 +70,7 @@ TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { EXPECT_LT(O.theta_d, 1.57); } -// Test commanded angles when drone is over the track +// Test commanded angles when drone is above the track TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; @@ -86,7 +86,7 @@ TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { EXPECT_GT(O.theta_d, -1.57); } -// Test commanded angles when drone is over and to the right of the track +// Test commanded angles when drone is above and to the right of the track TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { types::Inputs inputs; diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp index 1999abac8..1388f1f98 100644 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -10,14 +10,14 @@ class IntegralLosTest : public ::testing::Test { IntegralLosParams get_params() { IntegralLosParams params; - params.k_i_h = 0.1; // needs tuning - params.k_i_v = 0.1; // needs tuning - params.k_p_h = 0.667; // needs tuning - params.k_p_v = 0.582; // needs tuning + params.integral_gain_h = 0.5; + params.integral_gain_v = 0.5; + params.proportional_gain_h = 0.1; + params.proportional_gain_v = 0.1; params.time_step = 0.01; return params; } - + IntegralLOSGuidance Ilos_; const double tol = 1e-9; }; @@ -71,7 +71,7 @@ TEST_F(IntegralLosTest, T08_test_commanded_angles) { EXPECT_LT(O.theta_d, 1.57); } -// Test commanded angles when drone is over the track +// Test commanded angles when drone is above the track TEST_F(IntegralLosTest, T09_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; @@ -87,7 +87,7 @@ TEST_F(IntegralLosTest, T09_test_commanded_angles) { EXPECT_GT(O.theta_d, -1.57); } -// Test commanded angles when drone is over and to the right of the track +// Test commanded angles when drone is above and to the right of the track TEST_F(IntegralLosTest, T10_test_commanded_angles) { types::Inputs inputs; diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp index b35a3ba6f..65136a1d3 100644 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -9,15 +9,15 @@ class ProportionalLosTest : public ::testing::Test { ProportionalLosParams get_params() { ProportionalLosParams params; - params.lookahead_distance_h = 10.0; - params.lookahead_distance_v = 10.0; + params.lookahead_distance_h = 0.74; + params.lookahead_distance_v = 0.8; return params; } ProportionalLOSGuidance Plos_; const double tol = 1e-9; }; - + // Test commanded angles when drone is to the right of the track TEST_F(ProportionalLosTest, T06_test_commanded_angles) { types::Inputs inputs; @@ -67,7 +67,7 @@ TEST_F(ProportionalLosTest, T08_test_commanded_angles) { EXPECT_LT(O.theta_d, 1.57); } -// Test commanded angles when drone is over the track +// Test commanded angles when drone is above the track TEST_F(ProportionalLosTest, T09_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; @@ -83,7 +83,7 @@ TEST_F(ProportionalLosTest, T09_test_commanded_angles) { EXPECT_GT(O.theta_d, -1.57); } -// Test commanded angles when drone is over and to the right of the track +// Test commanded angles when drone is above and to the right of the track TEST_F(ProportionalLosTest, T10_test_commanded_angles) { types::Inputs inputs; diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp index e11f7d3dd..81dfda2a5 100644 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -10,10 +10,10 @@ class VectorFieldLosTest : public ::testing::Test { VectorFieldLosParams get_params() { VectorFieldLosParams params; - params.max_approach_angle_h = 30.0 * M_PI / 180.0; // 30 degrees in rad - params.max_approach_angle_v = 20.0 * M_PI / 180.0; // 20 degrees in rad - params.k_p_h = 1.5; // needs tuning - params.k_p_v = 0.9; // needs tuning + params.max_approach_angle_h = 1.0; + params.max_approach_angle_v = 1.0; + params.proportional_gain_h = 1.5; + params.proportional_gain_v = 0.9; params.time_step = 0.01; return params; } @@ -21,7 +21,7 @@ class VectorFieldLosTest : public ::testing::Test { VectorFieldLOSGuidance Vflos_; const double tol = 1e-9; }; - + // Test commanded angles when drone is to the right of the track TEST_F(VectorFieldLosTest, T06_test_commanded_angles) { types::Inputs inputs; @@ -73,7 +73,7 @@ TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { EXPECT_GT(O.theta_d, -1.57); } -// Test commanded angles when drone is over the track +// Test commanded angles when drone is above the track TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; @@ -90,7 +90,7 @@ TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { EXPECT_LT(O.theta_d, 1.57); } -// Test commanded angles when drone is over and to the right of the track +// Test commanded angles when drone is above and to the right of the track TEST_F(VectorFieldLosTest, T10_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; From d06dbe022ca9c682f4aeafef43d29438ca6424a3 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sat, 28 Mar 2026 17:36:58 +0100 Subject: [PATCH 67/87] Start: PR review fixes --- guidance/los_guidance/README.md | 2 +- .../los_guidance/config/guidance_params.yaml | 6 +-- .../include/los_guidance/lib/adaptive_los.hpp | 17 ++++---- .../include/los_guidance/lib/integral_los.hpp | 20 +++++---- .../los_guidance/lib/proportional_los.hpp | 18 ++++---- .../include/los_guidance/lib/types.hpp | 2 +- .../los_guidance/lib/vector_field_los.hpp | 20 +++++---- .../include/los_guidance/los_guidance_ros.hpp | 31 +++++++++----- .../los_guidance/scripts/test_scenarios.py | 2 +- .../los_guidance/src/lib/integral_los.cpp | 15 +++---- .../los_guidance/src/lib/proportional_los.cpp | 13 +++--- .../los_guidance/src/lib/vector_field_los.cpp | 17 ++++---- .../los_guidance/src/los_guidance_ros.cpp | 42 +++++++++++-------- .../los_guidance/test/adaptive_los_test.cpp | 2 +- .../los_guidance/test/integral_los_test.cpp | 10 ++--- .../test/proportional_los_test.cpp | 2 +- .../test/vector_field_los_test.cpp | 10 ++--- 17 files changed, 129 insertions(+), 100 deletions(-) diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 40c0a80c2..b482a74e6 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -220,7 +220,7 @@ Proportional LOS is the simplest LOS guidance law. \tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` -where +where - $\Delta_h$ horizontal lookahead distance - $\Delta_v$ vertical lookahead distance diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 82ddec457..8b174e6f9 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,5 +1,5 @@ #/** -# * @file guidance_params.yaml +# * @file guidance_params.yaml # * @brief Configuration parameters for the LOS guidance node. # * # * This file contains the tunable parameters for all supported Line-Of-Sight @@ -21,7 +21,7 @@ # * vector field LOS guidance. # * - `max_approach_angle_v`: Maximum allowed vertical approach angle for # * vector field LOS guidance. -# * - +# * - # */ adaptive_los: lookahead_distance_h: 0.9 @@ -53,4 +53,4 @@ common: slow_approach: false slow_down_distance: 1.5 u_slow_min: 0.1 - surge_rate_limit: 0.1 \ No newline at end of file + surge_rate_limit: 0.1 diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 5fc8eca57..3f7b7f39b 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include "los_guidance/lib/types.hpp" @@ -26,7 +26,7 @@ struct AdaptiveLosParams { double lookahead_distance_v{}; double adaptation_gain_h{}; double adaptation_gain_v{}; - double time_step{}; // in milliseconds + double time_step{}; // in milliseconds }; /** @@ -57,16 +57,17 @@ class AdaptiveLOSGuidance { private: /** - * @brief Updates the path heading and path pitch angles from the active path - * segment. + * @brief Updates the path heading and path pitch angles from the active + * path segment. * @param inputs Input values containing the previous and next path points. */ void update_angles(const types::Inputs& inputs); /** - * @brief Calculates the cross-track error in the path-fixed reference frame. - * @param inputs Input values containing the current vehicle position and path - * information. + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. * @return types::CrossTrackError The calculated horizontal and vertical * cross-track errors. */ @@ -119,4 +120,4 @@ class AdaptiveLOSGuidance { } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ \ No newline at end of file +#endif // LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index bd27e8137..2bf8d951c 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -10,7 +10,7 @@ #include #include -#include "los_guidance/lib/types.hpp" +#include "los_guidance/lib/types.hpp" namespace vortex::guidance::los { @@ -22,7 +22,7 @@ struct IntegralLosParams { double proportional_gain_v{}; double integral_gain_h{}; double integral_gain_v{}; - double time_step{}; // in milliseconds + double time_step{}; // in milliseconds }; /** @@ -53,16 +53,17 @@ class IntegralLOSGuidance { private: /** - * @brief Updates the path heading and path pitch angles from the active path - * segment. + * @brief Updates the path heading and path pitch angles from the active + * path segment. * @param inputs Input values containing the previous and next path points. */ void update_angles(const types::Inputs& inputs); /** - * @brief Calculates the cross-track error in the path-fixed reference frame. - * @param inputs Input values containing the current vehicle position and path - * information. + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. * @return types::CrossTrackError The calculated horizontal and vertical * cross-track errors. */ @@ -100,11 +101,12 @@ class IntegralLOSGuidance { Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; /** - * @brief Rotation representation for the path heading angle about the z-axis. + * @brief Rotation representation for the path heading angle about the + * z-axis. */ Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ \ No newline at end of file +#endif // LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp index 5a4a1a044..cf81bc774 100644 --- a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -12,7 +12,7 @@ #include "los_guidance/lib/types.hpp" -namespace vortex::guidance::los { +namespace vortex::guidance::los { /** * @brief Parameters for the proportional LOS guidance algorithm. @@ -50,16 +50,17 @@ class ProportionalLOSGuidance { private: /** - * @brief Updates the path heading and path pitch angles from the active path - * segment. + * @brief Updates the path heading and path pitch angles from the active + * path segment. * @param inputs Input values containing the previous and next path points. */ void update_angles(const types::Inputs& inputs); /** - * @brief Calculates the cross-track error in the path-fixed reference frame. - * @param inputs Input values containing the current vehicle position and path - * information. + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. * @return types::CrossTrackError The calculated horizontal and vertical * cross-track errors. */ @@ -87,11 +88,12 @@ class ProportionalLOSGuidance { Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; /** - * @brief Rotation representation for the path heading angle about the z-axis. + * @brief Rotation representation for the path heading angle about the + * z-axis. */ Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ \ No newline at end of file +#endif // LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index 3088c5045..76f8fa296 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -94,4 +94,4 @@ enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; } // namespace vortex::guidance::los::types -#endif // LOS_GUIDANCE__LIB__TYPES_HPP_ \ No newline at end of file +#endif // LOS_GUIDANCE__LIB__TYPES_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index 4938392e6..6f4ec133e 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -12,7 +12,7 @@ #include "los_guidance/lib/types.hpp" -namespace vortex::guidance::los { +namespace vortex::guidance::los { /** * @brief Parameters for the vector field LOS guidance algorithm. @@ -22,7 +22,7 @@ struct VectorFieldLosParams { double max_approach_angle_v{}; double proportional_gain_h{}; double proportional_gain_v{}; - double time_step{}; // in milliseconds + double time_step{}; // in milliseconds }; /** @@ -54,16 +54,17 @@ class VectorFieldLOSGuidance { private: /** - * @brief Updates the path heading and path pitch angles from the active path - * segment. + * @brief Updates the path heading and path pitch angles from the active + * path segment. * @param inputs Input values containing the previous and next path points. */ void update_angles(const types::Inputs& inputs); /** - * @brief Calculates the cross-track error in the path-fixed reference frame. - * @param inputs Input values containing the current vehicle position and path - * information. + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. * @return types::CrossTrackError The calculated horizontal and vertical * cross-track errors. */ @@ -91,11 +92,12 @@ class VectorFieldLOSGuidance { Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; /** - * @brief Rotation representation for the path heading angle about the z-axis. + * @brief Rotation representation for the path heading angle about the + * z-axis. */ Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; }; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ \ No newline at end of file +#endif // LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index e241cdd1d..64e261ac0 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -66,8 +66,10 @@ class LosGuidanceNode : public rclcpp::Node { void set_adaptive_los_guidance(YAML::Node config); /** - * @brief Initializes the proportional LOS guidance module from configuration. - * @param config YAML configuration node containing proportional LOS parameters. + * @brief Initializes the proportional LOS guidance module from + * configuration. + * @param config YAML configuration node containing proportional LOS + * parameters. */ void set_proportional_los_guidance(YAML::Node config); @@ -78,8 +80,10 @@ class LosGuidanceNode : public rclcpp::Node { void set_integral_los_guidance(YAML::Node config); /** - * @brief Initializes the vector field LOS guidance module from configuration. - * @param config YAML configuration node containing vector field LOS parameters. + * @brief Initializes the vector field LOS guidance module from + * configuration. + * @param config YAML configuration node containing vector field LOS + * parameters. */ void set_vector_field_guidance(YAML::Node config); @@ -120,7 +124,8 @@ class LosGuidanceNode : public rclcpp::Node { * @brief Handles an incoming LOS guidance action goal request. * @param uuid Unique identifier for the received goal. * @param goal Requested LOS guidance goal. - * @return rclcpp_action::GoalResponse Response indicating whether the goal is accepted. + * @return rclcpp_action::GoalResponse Response indicating whether the goal + * is accepted. */ rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, @@ -129,7 +134,8 @@ class LosGuidanceNode : public rclcpp::Node { /** * @brief Handles cancellation of an active LOS guidance goal. * @param goal_handle Handle to the goal being cancelled. - * @return rclcpp_action::CancelResponse Response indicating whether cancellation is accepted. + * @return rclcpp_action::CancelResponse Response indicating whether + * cancellation is accepted. */ rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr< @@ -141,7 +147,7 @@ class LosGuidanceNode : public rclcpp::Node { * @param goal_handle Handle to the accepted goal. */ void handle_accepted(const std::shared_ptr> goal_handle); + vortex_msgs::action::LOSGuidance>> goal_handle); /** * @brief Executes the LOS guidance action. @@ -153,7 +159,8 @@ class LosGuidanceNode : public rclcpp::Node { /** * @brief Service callback for changing the active LOS guidance method. * @param request Service request containing the desired LOS mode. - * @param response Service response indicating whether the mode change succeeded. + * @param response Service response indicating whether the mode change + * succeeded. */ void set_los_mode( const std::shared_ptr request, @@ -168,10 +175,12 @@ class LosGuidanceNode : public rclcpp::Node { current_pose); /** - * @brief Fills a LOS guidance reference message from computed outputs and inputs. + * @brief Fills a LOS guidance reference message from computed outputs and + * inputs. * @param output Calculated LOS guidance outputs. * @param inputs Current LOS guidance inputs. - * @return vortex_msgs::msg::LOSGuidance Populated LOS guidance reference message. + * @return vortex_msgs::msg::LOSGuidance Populated LOS guidance reference + * message. */ vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output, types::Inputs inputs); @@ -223,4 +232,4 @@ class LosGuidanceNode : public rclcpp::Node { } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ \ No newline at end of file +#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py index a79e7d99c..9b8a82a8c 100644 --- a/guidance/los_guidance/scripts/test_scenarios.py +++ b/guidance/los_guidance/scripts/test_scenarios.py @@ -66,7 +66,7 @@ def generate_waypoints(self, test_scenario): elif test_scenario == "test_pitch": # 0 = water surface, do not go above - # this test scenario has no seabed, so z can be however we need. + # this test scenario has no seabed, so z can be however we need. # Keep all depths safely between these return [ (3.0, 0.0, 1.0), # slight up diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index c6338d7e1..722e3c66f 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -5,14 +5,15 @@ namespace vortex::guidance::los { // Constructor IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) : m_params{params} {} - + // Angle Update void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; path_heading_ = std::atan2(difference.y, difference.x); - path_pitch_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + - difference.y * difference.y)); + path_pitch_ = std::atan2( + -difference.z, + std::sqrt(difference.x * difference.x + difference.y * difference.y)); rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); @@ -42,10 +43,10 @@ types::Outputs IntegralLOSGuidance::calculate_outputs( integrated_horizontal_error_ += cross_track_error.y_e * m_params.time_step; integrated_vertical_error_ += cross_track_error.z_e * m_params.time_step; - const double u_h = - m_params.proportional_gain_h * cross_track_error.y_e + m_params.integral_gain_h * integrated_horizontal_error_; - const double u_v = - m_params.proportional_gain_v * cross_track_error.z_e + m_params.integral_gain_v * integrated_vertical_error_; + const double u_h = m_params.proportional_gain_h * cross_track_error.y_e + + m_params.integral_gain_h * integrated_horizontal_error_; + const double u_v = m_params.proportional_gain_v * cross_track_error.z_e + + m_params.integral_gain_v * integrated_vertical_error_; const double desired_yaw = path_heading_ - std::atan(u_h); const double desired_pitch = path_pitch_ + std::atan(u_v); diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index 240137271..6a5dcddd9 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -6,14 +6,15 @@ namespace vortex::guidance::los { ProportionalLOSGuidance::ProportionalLOSGuidance( const ProportionalLosParams& params) : m_params{params} {} - + // Angle Update void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; path_heading_ = std::atan2(difference.y, difference.x); - path_pitch_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + - difference.y * difference.y)); + path_pitch_ = std::atan2( + -difference.z, + std::sqrt(difference.x * difference.x + difference.y * difference.y)); rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); @@ -43,8 +44,10 @@ types::Outputs ProportionalLOSGuidance::calculate_outputs( const double k_p_h = 1.0 / m_params.lookahead_distance_h; const double k_p_v = 1.0 / m_params.lookahead_distance_v; - const double desired_yaw = path_heading_ - std::atan(k_p_h * cross_track_error.y_e); - const double desired_pitch = path_pitch_ + std::atan(k_p_v * cross_track_error.z_e); + const double desired_yaw = + path_heading_ - std::atan(k_p_h * cross_track_error.y_e); + const double desired_pitch = + path_pitch_ + std::atan(k_p_v * cross_track_error.z_e); return types::Outputs{desired_yaw, desired_pitch}; } diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp index d2b34355e..d5fed4420 100644 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -6,14 +6,15 @@ namespace vortex::guidance::los { VectorFieldLOSGuidance::VectorFieldLOSGuidance( const VectorFieldLosParams& params) : m_params{params} {} - + // Angle Update void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { const types::Point difference = inputs.next_point - inputs.prev_point; path_heading_ = std::atan2(difference.y, difference.x); - path_pitch_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + - difference.y * difference.y)); + path_pitch_ = std::atan2( + -difference.z, + std::sqrt(difference.x * difference.x + difference.y * difference.y)); rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); @@ -40,11 +41,13 @@ types::Outputs VectorFieldLOSGuidance::calculate_outputs( const types::CrossTrackError cross_track_error = calculate_crosstrack_error(inputs); - const double approach_h = m_params.max_approach_angle_h * (2.0 / M_PI) * - std::atan(m_params.proportional_gain_h * cross_track_error.y_e); + const double approach_h = + m_params.max_approach_angle_h * (2.0 / M_PI) * + std::atan(m_params.proportional_gain_h * cross_track_error.y_e); - const double approach_v = m_params.max_approach_angle_v * (2.0 / M_PI) * - std::atan(m_params.proportional_gain_v * cross_track_error.z_e); + const double approach_v = + m_params.max_approach_angle_v * (2.0 / M_PI) * + std::atan(m_params.proportional_gain_v * cross_track_error.z_e); const double desired_yaw = path_heading_ - approach_h; const double desired_pitch = path_pitch_ - approach_v; diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 186337fed..f50437fec 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,16 +1,16 @@ #include "los_guidance/los_guidance_ros.hpp" #include -#include #include #include +#include #include #include "los_guidance/lib/types.hpp" #ifdef NDEBUG -constexpr bool debug = false; +constexpr bool debug = false; #else -constexpr bool debug = true; +constexpr bool debug = true; #endif const auto start_message = R"( @@ -29,7 +29,7 @@ LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) : Node("los_guidance_node", options) { double time_step_s = this->declare_parameter("time_step"); time_step_ = - std::chrono::milliseconds(static_cast(time_step_s * 1000)); //in + std::chrono::milliseconds(static_cast(time_step_s * 1000)); // in const std::string yaml_path = this->declare_parameter("los_config_file"); @@ -124,8 +124,10 @@ void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { adaptive_los_config["lookahead_distance_h"].as(); params.lookahead_distance_v = adaptive_los_config["lookahead_distance_v"].as(); - params.adaptation_gain_h = adaptive_los_config["adaptation_gain_h"].as(); - params.adaptation_gain_v = adaptive_los_config["adaptation_gain_v"].as(); + params.adaptation_gain_h = + adaptive_los_config["adaptation_gain_h"].as(); + params.adaptation_gain_v = + adaptive_los_config["adaptation_gain_v"].as(); params.time_step = static_cast(time_step_.count()) / 1000.0; adaptive_los_ = std::make_unique(params); @@ -147,10 +149,14 @@ void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; auto params = IntegralLosParams{}; - params.proportional_gain_h = integral_los_config["proportional_gain_h"].as(); - params.proportional_gain_v = integral_los_config["proportional_gain_v"].as(); - params.integral_gain_h = integral_los_config["integral_gain_h"].as(); - params.integral_gain_v = integral_los_config["integral_gain_v"].as(); + params.proportional_gain_h = + integral_los_config["proportional_gain_h"].as(); + params.proportional_gain_v = + integral_los_config["proportional_gain_v"].as(); + params.integral_gain_h = + integral_los_config["integral_gain_h"].as(); + params.integral_gain_v = + integral_los_config["integral_gain_v"].as(); params.time_step = static_cast(time_step_.count()) / 1000.0; integral_los_ = std::make_unique(params); @@ -164,8 +170,10 @@ void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { vector_field_config["max_approach_angle_h"].as(); params.max_approach_angle_v = vector_field_config["max_approach_angle_v"].as(); - params.proportional_gain_h = vector_field_config["proportional_gain_h"].as(); - params.proportional_gain_v = vector_field_config["proportional_gain_v"].as(); + params.proportional_gain_h = + vector_field_config["proportional_gain_h"].as(); + params.proportional_gain_v = + vector_field_config["proportional_gain_v"].as(); params.time_step = static_cast(time_step_.count()) / 1000.0; vector_field_los_ = std::make_unique(params); @@ -173,7 +181,6 @@ void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { void LosGuidanceNode::waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { - std::unique_lock lock(mutex_); const auto new_wp = types::Point::point_from_ros(wp_msg->point); @@ -189,7 +196,8 @@ void LosGuidanceNode::waypoint_callback( lock.unlock(); - spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, new_wp.z); + spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, + new_wp.z); } void LosGuidanceNode::pose_callback( @@ -349,7 +357,7 @@ void LosGuidanceNode::execute( rclcpp::Rate loop_rate(1000.0 / time_step_.count()); - while (rclcpp::ok()) { + while (rclcpp::ok()) { { std::unique_lock lock(mutex_); if (goal_handle->get_goal_id() == preempted_goal_id_) { @@ -401,7 +409,6 @@ void LosGuidanceNode::execute( if ((inputs_copy.current_position - inputs_copy.next_point) .as_vector() .norm() < goal_reached_tol_) { - reference_msg->pitch = 0.0; reference_msg->surge = 0.0; @@ -410,7 +417,7 @@ void LosGuidanceNode::execute( spdlog::info("Goal reached"); return; } - + reference_pub_->publish(std::move(reference_msg)); if (debug_current_odom_ && debug) { @@ -432,7 +439,6 @@ void LosGuidanceNode::execute( state_debug_pub_->publish(state_debug_msg); } - loop_rate.sleep(); } } diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp index b93b5363a..8c9b8e598 100644 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -1,7 +1,7 @@ #include "los_guidance/lib/adaptive_los.hpp" #include -namespace vortex::guidance::los { +namespace vortex::guidance::los { class AdaptiveLosTest : public ::testing::Test { protected: diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp index 1388f1f98..851c10b84 100644 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -10,14 +10,14 @@ class IntegralLosTest : public ::testing::Test { IntegralLosParams get_params() { IntegralLosParams params; - params.integral_gain_h = 0.5; - params.integral_gain_v = 0.5; - params.proportional_gain_h = 0.1; - params.proportional_gain_v = 0.1; + params.integral_gain_h = 0.5; + params.integral_gain_v = 0.5; + params.proportional_gain_h = 0.1; + params.proportional_gain_v = 0.1; params.time_step = 0.01; return params; } - + IntegralLOSGuidance Ilos_; const double tol = 1e-9; }; diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp index 65136a1d3..e26695d32 100644 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -17,7 +17,7 @@ class ProportionalLosTest : public ::testing::Test { ProportionalLOSGuidance Plos_; const double tol = 1e-9; }; - + // Test commanded angles when drone is to the right of the track TEST_F(ProportionalLosTest, T06_test_commanded_angles) { types::Inputs inputs; diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp index 81dfda2a5..10c316352 100644 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -10,10 +10,10 @@ class VectorFieldLosTest : public ::testing::Test { VectorFieldLosParams get_params() { VectorFieldLosParams params; - params.max_approach_angle_h = 1.0; - params.max_approach_angle_v = 1.0; - params.proportional_gain_h = 1.5; - params.proportional_gain_v = 0.9; + params.max_approach_angle_h = 1.0; + params.max_approach_angle_v = 1.0; + params.proportional_gain_h = 1.5; + params.proportional_gain_v = 0.9; params.time_step = 0.01; return params; } @@ -21,7 +21,7 @@ class VectorFieldLosTest : public ::testing::Test { VectorFieldLOSGuidance Vflos_; const double tol = 1e-9; }; - + // Test commanded angles when drone is to the right of the track TEST_F(VectorFieldLosTest, T06_test_commanded_angles) { types::Inputs inputs; From e87c62513a7decef19ff7dc3073b116729586659 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 29 Mar 2026 23:23:26 +0200 Subject: [PATCH 68/87] Add:A check for feasible goal --- .../los_guidance/config/guidance_params.yaml | 6 +- .../include/los_guidance/los_guidance_ros.hpp | 32 +- .../launch/guidance_test.launch.py | 6 +- .../los_guidance/src/lib/adaptive_los.cpp | 9 +- .../los_guidance/src/lib/integral_los.cpp | 9 +- .../los_guidance/src/lib/proportional_los.cpp | 8 +- .../los_guidance/src/lib/vector_field_los.cpp | 10 +- .../los_guidance/src/los_guidance_ros.cpp | 292 +++++++++++------- guidance/los_guidance/test/CMakeLists.txt | 1 + .../los_guidance/test/adaptive_los_test.cpp | 10 +- .../los_guidance/test/integral_los_test.cpp | 10 +- .../test/los_invalid_params_test.cpp | 56 ++++ .../test/proportional_los_test.cpp | 10 +- .../test/vector_field_los_test.cpp | 10 +- 14 files changed, 320 insertions(+), 149 deletions(-) create mode 100644 guidance/los_guidance/test/los_invalid_params_test.cpp diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 8b174e6f9..b27d8e5b2 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -50,7 +50,5 @@ common: u_desired: 0.3 goal_reached_tol: 0.20 max_pitch_angle: 0.96 - slow_approach: false - slow_down_distance: 1.5 - u_slow_min: 0.1 - surge_rate_limit: 0.1 + missed_goal_distance_margin: 1.0 + missed_goal_timeout: 10.0 diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 64e261ac0..d7409f72f 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -182,8 +182,25 @@ class LosGuidanceNode : public rclcpp::Node { * @return vortex_msgs::msg::LOSGuidance Populated LOS guidance reference * message. */ - vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output, - types::Inputs inputs); + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); + + /** + * @brief Checks if the given LOS guidance goal is feasible based on the + * provided inputs. + * @param inputs Current LOS guidance inputs. + * @return true if the goal is feasible, false otherwise. + */ + bool is_goal_feasible( + const types::Inputs& inputs, + std::shared_ptr goal); + + /** + * @brief Checks if the LOS guidance goal has been missed based on the + * provided inputs. + * @param inputs Current LOS guidance inputs. + * @return true if the goal is missed, false otherwise. + */ + bool is_goal_missed(const types::Inputs& inputs); bool has_active_segment_{false}; @@ -214,14 +231,13 @@ class LosGuidanceNode : public rclcpp::Node { double u_desired_{}; double goal_reached_tol_{}; double max_pitch_angle_{}; - bool slow_approach_{}; - double slow_down_distance_{}; - bool surge_initialized_{false}; - double u_slow_min_{}; - double commanded_surge_{}; - double surge_rate_limit_{}; types::ActiveLosMethod method_{}; + double nearest_been_to_goal_{std::numeric_limits::max()}; + double time_since_nearest_goal_{}; + double missed_goal_distance_margin_{}; + double missed_goal_timeout_{}; + std::unique_ptr adaptive_los_{}; std::unique_ptr integral_los_{}; std::unique_ptr proportional_los_{}; diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 0ba8ac78e..18b18a2ec 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -31,7 +31,7 @@ def launch_setup(context, *args, **kwargs): os.path.join(stonefish_dir, "launch", "simulation.launch.py") ), launch_arguments={ - "scenario": "nautilus_no_gpu", + "scenario": "default", "rendering": "true", }.items(), ) @@ -58,7 +58,7 @@ def launch_setup(context, *args, **kwargs): }.items(), ) - orca_sim = TimerAction( + drone_sim = TimerAction( period=12.0, actions=[ IncludeLaunchDescription( @@ -112,7 +112,7 @@ def launch_setup(context, *args, **kwargs): stonefish_sim, los_guidance_launch, velocity_controller_launch, - orca_sim, + drone_sim, set_autonomy, run_test_scenario, ] diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 62f179dd0..633c966e3 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -5,7 +5,14 @@ namespace vortex::guidance::los { AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) - : params_{params} {} + : params_{params} { + if (params.lookahead_distance_h <= 0.0 || + params.lookahead_distance_v <= 0.0 || params.adaptation_gain_h <= 0.0 || + params.adaptation_gain_v <= 0.0 || params.time_step <= 0.0) { + throw std::invalid_argument( + "AdaptiveLOSGuidance: all params must be > 0"); + } +} void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp index 722e3c66f..9add664c5 100644 --- a/guidance/los_guidance/src/lib/integral_los.cpp +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -4,7 +4,14 @@ namespace vortex::guidance::los { // Constructor IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) - : m_params{params} {} + : m_params(params) { + if (params.proportional_gain_h <= 0.0 || + params.proportional_gain_v <= 0.0 || params.integral_gain_h <= 0.0 || + params.integral_gain_v <= 0.0 || params.time_step <= 0.0) { + throw std::invalid_argument( + "IntegralLOSGuidance: all params must be > 0"); + } +} // Angle Update void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp index 6a5dcddd9..4cb0a2386 100644 --- a/guidance/los_guidance/src/lib/proportional_los.cpp +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -5,7 +5,13 @@ namespace vortex::guidance::los { // Constructor ProportionalLOSGuidance::ProportionalLOSGuidance( const ProportionalLosParams& params) - : m_params{params} {} + : m_params(params) { + if (params.lookahead_distance_h <= 0.0 || + params.lookahead_distance_v <= 0.0) { + throw std::invalid_argument( + "ProportionalLOSGuidance: all params must be > 0"); + } +} // Angle Update void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp index d5fed4420..ca1e9d267 100644 --- a/guidance/los_guidance/src/lib/vector_field_los.cpp +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -5,7 +5,15 @@ namespace vortex::guidance::los { // Constructor VectorFieldLOSGuidance::VectorFieldLOSGuidance( const VectorFieldLosParams& params) - : m_params{params} {} + : m_params(params) { + if (params.max_approach_angle_h <= 0.0 || + params.max_approach_angle_v <= 0.0 || + params.proportional_gain_h <= 0.0 || + params.proportional_gain_v <= 0.0 || params.time_step <= 0.0) { + throw std::invalid_argument( + "VectorFieldLOSGuidance: all params must be > 0"); + } +} // Angle Update void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index f50437fec..2d8a31def 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -28,8 +29,8 @@ namespace vortex::guidance::los { LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) : Node("los_guidance_node", options) { double time_step_s = this->declare_parameter("time_step"); - time_step_ = - std::chrono::milliseconds(static_cast(time_step_s * 1000)); // in + time_step_ = std::chrono::milliseconds(static_cast( + time_step_s * 1000)); // Convert seconds to milliseconds const std::string yaml_path = this->declare_parameter("los_config_file"); @@ -120,63 +121,88 @@ void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { auto adaptive_los_config = config["adaptive_los"]; auto params = AdaptiveLosParams{}; - params.lookahead_distance_h = - adaptive_los_config["lookahead_distance_h"].as(); - params.lookahead_distance_v = - adaptive_los_config["lookahead_distance_v"].as(); - params.adaptation_gain_h = - adaptive_los_config["adaptation_gain_h"].as(); - params.adaptation_gain_v = - adaptive_los_config["adaptation_gain_v"].as(); - params.time_step = static_cast(time_step_.count()) / 1000.0; - - adaptive_los_ = std::make_unique(params); + try { + params.lookahead_distance_h = + adaptive_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + adaptive_los_config["lookahead_distance_v"].as(); + params.adaptation_gain_h = + adaptive_los_config["adaptation_gain_h"].as(); + params.adaptation_gain_v = + adaptive_los_config["adaptation_gain_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + adaptive_los_ = std::make_unique(params); + + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load adaptive_los parameters: ") + e.what()); + } } void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { auto proportional_los_config = config["prop_los"]; auto params = ProportionalLosParams{}; - params.lookahead_distance_h = - proportional_los_config["lookahead_distance_h"].as(); - params.lookahead_distance_v = - proportional_los_config["lookahead_distance_v"].as(); + try { + params.lookahead_distance_h = + proportional_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + proportional_los_config["lookahead_distance_v"].as(); + + proportional_los_ = std::make_unique(params); - proportional_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load proportional_los parameters: ") + + e.what()); + } } void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; auto params = IntegralLosParams{}; - params.proportional_gain_h = - integral_los_config["proportional_gain_h"].as(); - params.proportional_gain_v = - integral_los_config["proportional_gain_v"].as(); - params.integral_gain_h = - integral_los_config["integral_gain_h"].as(); - params.integral_gain_v = - integral_los_config["integral_gain_v"].as(); - params.time_step = static_cast(time_step_.count()) / 1000.0; - - integral_los_ = std::make_unique(params); + try { + params.proportional_gain_h = + integral_los_config["proportional_gain_h"].as(); + params.proportional_gain_v = + integral_los_config["proportional_gain_v"].as(); + params.integral_gain_h = + integral_los_config["integral_gain_h"].as(); + params.integral_gain_v = + integral_los_config["integral_gain_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + integral_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load integral_los parameters: ") + e.what()); + } } void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { auto vector_field_config = config["vector_field_los"]; auto params = VectorFieldLosParams{}; - params.max_approach_angle_h = - vector_field_config["max_approach_angle_h"].as(); - params.max_approach_angle_v = - vector_field_config["max_approach_angle_v"].as(); - params.proportional_gain_h = - vector_field_config["proportional_gain_h"].as(); - params.proportional_gain_v = - vector_field_config["proportional_gain_v"].as(); - params.time_step = static_cast(time_step_.count()) / 1000.0; - - vector_field_los_ = std::make_unique(params); + try { + params.max_approach_angle_h = + vector_field_config["max_approach_angle_h"].as(); + params.max_approach_angle_v = + vector_field_config["max_approach_angle_v"].as(); + params.proportional_gain_h = + vector_field_config["proportional_gain_h"].as(); + params.proportional_gain_v = + vector_field_config["proportional_gain_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + vector_field_los_ = std::make_unique(params); + + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load vector_field_los parameters: ") + + e.what()); + } } void LosGuidanceNode::waypoint_callback( @@ -219,16 +245,22 @@ void LosGuidanceNode::odom_callback( rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { - (void)goal; - { std::unique_lock lock(mutex_); - if (goal_handle_) { - if (goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } + + if (!is_goal_feasible(path_inputs_, goal)) { + spdlog::info( + "Rejected goal request: waypoint is not reachable with current " + "pitch limit"); + lock.unlock(); + return rclcpp_action::GoalResponse::REJECT; + } + + if (goal_handle_ && goal_handle_->is_active()) { + spdlog::info("Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); } + lock.unlock(); } @@ -255,76 +287,96 @@ void LosGuidanceNode::handle_accepted( void LosGuidanceNode::set_los_mode( const std::shared_ptr request, std::shared_ptr response) { - method_ = static_cast(request->mode); - spdlog::info("LOS mode set to {}", static_cast(method_)); + { + std::unique_lock lock(mutex_); + method_ = static_cast(request->mode); + lock.unlock(); + } + + spdlog::info("LOS mode set to {}", static_cast(request->mode)); response->success = true; } vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( - types::Outputs outputs, - types::Inputs inputs) { + types::Outputs outputs) { vortex_msgs::msg::LOSGuidance reference_msg; const double clamped_pitch = std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; + reference_msg.surge = u_desired_; + return reference_msg; +} - if (slow_approach_) { - const double distance_to_goal = - (inputs.current_position - inputs.next_point).as_vector().norm(); +bool LosGuidanceNode::is_goal_feasible( + const types::Inputs& inputs, + std::shared_ptr goal) { + const auto& current_position = inputs.current_position; + const auto& goal_point = goal->goal.point; - double target_surge = u_desired_; + const double dx = goal_point.x - current_position.x; + const double dy = goal_point.y - current_position.y; + const double dz = goal_point.z - current_position.z; - if (distance_to_goal <= slow_down_distance_) { - const double alpha = - std::clamp(distance_to_goal / slow_down_distance_, 0.0, 1.0); - target_surge = u_slow_min_ + alpha * (u_desired_ - u_slow_min_); - } + const double horizontal_distance = std::sqrt(dx * dx + dy * dy); + const double required_pitch = std::atan2(-dz, horizontal_distance); - if (!surge_initialized_) { - commanded_surge_ = target_surge; - surge_initialized_ = true; - } else { - const double dt = static_cast(time_step_.count()) / 1000.0; - const double max_step = surge_rate_limit_ * dt; - const double delta = target_surge - commanded_surge_; - - if (delta > max_step) { - commanded_surge_ += max_step; - } else if (delta < -max_step) { - commanded_surge_ -= max_step; - } else { - commanded_surge_ = target_surge; - } - } + return std::abs(required_pitch) <= max_pitch_angle_; +} + +bool LosGuidanceNode::is_goal_missed(const types::Inputs& inputs) { + const double distance_to_goal = + (inputs.current_position - inputs.next_point).as_vector().norm(); + + const double dt = static_cast(time_step_.count()) / 1000.0; + + if (distance_to_goal < nearest_been_to_goal_) { + nearest_been_to_goal_ = distance_to_goal; + time_since_nearest_goal_ = 0.0; + return false; + } + + if (distance_to_goal > + nearest_been_to_goal_ + missed_goal_distance_margin_) { + time_since_nearest_goal_ += dt; } else { - commanded_surge_ = u_desired_; + time_since_nearest_goal_ = 0.0; } - reference_msg.surge = commanded_surge_; - return reference_msg; + return time_since_nearest_goal_ >= missed_goal_timeout_; } YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { - YAML::Node config = YAML::LoadFile(yaml_file_path); - return config; + try { + YAML::Node config = YAML::LoadFile(yaml_file_path); + return config; + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load LOS config file '") + yaml_file_path + + "': " + e.what()); + } } void LosGuidanceNode::parse_common_config(YAML::Node common_config) { - std::unique_lock lock(mutex_); - u_desired_ = common_config["u_desired"].as(); - max_pitch_angle_ = common_config["max_pitch_angle"].as(); - goal_reached_tol_ = common_config["goal_reached_tol"].as(); - slow_approach_ = common_config["slow_approach"].as(); - slow_down_distance_ = common_config["slow_down_distance"].as(); - u_slow_min_ = common_config["u_slow_min"].as(); - surge_rate_limit_ = common_config["surge_rate_limit"].as(); + try { + std::unique_lock lock(mutex_); + u_desired_ = common_config["u_desired"].as(); + max_pitch_angle_ = common_config["max_pitch_angle"].as(); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); + missed_goal_timeout_ = + common_config["missed_goal_timeout"].as(); + missed_goal_distance_margin_ = + common_config["missed_goal_distance_margin"].as(); - method_ = static_cast( - common_config["active_los_method"].as()); + method_ = static_cast( + common_config["active_los_method"].as()); - lock.unlock(); + lock.unlock(); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load common parameters: ") + e.what()); + } } void LosGuidanceNode::execute( @@ -344,16 +396,22 @@ void LosGuidanceNode::execute( const auto new_wp = types::Point::point_from_ros(los_waypoint.point); - if (!has_active_segment_) { - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = new_wp; - has_active_segment_ = true; - } else { - path_inputs_.prev_point = path_inputs_.next_point; - path_inputs_.next_point = new_wp; + { + std::unique_lock lock(mutex_); + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } + lock.unlock(); } auto result = std::make_shared(); + nearest_been_to_goal_ = std::numeric_limits::infinity(); + time_since_nearest_goal_ = 0.0; rclcpp::Rate loop_rate(1000.0 / time_step_.count()); @@ -376,15 +434,29 @@ void LosGuidanceNode::execute( } types::Inputs inputs_copy; + types::ActiveLosMethod method_copy; + nav_msgs::msg::Odometry::SharedPtr odom_copy; + double goal_reached_tol_copy; + { std::unique_lock lock(mutex_); inputs_copy = path_inputs_; + method_copy = method_; + odom_copy = debug_current_odom_; + goal_reached_tol_copy = goal_reached_tol_; lock.unlock(); } + if (is_goal_missed(inputs_copy)) { + result->success = false; + goal_handle->abort(result); + spdlog::info("Aborting goal: waypoint missed"); + return; + } + types::Outputs outputs; - switch (method_) { + switch (method_copy) { case types::ActiveLosMethod::ADAPTIVE: outputs = adaptive_los_->calculate_outputs(inputs_copy); break; @@ -403,12 +475,13 @@ void LosGuidanceNode::execute( goal_handle->abort(result); return; } + auto reference_msg = std::make_unique( - fill_los_reference(outputs, inputs_copy)); + fill_los_reference(outputs)); if ((inputs_copy.current_position - inputs_copy.next_point) .as_vector() - .norm() < goal_reached_tol_) { + .norm() < goal_reached_tol_copy) { reference_msg->pitch = 0.0; reference_msg->surge = 0.0; @@ -420,17 +493,16 @@ void LosGuidanceNode::execute( reference_pub_->publish(std::move(reference_msg)); - if (debug_current_odom_ && debug) { - const auto& v = debug_current_odom_->twist.twist.linear; + if (debug && odom_copy) { + const auto& v = odom_copy->twist.twist.linear; double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); vortex_msgs::msg::LOSGuidance state_debug_msg; - Eigen::Vector3d euler = - vortex::utils::math::quat_to_euler(Eigen::Quaterniond( - debug_current_odom_->pose.pose.orientation.w, - debug_current_odom_->pose.pose.orientation.x, - debug_current_odom_->pose.pose.orientation.y, - debug_current_odom_->pose.pose.orientation.z)); + Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( + Eigen::Quaterniond(odom_copy->pose.pose.orientation.w, + odom_copy->pose.pose.orientation.x, + odom_copy->pose.pose.orientation.y, + odom_copy->pose.pose.orientation.z)); state_debug_msg.pitch = euler.y(); state_debug_msg.yaw = euler.z(); diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index c0cdc1ae7..c06410f19 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -11,6 +11,7 @@ add_executable( proportional_los_test.cpp integral_los_test.cpp vector_field_los_test.cpp + los_invalid_params_test.cpp ) diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp index 8c9b8e598..afd707604 100644 --- a/guidance/los_guidance/test/adaptive_los_test.cpp +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -22,7 +22,7 @@ class AdaptiveLosTest : public ::testing::Test { }; // Test commanded angles when drone is to the right of the track -TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { +TEST_F(AdaptiveLosTest, T01_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -39,7 +39,7 @@ TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { } // Test commanded angles when drone is to the left of the track -TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { +TEST_F(AdaptiveLosTest, T02_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -55,7 +55,7 @@ TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { } // Test commanded angles when drone is under the track -TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { +TEST_F(AdaptiveLosTest, T03_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -71,7 +71,7 @@ TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { } // Test commanded angles when drone is above the track -TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { +TEST_F(AdaptiveLosTest, T04_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -88,7 +88,7 @@ TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { // Test commanded angles when drone is above and to the right of the track -TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { +TEST_F(AdaptiveLosTest, T05_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp index 851c10b84..7e06559f4 100644 --- a/guidance/los_guidance/test/integral_los_test.cpp +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -23,7 +23,7 @@ class IntegralLosTest : public ::testing::Test { }; // Test commanded angles when drone is to the right of the track -TEST_F(IntegralLosTest, T06_test_commanded_angles) { +TEST_F(IntegralLosTest, T01_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -40,7 +40,7 @@ TEST_F(IntegralLosTest, T06_test_commanded_angles) { } // Test commanded angles when drone is to the left of the track -TEST_F(IntegralLosTest, T07_test_commanded_angles) { +TEST_F(IntegralLosTest, T02_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -56,7 +56,7 @@ TEST_F(IntegralLosTest, T07_test_commanded_angles) { } // Test commanded angles when drone is under the track -TEST_F(IntegralLosTest, T08_test_commanded_angles) { +TEST_F(IntegralLosTest, T03_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -72,7 +72,7 @@ TEST_F(IntegralLosTest, T08_test_commanded_angles) { } // Test commanded angles when drone is above the track -TEST_F(IntegralLosTest, T09_test_commanded_angles) { +TEST_F(IntegralLosTest, T04_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -89,7 +89,7 @@ TEST_F(IntegralLosTest, T09_test_commanded_angles) { // Test commanded angles when drone is above and to the right of the track -TEST_F(IntegralLosTest, T10_test_commanded_angles) { +TEST_F(IntegralLosTest, T05_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; diff --git a/guidance/los_guidance/test/los_invalid_params_test.cpp b/guidance/los_guidance/test/los_invalid_params_test.cpp new file mode 100644 index 000000000..4473c1c66 --- /dev/null +++ b/guidance/los_guidance/test/los_invalid_params_test.cpp @@ -0,0 +1,56 @@ +#include +#include + +#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/vector_field_los.hpp" + +namespace vortex::guidance::los { + +TEST(LosInvalidParamsTest, AdaptiveLosRejectsNegativeLookaheadDistance) { + AdaptiveLosParams params; + params.lookahead_distance_h = -0.9; + params.lookahead_distance_v = 1.4; + params.adaptation_gain_h = 0.03; + params.adaptation_gain_v = 0.02; + params.time_step = 0.01; + + EXPECT_THROW( + { AdaptiveLOSGuidance guidance(params); }, std::invalid_argument); +} + +TEST(LosInvalidParamsTest, ProportionalLosRejectsZeroLookaheadDistance) { + ProportionalLosParams params; + params.lookahead_distance_h = 0.0; + params.lookahead_distance_v = 0.8; + + EXPECT_THROW( + { ProportionalLOSGuidance guidance(params); }, std::invalid_argument); +} + +TEST(LosInvalidParamsTest, IntegralLosRejectsZeroTimeStep) { + IntegralLosParams params; + params.proportional_gain_h = 0.5; + params.proportional_gain_v = 0.5; + params.integral_gain_h = 0.1; + params.integral_gain_v = 0.1; + params.time_step = 0.0; + + EXPECT_THROW( + { IntegralLOSGuidance guidance(params); }, std::invalid_argument); +} + +TEST(LosInvalidParamsTest, VectorFieldLosRejectsNegativeApproachAngle) { + VectorFieldLosParams params; + params.max_approach_angle_h = -1.0; + params.max_approach_angle_v = 1.0; + params.proportional_gain_h = 1.5; + params.proportional_gain_v = 0.9; + params.time_step = 0.01; + + EXPECT_THROW( + { VectorFieldLOSGuidance guidance(params); }, std::invalid_argument); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp index e26695d32..311d7d23a 100644 --- a/guidance/los_guidance/test/proportional_los_test.cpp +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -19,7 +19,7 @@ class ProportionalLosTest : public ::testing::Test { }; // Test commanded angles when drone is to the right of the track -TEST_F(ProportionalLosTest, T06_test_commanded_angles) { +TEST_F(ProportionalLosTest, T01_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -36,7 +36,7 @@ TEST_F(ProportionalLosTest, T06_test_commanded_angles) { } // Test commanded angles when drone is to the left of the track -TEST_F(ProportionalLosTest, T07_test_commanded_angles) { +TEST_F(ProportionalLosTest, T02_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -52,7 +52,7 @@ TEST_F(ProportionalLosTest, T07_test_commanded_angles) { } // Test commanded angles when drone is under the track -TEST_F(ProportionalLosTest, T08_test_commanded_angles) { +TEST_F(ProportionalLosTest, T03_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -68,7 +68,7 @@ TEST_F(ProportionalLosTest, T08_test_commanded_angles) { } // Test commanded angles when drone is above the track -TEST_F(ProportionalLosTest, T09_test_commanded_angles) { +TEST_F(ProportionalLosTest, T04_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -85,7 +85,7 @@ TEST_F(ProportionalLosTest, T09_test_commanded_angles) { // Test commanded angles when drone is above and to the right of the track -TEST_F(ProportionalLosTest, T10_test_commanded_angles) { +TEST_F(ProportionalLosTest, T05_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp index 10c316352..f5f121920 100644 --- a/guidance/los_guidance/test/vector_field_los_test.cpp +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -23,7 +23,7 @@ class VectorFieldLosTest : public ::testing::Test { }; // Test commanded angles when drone is to the right of the track -TEST_F(VectorFieldLosTest, T06_test_commanded_angles) { +TEST_F(VectorFieldLosTest, T01_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -40,7 +40,7 @@ TEST_F(VectorFieldLosTest, T06_test_commanded_angles) { } // Test commanded angles when drone is to the left of the track -TEST_F(VectorFieldLosTest, T07_test_commanded_angles) { +TEST_F(VectorFieldLosTest, T02_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -57,7 +57,7 @@ TEST_F(VectorFieldLosTest, T07_test_commanded_angles) { } // Test commanded angles when drone is under the track -TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { +TEST_F(VectorFieldLosTest, T03_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -74,7 +74,7 @@ TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { } // Test commanded angles when drone is above the track -TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { +TEST_F(VectorFieldLosTest, T04_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; @@ -91,7 +91,7 @@ TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { } // Test commanded angles when drone is above and to the right of the track -TEST_F(VectorFieldLosTest, T10_test_commanded_angles) { +TEST_F(VectorFieldLosTest, T05_test_commanded_angles) { types::Inputs inputs; inputs.prev_point = types::Point{0.0, 0.0, 0.0}; inputs.next_point = types::Point{1.0, 0.0, 0.0}; From a4f6506cf875ea55597b312212243bd10d8436a0 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 29 Mar 2026 23:46:16 +0200 Subject: [PATCH 69/87] Fix: pre commit issues --- .../los_guidance/include/los_guidance/los_guidance_ros.hpp | 1 + guidance/los_guidance/src/los_guidance_ros.cpp | 4 ---- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index d7409f72f..698507d3e 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 2d8a31def..4fb7f591a 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -2,7 +2,6 @@ #include #include #include -#include #include #include @@ -133,7 +132,6 @@ void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { params.time_step = static_cast(time_step_.count()) / 1000.0; adaptive_los_ = std::make_unique(params); - } catch (const YAML::Exception& e) { throw std::runtime_error( std::string("Failed to load adaptive_los parameters: ") + e.what()); @@ -151,7 +149,6 @@ void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { proportional_los_config["lookahead_distance_v"].as(); proportional_los_ = std::make_unique(params); - } catch (const YAML::Exception& e) { throw std::runtime_error( std::string("Failed to load proportional_los parameters: ") + @@ -197,7 +194,6 @@ void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { params.time_step = static_cast(time_step_.count()) / 1000.0; vector_field_los_ = std::make_unique(params); - } catch (const YAML::Exception& e) { throw std::runtime_error( std::string("Failed to load vector_field_los parameters: ") + From 8168902bb24b3e5e06cd65525807c86c633a801c Mon Sep 17 00:00:00 2001 From: Anbit Date: Mon, 30 Mar 2026 18:20:38 +0200 Subject: [PATCH 70/87] Fix: feasible goal fix and new logo --- .../los_guidance/src/los_guidance_ros.cpp | 38 +++++++++++-------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 4fb7f591a..4c11dfceb 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -14,12 +14,12 @@ constexpr bool debug = true; #endif const auto start_message = R"( - _ ___ ____ ____ _ _ - | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ - | | | | | \___ \ | | _| | | | |/ _` |/ _` | '_ \ / __/ _ \ - | |__| |_| |___) | | |_| | |_| | | (_| | (_| | | | | (_| __/ - |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| - +██╗ ██████╗ ███████╗ ██████╗ ██╗ ██╗██╗██████╗ █████╗ ███╗ ██╗ ██████╗███████╗ +██║ ██╔═══██╗██╔════╝ ██╔════╝ ██║ ██║██║██╔══██╗██╔══██╗████╗ ██║██╔════╝██╔════╝ +██║ ██║ ██║███████╗ ██║ ███╗██║ ██║██║██║ ██║███████║██╔██╗ ██║██║ █████╗ +██║ ██║ ██║╚════██║ ██║ ██║██║ ██║██║██║ ██║██╔══██║██║╚██╗██║██║ ██╔══╝ +███████╗╚██████╔╝███████║ ╚██████╔╝╚██████╔╝██║██████╔╝██║ ██║██║ ╚████║╚██████╗███████╗ +╚══════╝ ╚═════╝ ╚══════╝ ╚═════╝ ╚═════╝ ╚═╝╚═════╝ ╚═╝ ╚═╝╚═╝ ╚═══╝ ╚═════╝╚══════╝ )"; namespace vortex::guidance::los { @@ -241,26 +241,32 @@ void LosGuidanceNode::odom_callback( rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { + types::Inputs inputs_copy; + { std::unique_lock lock(mutex_); + inputs_copy = path_inputs_; + lock.unlock(); + } - if (!is_goal_feasible(path_inputs_, goal)) { - spdlog::info( - "Rejected goal request: waypoint is not reachable with current " - "pitch limit"); - lock.unlock(); - return rclcpp_action::GoalResponse::REJECT; - } + if (!is_goal_feasible(inputs_copy, goal)) { + RCLCPP_WARN(this->get_logger(), + "Rejected goal request: waypoint is not reachable with " + "current pitch limit"); + return rclcpp_action::GoalResponse::REJECT; + } + { + std::unique_lock lock(mutex_); if (goal_handle_ && goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); + RCLCPP_INFO(this->get_logger(), + "Aborting current goal and accepting new goal"); preempted_goal_id_ = goal_handle_->get_goal_id(); } - lock.unlock(); } - spdlog::info("Accepted goal request"); + RCLCPP_INFO(this->get_logger(), "Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } From 9c0b6c359fc6e24664ab7a41466d869127fcabe2 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 31 Mar 2026 12:12:27 +0200 Subject: [PATCH 71/87] Fix: Guidance test file not working --- .../launch/guidance_test.launch.py | 81 +++++++------------ 1 file changed, 30 insertions(+), 51 deletions(-) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 18b18a2ec..6ba9dc7f4 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -9,6 +9,7 @@ OpaqueFunction, TimerAction, ) +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration @@ -21,24 +22,33 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) test_scenario = LaunchConfiguration("test_scenario").perform(context) + use_keyboard_joy = LaunchConfiguration("use_keyboard_joy") stonefish_dir = get_package_share_directory("stonefish_sim") + auv_setup_dir = get_package_share_directory("auv_setup") los_guidance_dir = get_package_share_directory("los_guidance") - velocity_controller_dir = get_package_share_directory('velocity_controller') + keyboard_joy_dir = get_package_share_directory("keyboard_joy") stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(stonefish_dir, "launch", "simulation.launch.py") ), launch_arguments={ - "scenario": "default", + "drone": drone, "rendering": "true", }.items(), ) - los_guidance_launch = IncludeLaunchDescription( + keyboard_joy = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(keyboard_joy_dir, "launch", "keyboard_joy_node.launch.py") + ), + condition=IfCondition(use_keyboard_joy), + ) + + dp_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(los_guidance_dir, "launch", "los_guidance.launch.py") + os.path.join(auv_setup_dir, "launch", "dp.launch.py") ), launch_arguments={ "drone": drone, @@ -46,62 +56,27 @@ def launch_setup(context, *args, **kwargs): }.items(), ) - velocity_controller_launch = IncludeLaunchDescription( + drone_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join( - velocity_controller_dir, 'launch', 'velocity_controller.launch.py' - ) + os.path.join(stonefish_dir, "launch", "drone_sim.launch.py") ), launch_arguments={ "drone": drone, - "namespace": namespace, }.items(), ) - drone_sim = TimerAction( - period=12.0, - actions=[ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, "launch", "drone_sim.launch.py") - ) - ) - ], - ) - - set_autonomy = TimerAction( - period=12.0, - actions=[ - ExecuteProcess( - cmd=[ - "bash", - "-lc", - ( - f"ros2 service call /{namespace}/set_killswitch " - "vortex_msgs/srv/SetKillswitch " - "\"{killswitch_on: false}\" " - "&& " - f"ros2 service call /{namespace}/set_operation_mode " - "vortex_msgs/srv/SetOperationMode " - "\"{requested_operation_mode: {operation_mode: 1}}\"" - ), - ], - output="screen", - ), - ], - ) - run_test_scenario = TimerAction( period=20.0, actions=[ ExecuteProcess( cmd=[ - "bash", - "-lc", - ( - f"python3 {os.path.join(los_guidance_dir, 'scripts', 'test_scenarios.py')} " - f"--ros-args -p drone:={drone} -p test_scenario:={test_scenario}" - ), + "python3", + os.path.join(los_guidance_dir, "scripts", "test_scenarios.py"), + "--ros-args", + "-p", + f"drone:={drone}", + "-p", + f"test_scenario:={test_scenario}", ], output="screen", ) @@ -110,10 +85,9 @@ def launch_setup(context, *args, **kwargs): return [ stonefish_sim, - los_guidance_launch, - velocity_controller_launch, + keyboard_joy, + dp_launch, drone_sim, - set_autonomy, run_test_scenario, ] @@ -127,6 +101,11 @@ def generate_launch_description(): default_value="4_corner", description="Scenario to run: 4_corner, circle, test_pitch, opposite_point", ), + DeclareLaunchArgument( + "use_keyboard_joy", + default_value="true", + description="Launch keyboard joy node", + ), OpaqueFunction(function=launch_setup), ] ) From 4c09390805fbe4e3ce8e3f71e7e1653c2bb9132d Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 31 Mar 2026 12:29:52 +0200 Subject: [PATCH 72/87] Add: Added velocity dir to test launch file --- guidance/los_guidance/launch/guidance_test.launch.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 6ba9dc7f4..84a045796 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -25,9 +25,9 @@ def launch_setup(context, *args, **kwargs): use_keyboard_joy = LaunchConfiguration("use_keyboard_joy") stonefish_dir = get_package_share_directory("stonefish_sim") - auv_setup_dir = get_package_share_directory("auv_setup") los_guidance_dir = get_package_share_directory("los_guidance") keyboard_joy_dir = get_package_share_directory("keyboard_joy") + velocity_controller_dir = get_package_share_directory("velocity_controller") stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -46,9 +46,11 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(use_keyboard_joy), ) - dp_launch = IncludeLaunchDescription( + velocity_controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(auv_setup_dir, "launch", "dp.launch.py") + os.path.join( + velocity_controller_dir, "launch", "velocity_controller.launch.py" + ) ), launch_arguments={ "drone": drone, @@ -86,7 +88,7 @@ def launch_setup(context, *args, **kwargs): return [ stonefish_sim, keyboard_joy, - dp_launch, + velocity_controller_launch, drone_sim, run_test_scenario, ] @@ -94,7 +96,7 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( - declare_drone_and_namespace_args(default_drone="orca") + declare_drone_and_namespace_args(default_drone="nautilus") + [ DeclareLaunchArgument( "test_scenario", From 9e5792e3df4b44e0cad74cf1e34681ec4e399766 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 31 Mar 2026 12:35:49 +0200 Subject: [PATCH 73/87] Add: los guidance launch path in test file --- .../los_guidance/launch/guidance_test.launch.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 84a045796..f854df563 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -39,6 +39,19 @@ def launch_setup(context, *args, **kwargs): }.items(), ) + + los_guidance_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + los_guidance_dir, "launch", "los_guidance.launch.py" + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + ) + keyboard_joy = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(keyboard_joy_dir, "launch", "keyboard_joy_node.launch.py") @@ -88,6 +101,7 @@ def launch_setup(context, *args, **kwargs): return [ stonefish_sim, keyboard_joy, + los_guidance_launch, velocity_controller_launch, drone_sim, run_test_scenario, From 557e15fd79f097cc5f03581c46c1c5c0d29a2171 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 31 Mar 2026 12:44:27 +0200 Subject: [PATCH 74/87] Add: scenario with no floor --- guidance/los_guidance/launch/guidance_test.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index f854df563..c6b29d661 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -35,6 +35,7 @@ def launch_setup(context, *args, **kwargs): ), launch_arguments={ "drone": drone, + "scenario": "nautilus_no_gpu", "rendering": "true", }.items(), ) From 7616fd003aa1d703cd4be91ca7290c7bdcab4802 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 31 Mar 2026 12:44:51 +0200 Subject: [PATCH 75/87] add --- guidance/los_guidance/launch/guidance_test.launch.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index c6b29d661..594c0ec66 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -40,12 +40,9 @@ def launch_setup(context, *args, **kwargs): }.items(), ) - los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join( - los_guidance_dir, "launch", "los_guidance.launch.py" - ) + os.path.join(los_guidance_dir, "launch", "los_guidance.launch.py") ), launch_arguments={ "drone": drone, From 8debeccc2ede885104f0ef0a4d0b6eba671ff212 Mon Sep 17 00:00:00 2001 From: Anbit Date: Fri, 3 Apr 2026 16:33:20 +0200 Subject: [PATCH 76/87] Add: new params for ALOS and reset for adaptive parasm --- guidance/los_guidance/config/guidance_params.yaml | 6 +++--- .../include/los_guidance/lib/adaptive_los.hpp | 5 +++++ guidance/los_guidance/launch/guidance_test.launch.py | 12 ++++++++++-- guidance/los_guidance/src/lib/adaptive_los.cpp | 5 +++++ guidance/los_guidance/src/los_guidance_ros.cpp | 2 ++ 5 files changed, 25 insertions(+), 5 deletions(-) diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index b27d8e5b2..16c66b470 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -24,10 +24,10 @@ # * - # */ adaptive_los: - lookahead_distance_h: 0.9 + lookahead_distance_h: 2.0 lookahead_distance_v: 1.4 - adaptation_gain_h: 0.03 - adaptation_gain_v: 0.02 + adaptation_gain_h: 0.005 + adaptation_gain_v: 0.005 prop_los: lookahead_distance_h: 0.74 diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 3f7b7f39b..68cffec71 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -55,6 +55,11 @@ class AdaptiveLOSGuidance { */ types::Outputs calculate_outputs(const types::Inputs& inputs); + /** + * @brief resets the adaptive values when new-segment starts + */ + void reset(); + private: /** * @brief Updates the path heading and path pitch angles from the active diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 594c0ec66..299e48f95 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -27,7 +27,8 @@ def launch_setup(context, *args, **kwargs): stonefish_dir = get_package_share_directory("stonefish_sim") los_guidance_dir = get_package_share_directory("los_guidance") keyboard_joy_dir = get_package_share_directory("keyboard_joy") - velocity_controller_dir = get_package_share_directory("velocity_controller") + velocity_controller_dir = get_package_share_directory("velocity_controller_lqr") + utility_dir = get_package_share_directory("vortex_utility_nodes") stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -60,7 +61,7 @@ def launch_setup(context, *args, **kwargs): velocity_controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( - velocity_controller_dir, "launch", "velocity_controller.launch.py" + velocity_controller_dir, "launch", "velocity_controller_lqr.launch.py" ) ), launch_arguments={ @@ -78,6 +79,12 @@ def launch_setup(context, *args, **kwargs): }.items(), ) + utility_node = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(utility_dir, "launch", "message_publisher.launch.py") + ) + ) + run_test_scenario = TimerAction( period=20.0, actions=[ @@ -102,6 +109,7 @@ def launch_setup(context, *args, **kwargs): los_guidance_launch, velocity_controller_launch, drone_sim, + utility_node, run_test_scenario, ] diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp index 633c966e3..c3ad64fc5 100644 --- a/guidance/los_guidance/src/lib/adaptive_los.cpp +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -14,6 +14,11 @@ AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) } } +void AdaptiveLOSGuidance::reset() { + beta_c_hat_ = 0.0; + alpha_c_hat_ = 0.0; +} + void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { const double dx = inputs.next_point.x - inputs.prev_point.x; const double dy = inputs.next_point.y - inputs.prev_point.y; diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 4c11dfceb..9b7007a66 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -411,6 +411,8 @@ void LosGuidanceNode::execute( lock.unlock(); } + adaptive_los_->reset(); + auto result = std::make_shared(); nearest_been_to_goal_ = std::numeric_limits::infinity(); time_since_nearest_goal_ = 0.0; From 9c0a3ed4f5c5161c597d2e1b6925d017eaf0488e Mon Sep 17 00:00:00 2001 From: Anbit Date: Fri, 10 Apr 2026 14:56:36 +0200 Subject: [PATCH 77/87] Add: Slowig down method while hedding error is too big --- .../config/param_velocity_controller_lqr.yaml | 24 +++++----- .../los_guidance/config/guidance_params.yaml | 12 ++--- .../include/los_guidance/los_guidance_ros.hpp | 12 +++++ .../los_guidance/src/los_guidance_ros.cpp | 45 +++++++++++++++++-- 4 files changed, 72 insertions(+), 21 deletions(-) diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml index 727c1c71e..e5224f4bd 100644 --- a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -1,23 +1,23 @@ /**: ros__parameters: - dt: 0.1 + dt: 0.05 LQR_params: - q_surge: 75 - q_pitch: 175 - q_yaw: 175 + q_surge: 40 + q_pitch: 80 + q_yaw: 80 - r_surge: 0.3 - r_pitch: 0.4 - r_yaw: 0.4 + r_surge: 0.4 + r_pitch: 0.6 + r_yaw: 0.6 - i_surge: 0.3 - i_pitch: 0.4 - i_yaw: 0.3 + i_surge: 0.2 + i_pitch: 0.2 + i_yaw: 0.2 i_weight: 0.5 - inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + inertia_matrix: [53.7, 0.0, 0.0, 0.0, 23.1128, 0.1025, 0.0, 0.1025, 26.23998] #Clamp parameter - max_force: 99.5 + max_force: 80.0 diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 16c66b470..698a5562b 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -24,14 +24,14 @@ # * - # */ adaptive_los: - lookahead_distance_h: 2.0 - lookahead_distance_v: 1.4 - adaptation_gain_h: 0.005 + lookahead_distance_h: 3.5 + lookahead_distance_v: 3.5 + adaptation_gain_h: 0.002 adaptation_gain_v: 0.005 prop_los: - lookahead_distance_h: 0.74 - lookahead_distance_v: 0.8 + lookahead_distance_h: 4.5 + lookahead_distance_v: 1.8 integer_los: proportional_gain_h: 0.5 @@ -47,7 +47,7 @@ vector_field_los: common: active_los_method: 2 - u_desired: 0.3 + u_desired: 0.2 goal_reached_tol: 0.20 max_pitch_angle: 0.96 missed_goal_distance_margin: 1.0 diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 698507d3e..9709becbd 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -121,6 +122,14 @@ class LosGuidanceNode : public rclcpp::Node { */ void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + /** + * @brief Callback for receiving odometry updates from + * utils/message_publisher. + * @param msg Received odometry message. + */ + void odom_msg_callback( + const vortex_msgs::msg::PoseEulerStamped::SharedPtr msg); + /** * @brief Handles an incoming LOS guidance action goal request. * @param uuid Unique identifier for the received goal. @@ -217,6 +226,8 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr + message_pub_sub_; rclcpp::TimerBase::SharedPtr reference_pub_timer_; rclcpp::CallbackGroup::SharedPtr cb_group_; @@ -232,6 +243,7 @@ class LosGuidanceNode : public rclcpp::Node { double u_desired_{}; double goal_reached_tol_{}; double max_pitch_angle_{}; + double current_yaw_{}; types::ActiveLosMethod method_{}; double nearest_been_to_goal_{std::numeric_limits::max()}; diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 9b7007a66..f17249579 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include "los_guidance/lib/types.hpp" @@ -53,6 +54,8 @@ void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.waypoint"); this->declare_parameter("topics.odom"); + this->declare_parameter( + "topics.odom_tf_rpy", "/utils/message_publisher/odom_tf_rpy"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string guidance_topic = @@ -60,6 +63,8 @@ void LosGuidanceNode::set_subscribers_and_publisher() { std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); std::string odom_topic = this->get_parameter("topics.odom").as_string(); + std::string odom_tf_rpy_topic = + this->get_parameter("topics.odom_tf_rpy").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); @@ -84,6 +89,12 @@ void LosGuidanceNode::set_subscribers_and_publisher() { odom_topic, qos_sensor_data, std::bind(&LosGuidanceNode::odom_callback, this, std::placeholders::_1)); + + message_pub_sub_ = + this->create_subscription( + odom_tf_rpy_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::odom_msg_callback, this, + std::placeholders::_1)); } void LosGuidanceNode::set_action_server() { @@ -238,6 +249,13 @@ void LosGuidanceNode::odom_callback( lock.unlock(); } +void LosGuidanceNode::odom_msg_callback( + const vortex_msgs::msg::PoseEulerStamped::SharedPtr msg) { + std::unique_lock lock(mutex_); + current_yaw_ = msg->yaw; + lock.unlock(); +} + rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { @@ -303,11 +321,32 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs) { vortex_msgs::msg::LOSGuidance reference_msg; - const double clamped_pitch = - std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); + double max_pitch_angle_copy; + double current_yaw_copy; + double u_desired_copy; + + { + std::unique_lock lock(mutex_); + max_pitch_angle_copy = max_pitch_angle_; + current_yaw_copy = current_yaw_; + u_desired_copy = u_desired_; + } + + const double clamped_pitch = std::clamp( + outputs.theta_d, -max_pitch_angle_copy, max_pitch_angle_copy); + reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; - reference_msg.surge = u_desired_; + + double yaw_error = + vortex::utils::math::ssa(outputs.psi_d - current_yaw_copy); + double abs_err = std::abs(yaw_error); + + double u_cmd = u_desired_copy / (1.0 + 0.5 * abs_err); + u_cmd = std::clamp(u_cmd, 0.12, u_desired_copy); + + reference_msg.surge = u_cmd; + return reference_msg; } From 3206eb3de6031cb445223b02486ce9e82a08e9c7 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 14 Apr 2026 17:25:27 +0200 Subject: [PATCH 78/87] Fix: Comment issues --- .../los_guidance/include/los_guidance/lib/adaptive_los.hpp | 2 +- .../los_guidance/include/los_guidance/lib/integral_los.hpp | 2 +- .../include/los_guidance/lib/vector_field_los.hpp | 2 +- guidance/los_guidance/src/los_guidance_ros.cpp | 4 ++-- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp index 68cffec71..aee31d9c6 100644 --- a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -26,7 +26,7 @@ struct AdaptiveLosParams { double lookahead_distance_v{}; double adaptation_gain_h{}; double adaptation_gain_v{}; - double time_step{}; // in milliseconds + double time_step{}; }; /** diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp index 2bf8d951c..c33289fd3 100644 --- a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -22,7 +22,7 @@ struct IntegralLosParams { double proportional_gain_v{}; double integral_gain_h{}; double integral_gain_v{}; - double time_step{}; // in milliseconds + double time_step{}; }; /** diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp index 6f4ec133e..802ac15c6 100644 --- a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -22,7 +22,7 @@ struct VectorFieldLosParams { double max_approach_angle_v{}; double proportional_gain_h{}; double proportional_gain_v{}; - double time_step{}; // in milliseconds + double time_step{}; }; /** diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index f17249579..09a5c81e8 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -29,8 +29,8 @@ namespace vortex::guidance::los { LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) : Node("los_guidance_node", options) { double time_step_s = this->declare_parameter("time_step"); - time_step_ = std::chrono::milliseconds(static_cast( - time_step_s * 1000)); // Convert seconds to milliseconds + time_step_ = + std::chrono::milliseconds(static_cast(time_step_s * 1000)); const std::string yaml_path = this->declare_parameter("los_config_file"); From 55131a9ba2cd609782dc43532c10ee14ecc91914 Mon Sep 17 00:00:00 2001 From: Anbit Date: Tue, 14 Apr 2026 18:43:30 +0200 Subject: [PATCH 79/87] Refactor: To use the new vortex msg type for action server --- .../include/los_guidance/los_guidance_ros.hpp | 16 ++++++------- .../los_guidance/src/los_guidance_ros.cpp | 23 ++++++++++--------- 2 files changed, 20 insertions(+), 19 deletions(-) diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 9709becbd..4b775b24a 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include #include #include @@ -139,7 +139,7 @@ class LosGuidanceNode : public rclcpp::Node { */ rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal); + std::shared_ptr goal); /** * @brief Handles cancellation of an active LOS guidance goal. @@ -149,7 +149,7 @@ class LosGuidanceNode : public rclcpp::Node { */ rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr< - rclcpp_action::ServerGoalHandle> + rclcpp_action::ServerGoalHandle> goal_handle); /** @@ -157,14 +157,14 @@ class LosGuidanceNode : public rclcpp::Node { * @param goal_handle Handle to the accepted goal. */ void handle_accepted(const std::shared_ptr> goal_handle); + vortex_msgs::action::SetWaypoint>> goal_handle); /** * @brief Executes the LOS guidance action. * @param goal_handle Handle to the active LOS guidance goal. */ void execute(const std::shared_ptr> goal_handle); + vortex_msgs::action::SetWaypoint>> goal_handle); /** * @brief Service callback for changing the active LOS guidance method. @@ -202,7 +202,7 @@ class LosGuidanceNode : public rclcpp::Node { */ bool is_goal_feasible( const types::Inputs& inputs, - std::shared_ptr goal); + std::shared_ptr goal); /** * @brief Checks if the LOS guidance goal has been missed based on the @@ -214,7 +214,7 @@ class LosGuidanceNode : public rclcpp::Node { bool has_active_segment_{false}; - rclcpp_action::Server::SharedPtr + rclcpp_action::Server::SharedPtr action_server_; rclcpp::Service::SharedPtr los_mode_service_; rclcpp::Publisher::SharedPtr reference_pub_; @@ -236,7 +236,7 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp_action::GoalUUID preempted_goal_id_; std::shared_ptr< - rclcpp_action::ServerGoalHandle> + rclcpp_action::ServerGoalHandle> goal_handle_; types::Inputs path_inputs_{}; diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 09a5c81e8..53c97f53f 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -106,7 +107,7 @@ void LosGuidanceNode::set_action_server() { this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); action_server_ = - rclcpp_action::create_server( + rclcpp_action::create_server( this, action_server_name, std::bind(&LosGuidanceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), @@ -258,7 +259,7 @@ void LosGuidanceNode::odom_msg_callback( rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, - std::shared_ptr goal) { + std::shared_ptr goal) { types::Inputs inputs_copy; { @@ -290,7 +291,7 @@ rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( const std::shared_ptr< - rclcpp_action::ServerGoalHandle> + rclcpp_action::ServerGoalHandle> goal_handle) { spdlog::info("Received request to cancel goal"); (void)goal_handle; @@ -299,7 +300,7 @@ rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( void LosGuidanceNode::handle_accepted( const std::shared_ptr< - rclcpp_action::ServerGoalHandle> + rclcpp_action::ServerGoalHandle> goal_handle) { std::thread{[this, goal_handle]() { execute(goal_handle); }}.detach(); } @@ -352,9 +353,9 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( bool LosGuidanceNode::is_goal_feasible( const types::Inputs& inputs, - std::shared_ptr goal) { + std::shared_ptr goal) { const auto& current_position = inputs.current_position; - const auto& goal_point = goal->goal.point; + const auto& goal_point = goal->waypoint.pose.position; const double dx = goal_point.x - current_position.x; const double dy = goal_point.y - current_position.y; @@ -422,7 +423,7 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { void LosGuidanceNode::execute( const std::shared_ptr< - rclcpp_action::ServerGoalHandle> + rclcpp_action::ServerGoalHandle> goal_handle) { { std::unique_lock lock(mutex_); @@ -432,10 +433,10 @@ void LosGuidanceNode::execute( spdlog::info("Executing goal"); - const geometry_msgs::msg::PointStamped los_waypoint = - goal_handle->get_goal()->goal; + const geometry_msgs::msg::Point los_waypoint = + goal_handle->get_goal()->waypoint.pose.position; - const auto new_wp = types::Point::point_from_ros(los_waypoint.point); + const auto new_wp = types::Point::point_from_ros(los_waypoint); { std::unique_lock lock(mutex_); @@ -452,7 +453,7 @@ void LosGuidanceNode::execute( adaptive_los_->reset(); - auto result = std::make_shared(); + auto result = std::make_shared(); nearest_been_to_goal_ = std::numeric_limits::infinity(); time_since_nearest_goal_ = 0.0; From f9532c8b50a76c2b43ab191789bb63bc42974cff Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 16 Apr 2026 13:45:23 +0200 Subject: [PATCH 80/87] Fix: LOS build fail --- .../include/los_guidance/los_guidance_ros.hpp | 25 ++++++++++--------- .../los_guidance/src/los_guidance_ros.cpp | 18 +++++++------ 2 files changed, 23 insertions(+), 20 deletions(-) diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 4b775b24a..62b76c92a 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -14,8 +14,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -139,7 +138,8 @@ class LosGuidanceNode : public rclcpp::Node { */ rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, - std::shared_ptr goal); + std::shared_ptr + goal); /** * @brief Handles cancellation of an active LOS guidance goal. @@ -148,23 +148,23 @@ class LosGuidanceNode : public rclcpp::Node { * cancellation is accepted. */ rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle); + const std::shared_ptr> goal_handle); /** * @brief Handles an accepted LOS guidance goal. * @param goal_handle Handle to the accepted goal. */ - void handle_accepted(const std::shared_ptr> goal_handle); + void handle_accepted( + const std::shared_ptr> goal_handle); /** * @brief Executes the LOS guidance action. * @param goal_handle Handle to the active LOS guidance goal. */ void execute(const std::shared_ptr> goal_handle); + vortex_msgs::action::GuidanceWaypoint>> goal_handle); /** * @brief Service callback for changing the active LOS guidance method. @@ -202,7 +202,8 @@ class LosGuidanceNode : public rclcpp::Node { */ bool is_goal_feasible( const types::Inputs& inputs, - std::shared_ptr goal); + std::shared_ptr + goal); /** * @brief Checks if the LOS guidance goal has been missed based on the @@ -214,7 +215,7 @@ class LosGuidanceNode : public rclcpp::Node { bool has_active_segment_{false}; - rclcpp_action::Server::SharedPtr + rclcpp_action::Server::SharedPtr action_server_; rclcpp::Service::SharedPtr los_mode_service_; rclcpp::Publisher::SharedPtr reference_pub_; @@ -236,7 +237,7 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp_action::GoalUUID preempted_goal_id_; std::shared_ptr< - rclcpp_action::ServerGoalHandle> + rclcpp_action::ServerGoalHandle> goal_handle_; types::Inputs path_inputs_{}; diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 53c97f53f..75fca0bb9 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -107,7 +107,7 @@ void LosGuidanceNode::set_action_server() { this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); action_server_ = - rclcpp_action::create_server( + rclcpp_action::create_server( this, action_server_name, std::bind(&LosGuidanceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), @@ -259,7 +259,7 @@ void LosGuidanceNode::odom_msg_callback( rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, - std::shared_ptr goal) { + std::shared_ptr goal) { types::Inputs inputs_copy; { @@ -291,7 +291,7 @@ rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( const std::shared_ptr< - rclcpp_action::ServerGoalHandle> + rclcpp_action::ServerGoalHandle> goal_handle) { spdlog::info("Received request to cancel goal"); (void)goal_handle; @@ -300,7 +300,7 @@ rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( void LosGuidanceNode::handle_accepted( const std::shared_ptr< - rclcpp_action::ServerGoalHandle> + rclcpp_action::ServerGoalHandle> goal_handle) { std::thread{[this, goal_handle]() { execute(goal_handle); }}.detach(); } @@ -353,7 +353,7 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( bool LosGuidanceNode::is_goal_feasible( const types::Inputs& inputs, - std::shared_ptr goal) { + std::shared_ptr goal) { const auto& current_position = inputs.current_position; const auto& goal_point = goal->waypoint.pose.position; @@ -423,7 +423,7 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { void LosGuidanceNode::execute( const std::shared_ptr< - rclcpp_action::ServerGoalHandle> + rclcpp_action::ServerGoalHandle> goal_handle) { { std::unique_lock lock(mutex_); @@ -453,7 +453,8 @@ void LosGuidanceNode::execute( adaptive_los_->reset(); - auto result = std::make_shared(); + auto result = + std::make_shared(); nearest_been_to_goal_ = std::numeric_limits::infinity(); time_since_nearest_goal_ = 0.0; @@ -487,7 +488,8 @@ void LosGuidanceNode::execute( inputs_copy = path_inputs_; method_copy = method_; odom_copy = debug_current_odom_; - goal_reached_tol_copy = goal_reached_tol_; + goal_reached_tol_copy = + goal_handle->get_goal()->convergence_threshold; lock.unlock(); } From 11fb8d956693fdabb7f04e13f7ac7abe2898eea8 Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 16 Apr 2026 14:30:58 +0200 Subject: [PATCH 81/87] Add: add me in the mainterner list --- .../launch/guidance_test.launch.py | 72 +++++++++---------- .../launch/los_guidance.launch.py | 26 +++---- guidance/los_guidance/package.xml | 1 + 3 files changed, 50 insertions(+), 49 deletions(-) diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py index 299e48f95..ce1aef56a 100644 --- a/guidance/los_guidance/launch/guidance_test.launch.py +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -21,39 +21,39 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) - test_scenario = LaunchConfiguration("test_scenario").perform(context) - use_keyboard_joy = LaunchConfiguration("use_keyboard_joy") + test_scenario = LaunchConfiguration('test_scenario').perform(context) + use_keyboard_joy = LaunchConfiguration('use_keyboard_joy') - stonefish_dir = get_package_share_directory("stonefish_sim") - los_guidance_dir = get_package_share_directory("los_guidance") - keyboard_joy_dir = get_package_share_directory("keyboard_joy") - velocity_controller_dir = get_package_share_directory("velocity_controller_lqr") - utility_dir = get_package_share_directory("vortex_utility_nodes") + stonefish_dir = get_package_share_directory('stonefish_sim') + los_guidance_dir = get_package_share_directory('los_guidance') + keyboard_joy_dir = get_package_share_directory('keyboard_joy') + velocity_controller_dir = get_package_share_directory('velocity_controller_lqr') + utility_dir = get_package_share_directory('vortex_utility_nodes') stonefish_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, "launch", "simulation.launch.py") + os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') ), launch_arguments={ - "drone": drone, - "scenario": "nautilus_no_gpu", - "rendering": "true", + 'drone': drone, + 'scenario': 'nautilus_no_gpu', + 'rendering': 'true', }.items(), ) los_guidance_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(los_guidance_dir, "launch", "los_guidance.launch.py") + os.path.join(los_guidance_dir, 'launch', 'los_guidance.launch.py') ), launch_arguments={ - "drone": drone, - "namespace": namespace, + 'drone': drone, + 'namespace': namespace, }.items(), ) keyboard_joy = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(keyboard_joy_dir, "launch", "keyboard_joy_node.launch.py") + os.path.join(keyboard_joy_dir, 'launch', 'keyboard_joy_node.launch.py') ), condition=IfCondition(use_keyboard_joy), ) @@ -61,27 +61,27 @@ def launch_setup(context, *args, **kwargs): velocity_controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( - velocity_controller_dir, "launch", "velocity_controller_lqr.launch.py" + velocity_controller_dir, 'launch', 'velocity_controller_lqr.launch.py' ) ), launch_arguments={ - "drone": drone, - "namespace": namespace, + 'drone': drone, + 'namespace': namespace, }.items(), ) drone_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(stonefish_dir, "launch", "drone_sim.launch.py") + os.path.join(stonefish_dir, 'launch', 'drone_sim.launch.py') ), launch_arguments={ - "drone": drone, + 'drone': drone, }.items(), ) utility_node = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(utility_dir, "launch", "message_publisher.launch.py") + os.path.join(utility_dir, 'launch', 'message_publisher.launch.py') ) ) @@ -90,15 +90,15 @@ def launch_setup(context, *args, **kwargs): actions=[ ExecuteProcess( cmd=[ - "python3", - os.path.join(los_guidance_dir, "scripts", "test_scenarios.py"), - "--ros-args", - "-p", - f"drone:={drone}", - "-p", - f"test_scenario:={test_scenario}", + 'python3', + os.path.join(los_guidance_dir, 'scripts', 'test_scenarios.py'), + '--ros-args', + '-p', + f'drone:={drone}', + '-p', + f'test_scenario:={test_scenario}', ], - output="screen", + output='screen', ) ], ) @@ -116,17 +116,17 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( - declare_drone_and_namespace_args(default_drone="nautilus") + declare_drone_and_namespace_args(default_drone='nautilus') + [ DeclareLaunchArgument( - "test_scenario", - default_value="4_corner", - description="Scenario to run: 4_corner, circle, test_pitch, opposite_point", + 'test_scenario', + default_value='4_corner', + description='Scenario to run: 4_corner, circle, test_pitch, opposite_point', ), DeclareLaunchArgument( - "use_keyboard_joy", - default_value="true", - description="Launch keyboard joy node", + 'use_keyboard_joy', + default_value='true', + description='Launch keyboard joy node', ), OpaqueFunction(function=launch_setup), ] diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 6ffefc6df..bf205dd33 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -15,32 +15,32 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) los_config = os.path.join( - get_package_share_directory("los_guidance"), - "config", - "guidance_params.yaml", + get_package_share_directory('los_guidance'), + 'config', + 'guidance_params.yaml', ) drone_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - f"{drone}.yaml", + get_package_share_directory('auv_setup'), + 'config', + 'robots', + f'{drone}.yaml', ) return [ Node( - package="los_guidance", - executable="los_guidance_node", - name="los_guidance_node", + package='los_guidance', + executable='los_guidance_node', + name='los_guidance_node', namespace=namespace, parameters=[ drone_params, { - "los_config_file": los_config, - "time_step": 0.1, + 'los_config_file': los_config, + 'time_step': 0.1, }, ], - output="screen", + output='screen', ) ] diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 450afeae7..9d2ad338a 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -5,6 +5,7 @@ 0.0.0 LOS Guidance module for guidance of AUV talhanc + anbita MIT ament_cmake From aa03180a1de67413d967cd17fe5d5ab8a8634c04 Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 16 Apr 2026 17:17:03 +0200 Subject: [PATCH 82/87] for running tests again --- guidance/los_guidance/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 9d2ad338a..aabd47a6f 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -5,7 +5,7 @@ 0.0.0 LOS Guidance module for guidance of AUV talhanc - anbita + anbit MIT ament_cmake From 13147ec5b71334a0c71e3fb8ea99074637063369 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 19 Apr 2026 17:46:33 +0200 Subject: [PATCH 83/87] Refactor make changes to match new action type --- .github/workflows/simulator-test.yml | 1 + guidance/los_guidance/README.md | 6 +-- .../include/los_guidance/los_guidance_ros.hpp | 22 +++++---- .../los_guidance/scripts/test_scenarios.py | 48 +++++++++++-------- .../los_guidance/src/los_guidance_ros.cpp | 42 ++++++++++------ tests/simulator_tests/los_test/send_goal.py | 28 ++++++++--- 6 files changed, 95 insertions(+), 52 deletions(-) diff --git a/.github/workflows/simulator-test.yml b/.github/workflows/simulator-test.yml index c3a462f54..55941e4b3 100644 --- a/.github/workflows/simulator-test.yml +++ b/.github/workflows/simulator-test.yml @@ -22,4 +22,5 @@ jobs: "tests/simulator_tests/waypoint_navigation/orca_pseudoinverse.sh", "tests/simulator_tests/waypoint_manager_test/simulator_test.sh", "tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh", + "tests/simulator_tests/los_test/simulator_test.sh" ]' diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index b482a74e6..6bb0c244b 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -41,8 +41,8 @@ A waypoint can be sent to the guidance node using the action interface: ``` ros2 action send_goal /drone_name/los_guidance \ -vortex_msgs/action/LOSGuidance \ -"{goal: {header: {frame_id: world_ned}, point: {x: 0.0, y: 0.0, z: 0.0}}}" +vortex_msgs/action/GuidanceWaypoint \ +"{waypoint: {pose: {position: {x: 1.0, y: 2.0, z: 3.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, waypoint_mode: {mode: 1}}, convergence_threshold: 0.3}" ``` This command instructs the guidance node to start following a path toward the waypoint. @@ -344,7 +344,7 @@ ros2 launch los_guidance guidance_test.launch.py test_scenario:=circle | Interface | Name | Type | Message-Type | |----------|------|------|---------| -| Action Server | `/drone_name/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | +| Action Server | `/drone_name/los_guidance` | Goal input | `vortex_msgs/action/GuidanceWaypoint` | | Subscriber | `/drone_name/pose` | Vehicle pose | `geometry_msgs/PoseWithCovarianceStamped` | | Subscriber | `/drone_name/odom` | Vehicle velocity | `nav_msgs/Odometry` | | Publisher | `/drone_name/guidance/los` | Guidance reference (yaw, pitch, surge) | `vortex_msgs/LOSGuidance` | diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index fd7a2deb1..ae93f5b7a 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -45,6 +45,13 @@ class LosGuidanceNode : public rclcpp::Node { explicit LosGuidanceNode(const rclcpp::NodeOptions& options); private: + /** + * @brief Type alias for the goal handle used by the LOS guidance action + * server. + */ + using GoalHandleGuidanceWaypoint = + rclcpp_action::ServerGoalHandle; + /** * @brief Sets up the ROS subscribers and publishers used by the node. */ @@ -148,23 +155,20 @@ class LosGuidanceNode : public rclcpp::Node { * cancellation is accepted. */ rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr> goal_handle); + const std::shared_ptr goal_handle); /** * @brief Handles an accepted LOS guidance goal. * @param goal_handle Handle to the accepted goal. */ void handle_accepted( - const std::shared_ptr> goal_handle); + const std::shared_ptr goal_handle); /** * @brief Executes the LOS guidance action. * @param goal_handle Handle to the active LOS guidance goal. */ - void execute(const std::shared_ptr> goal_handle); + void execute(const std::shared_ptr goal_handle); /** * @brief Service callback for changing the active LOS guidance method. @@ -236,10 +240,8 @@ class LosGuidanceNode : public rclcpp::Node { std::mutex mutex_; rclcpp_action::GoalUUID preempted_goal_id_; - std::shared_ptr< - rclcpp_action::ServerGoalHandle> - rclcpp_action::ServerGoalHandle < - vortex_msgs::action::GuidanceWaypoint >> goal_handle_; + + std::shared_ptr goal_handle_; types::Inputs path_inputs_{}; double u_desired_{}; diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py index 9b8a82a8c..3150e902c 100644 --- a/guidance/los_guidance/scripts/test_scenarios.py +++ b/guidance/los_guidance/scripts/test_scenarios.py @@ -1,10 +1,11 @@ import math import rclpy +from geometry_msgs.msg import Pose from rclpy.action import ActionClient from rclpy.node import Node -from std_msgs.msg import Header -from vortex_msgs.action import LOSGuidance +from vortex_msgs.action import GuidanceWaypoint +from vortex_msgs.msg import Waypoint, WaypointMode class WaypointTest(Node): @@ -19,7 +20,7 @@ def __init__(self): self._action_client = ActionClient( self, - LOSGuidance, + GuidanceWaypoint, f"/{self.drone}/los_guidance", ) @@ -65,18 +66,14 @@ def generate_waypoints(self, test_scenario): return waypoints elif test_scenario == "test_pitch": - # 0 = water surface, do not go above - # this test scenario has no seabed, so z can be however we need. - # Keep all depths safely between these return [ - (3.0, 0.0, 1.0), # slight up - (6.0, 0.0, 2.0), # slight down - (9.0, 0.0, 1.0), # up again - (12.0, 0.0, 2.0), # down again + (3.0, 0.0, 1.0), + (6.0, 0.0, 2.0), + (9.0, 0.0, 1.0), + (12.0, 0.0, 2.0), ] elif test_scenario == "opposite_point": - # Go to one point, then the exact opposite point return [ (6.0, 4.0, self.depth), (-6.0, -4.0, self.depth), @@ -96,16 +93,29 @@ def send_next_goal(self): self._action_client.wait_for_server() - goal_msg = LOSGuidance.Goal() - - header = Header() - header.frame_id = "world_ned" - goal_msg.goal.header = header + goal_msg = GuidanceWaypoint.Goal() x, y, z = self.waypoints[self.current_index] - goal_msg.goal.point.x = float(x) - goal_msg.goal.point.y = float(y) - goal_msg.goal.point.z = float(z) + + pose = Pose() + pose.position.x = float(x) + pose.position.y = float(y) + pose.position.z = float(z) + + pose.orientation.x = 0.0 + pose.orientation.y = 0.0 + pose.orientation.z = 0.0 + pose.orientation.w = 1.0 + + waypoint = Waypoint() + waypoint.pose = pose + + waypoint_mode = WaypointMode() + waypoint_mode.mode = WaypointMode.ONLY_POSITION + waypoint.waypoint_mode = waypoint_mode + + goal_msg.waypoint = waypoint + goal_msg.convergence_threshold = 0.3 self.get_logger().info( f"Sending waypoint {self.current_index + 1}/{len(self.waypoints)}: " diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 9caa2edbd..c4335f2a7 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -50,6 +50,7 @@ LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) spdlog::info(start_message); } +// Subscribers + publishers void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); @@ -98,6 +99,7 @@ void LosGuidanceNode::set_subscribers_and_publisher() { std::placeholders::_1)); } +// Action server setup void LosGuidanceNode::set_action_server() { this->declare_parameter("action_servers.los"); std::string action_server_name = @@ -118,6 +120,7 @@ void LosGuidanceNode::set_action_server() { rcl_action_server_get_default_options(), cb_group_); } +// Service server setup void LosGuidanceNode::set_service_server() { this->declare_parameter("services.los_mode", "set_los_mode"); std::string service_name = @@ -128,6 +131,7 @@ void LosGuidanceNode::set_service_server() { std::placeholders::_1, std::placeholders::_2)); } +// Adaptive LOS setup void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { auto adaptive_los_config = config["adaptive_los"]; auto params = AdaptiveLosParams{}; @@ -150,6 +154,7 @@ void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { } } +// Proportional LOS setup void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { auto proportional_los_config = config["prop_los"]; auto params = ProportionalLosParams{}; @@ -168,6 +173,7 @@ void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { } } +// Integral LOS setup void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { auto integral_los_config = config["integer_los"]; auto params = IntegralLosParams{}; @@ -190,6 +196,7 @@ void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { } } +// Vector field LOS setup void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { auto vector_field_config = config["vector_field_los"]; auto params = VectorFieldLosParams{}; @@ -213,6 +220,7 @@ void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { } } +// Waypoint callback void LosGuidanceNode::waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { std::unique_lock lock(mutex_); @@ -234,6 +242,7 @@ void LosGuidanceNode::waypoint_callback( new_wp.z); } +// Pose callback void LosGuidanceNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { @@ -243,6 +252,7 @@ void LosGuidanceNode::pose_callback( lock.unlock(); } +// Odometry callback void LosGuidanceNode::odom_callback( const nav_msgs::msg::Odometry::SharedPtr msg) { std::unique_lock lock(mutex_); @@ -250,6 +260,7 @@ void LosGuidanceNode::odom_callback( lock.unlock(); } +// Euler (yaw) callback void LosGuidanceNode::odom_msg_callback( const vortex_msgs::msg::PoseEulerStamped::SharedPtr msg) { std::unique_lock lock(mutex_); @@ -257,6 +268,7 @@ void LosGuidanceNode::odom_msg_callback( lock.unlock(); } +// Goal handler rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { @@ -281,32 +293,29 @@ rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( RCLCPP_INFO(this->get_logger(), "Aborting current goal and accepting new goal"); preempted_goal_id_ = goal_handle_->get_goal_id(); + lock.unlock(); } - lock.unlock(); } RCLCPP_INFO(this->get_logger(), "Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } +// Cancel handler rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - rclcpp_action::ServerGoalHandle < - vortex_msgs::action::GuidanceWaypoint >> goal_handle) { + const std::shared_ptr goal_handle) { spdlog::info("Received request to cancel goal"); (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; } +// Accepted handler void LosGuidanceNode::handle_accepted( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - rclcpp_action::ServerGoalHandle < - vortex_msgs::action::GuidanceWaypoint >> goal_handle) { + const std::shared_ptr goal_handle) { std::thread{[this, goal_handle]() { execute(goal_handle); }}.detach(); } +// Service callback void LosGuidanceNode::set_los_mode( const std::shared_ptr request, std::shared_ptr response) { @@ -320,6 +329,7 @@ void LosGuidanceNode::set_los_mode( response->success = true; } +// Fill LOS reference message vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs) { vortex_msgs::msg::LOSGuidance reference_msg; @@ -346,13 +356,14 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( double abs_err = std::abs(yaw_error); double u_cmd = u_desired_copy / (1.0 + 0.5 * abs_err); - u_cmd = std::clamp(u_cmd, 0.12, u_desired_copy); + u_cmd = std::clamp(u_cmd, 0.15, u_desired_copy); reference_msg.surge = u_cmd; return reference_msg; } +// Check if goal is feasible bool LosGuidanceNode::is_goal_feasible( const types::Inputs& inputs, std::shared_ptr goal) { @@ -369,6 +380,7 @@ bool LosGuidanceNode::is_goal_feasible( return std::abs(required_pitch) <= max_pitch_angle_; } +// Check if goal is missed bool LosGuidanceNode::is_goal_missed(const types::Inputs& inputs) { const double distance_to_goal = (inputs.current_position - inputs.next_point).as_vector().norm(); @@ -391,6 +403,7 @@ bool LosGuidanceNode::is_goal_missed(const types::Inputs& inputs) { return time_since_nearest_goal_ >= missed_goal_timeout_; } +// Load LOS config YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { try { YAML::Node config = YAML::LoadFile(yaml_file_path); @@ -402,9 +415,11 @@ YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { } } +// Parse common config void LosGuidanceNode::parse_common_config(YAML::Node common_config) { try { std::unique_lock lock(mutex_); + u_desired_ = common_config["u_desired"].as(); max_pitch_angle_ = common_config["max_pitch_angle"].as(); goal_reached_tol_ = common_config["goal_reached_tol"].as(); @@ -423,11 +438,9 @@ void LosGuidanceNode::parse_common_config(YAML::Node common_config) { } } +// Execute action void LosGuidanceNode::execute( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - rclcpp_action::ServerGoalHandle < - vortex_msgs::action::GuidanceWaypoint >> goal_handle) { + const std::shared_ptr goal_handle) { { std::unique_lock lock(mutex_); this->goal_handle_ = goal_handle; @@ -458,6 +471,7 @@ void LosGuidanceNode::execute( auto result = std::make_shared(); + nearest_been_to_goal_ = std::numeric_limits::infinity(); time_since_nearest_goal_ = 0.0; diff --git a/tests/simulator_tests/los_test/send_goal.py b/tests/simulator_tests/los_test/send_goal.py index 7acbac729..d35c68eea 100644 --- a/tests/simulator_tests/los_test/send_goal.py +++ b/tests/simulator_tests/los_test/send_goal.py @@ -1,13 +1,15 @@ import rclpy +from geometry_msgs.msg import Pose from rclpy.action import ActionClient from rclpy.node import Node from vortex_msgs.action import GuidanceWaypoint +from vortex_msgs.msg import Waypoint, WaypointMode class LOSGuidanceClient(Node): def __init__(self): super().__init__('los_guidance_client') - # Create the action client + self._action_client = ActionClient( self, GuidanceWaypoint, '/nautilus/los_guidance' ) @@ -16,13 +18,27 @@ def __init__(self): def send_goal(self): goal_msg = GuidanceWaypoint.Goal() - # Create a message with the goal - goal_msg.waypoint.pose.position.x = 20.0 - goal_msg.waypoint.pose.position.y = 20.0 - goal_msg.waypoint.pose.position.z = 5.0 + pose = Pose() + pose.position.x = 20.0 + pose.position.y = 20.0 + pose.position.z = 5.0 + pose.orientation.x = 0.0 + pose.orientation.y = 0.0 + pose.orientation.z = 0.0 + pose.orientation.w = 1.0 + + waypoint = Waypoint() + waypoint.pose = pose + + waypoint_mode = WaypointMode() + waypoint_mode.mode = WaypointMode.ONLY_POSITION + waypoint.waypoint_mode = waypoint_mode + + goal_msg.waypoint = waypoint + goal_msg.convergence_threshold = 0.5 - # Send the goal asynchronously self._action_client.wait_for_server(timeout_sec=10.0) + self.get_logger().info('Sending goal...') self._send_goal_future = self._action_client.send_goal_async(goal_msg) self._send_goal_future.add_done_callback(self.goal_response_callback) From 78d9cae98cc683319f2ac4087453e13c0f6342d1 Mon Sep 17 00:00:00 2001 From: Anbit Date: Sun, 19 Apr 2026 18:50:16 +0200 Subject: [PATCH 84/87] Fix: Los test issues --- auv_setup/launch/autopilot.launch.py | 7 ++++++- guidance/los_guidance/launch/los_guidance.launch.py | 1 - guidance/los_guidance/src/los_guidance_ros.cpp | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index 3a84897c4..6cba6f9ae 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -40,7 +40,12 @@ def launch_setup(context, *args, **kwargs): executable="los_guidance_node", name="los_guidance_node", namespace=namespace, - parameters=[drone_params, los_config], + parameters=[ + drone_params, + { + "los_config_file": los_config, + }, + ], output="screen", ) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index bf205dd33..edd0c2b1d 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -37,7 +37,6 @@ def launch_setup(context, *args, **kwargs): drone_params, { 'los_config_file': los_config, - 'time_step': 0.1, }, ], output='screen', diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index c4335f2a7..3017e7ba4 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -29,7 +29,7 @@ namespace vortex::guidance::los { // Constructor LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) : Node("los_guidance_node", options) { - double time_step_s = this->declare_parameter("time_step"); + double time_step_s = 0.1; time_step_ = std::chrono::milliseconds(static_cast(time_step_s * 1000)); From 52d1daa1609e1ab2b266f6d0b2330bc3240e6b1d Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 7 May 2026 14:44:00 +0200 Subject: [PATCH 85/87] Start: Splitting of ros and state code --- guidance/los_guidance/CMakeLists.txt | 3 +- .../los_guidance/lib/guidance_manager.hpp | 177 ++++++++++ .../{ => ros}/los_guidance_ros.hpp | 125 +------ .../launch/los_guidance.launch.py | 4 +- guidance/los_guidance/package.xml | 1 + .../los_guidance/src/lib/guidance_manager.cpp | 310 ++++++++++++++++++ .../src/{ => ros}/los_guidance_ros.cpp | 293 ++--------------- 7 files changed, 532 insertions(+), 381 deletions(-) create mode 100644 guidance/los_guidance/include/los_guidance/lib/guidance_manager.hpp rename guidance/los_guidance/include/los_guidance/{ => ros}/los_guidance_ros.hpp (60%) create mode 100644 guidance/los_guidance/src/lib/guidance_manager.cpp rename guidance/los_guidance/src/{ => ros}/los_guidance_ros.cpp (54%) diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index f8e7e7b4c..f4cc6c115 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -30,8 +30,9 @@ add_library(${LIB_NAME} SHARED src/lib/proportional_los.cpp src/lib/integral_los.cpp src/lib/adaptive_los.cpp - src/los_guidance_ros.cpp src/lib/vector_field_los.cpp + src/lib/guidance_manager.cpp + src/ros/los_guidance_ros.cpp ) ament_target_dependencies(${LIB_NAME} diff --git a/guidance/los_guidance/include/los_guidance/lib/guidance_manager.hpp b/guidance/los_guidance/include/los_guidance/lib/guidance_manager.hpp new file mode 100644 index 000000000..59259ff13 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/guidance_manager.hpp @@ -0,0 +1,177 @@ +/** + * @file guidance_manager.hpp + * @brief The LosGuidanceStateManager class executes LOS guidance logic, + * manages LOS method selection, and tracks goal feasibility and + * progress. + */ +#ifndef LOS_GUIDANCE__LIB__GUIDANCE_MANAGER_HPP_ +#define LOS_GUIDANCE__LIB__GUIDANCE_MANAGER_HPP_ + +#include +#include +#include +#include +#include + +#include +#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" +#include "los_guidance/lib/vector_field_los.hpp" + +namespace vortex::guidance::los { + +class LosGuidanceStateManager { + public: + /** + * @brief Constructs a LosGuidanceStateManager object and loads LOS + * guidance configuration from the specified YAML file. + * @param yaml_file_path Path to the YAML configuration file. + */ + explicit LosGuidanceStateManager(const std::string& yaml_file_path); + + /** + * @brief Initializes the adaptive LOS guidance module from configuration. + * @param config YAML configuration node containing adaptive LOS parameters. + */ + void set_adaptive_los_guidance(YAML::Node config); + + /** + * @brief Initializes the proportional LOS guidance module from + * configuration. + * @param config YAML configuration node containing proportional LOS + * parameters. + */ + void set_proportional_los_guidance(YAML::Node config); + + /** + * @brief Initializes the integral LOS guidance module from configuration. + * @param config YAML configuration node containing integral LOS parameters. + */ + void set_integral_los_guidance(YAML::Node config); + + /** + * @brief Initializes the vector field LOS guidance module from + * configuration. + * @param config YAML configuration node containing vector field LOS + * parameters. + */ + void set_vector_field_guidance(YAML::Node config); + + /** + * @brief Loads the LOS guidance YAML configuration file. + * @param yaml_file_path Path to the YAML configuration file. + * @return YAML::Node Parsed YAML configuration. + */ + YAML::Node get_los_config(std::string yaml_file_path); + + /** + * @brief Parses common guidance parameters shared by all LOS methods. + * @param common_config YAML node containing common guidance parameters. + */ + void parse_common_config(YAML::Node common_config); + + /** + * @brief Checks if the given LOS guidance goal is feasible based on the + * current state. + * @param goal The goal to check for feasibility. + * @return true if the goal is feasible, false otherwise. + */ + bool is_goal_feasible( + std::shared_ptr + goal); + + /** + * @brief Checks if the LOS guidance goal has been missed based on the + * current state. + * @return true if the goal is missed, false otherwise. + */ + bool is_goal_missed(); + + /** + * @brief Checks if the LOS guidance goal has been reached. + * @param tolerance Distance tolerance for goal convergence. + * @return true if the goal is reached, false otherwise. + */ + bool is_goal_reached(double tolerance); + + /** + * @brief Updates the current waypoint. + * @param new_wp New waypoint to navigate to. + */ + void update_waypoint(const types::Point& new_wp); + + /** + * @brief Updates the current vehicle position. + * @param position Current position of the vehicle. + */ + void update_position(const types::Point& position); + + /** + * @brief Updates the current yaw angle. + * @param yaw Current yaw angle in radians. + */ + void update_yaw(double yaw); + + /** + * @brief Sets the active LOS guidance method. + * @param method The LOS method to use. + */ + void set_los_method(types::ActiveLosMethod method); + + /** + * @brief Initializes a new goal. + * @param new_wp New waypoint to navigate to. + */ + void initialize_goal(const types::Point& new_wp); + + /** + * @brief Calculates LOS guidance outputs based on current state. + * @return types::Outputs Calculated guidance outputs. + */ + types::Outputs calculate_outputs(); + + /** + * @brief Gets the maximum pitch angle limit. + * @return Maximum pitch angle in radians. + */ + double get_max_pitch_angle() const; + + /** + * @brief Gets the current yaw angle. + * @return Current yaw angle in radians. + */ + double get_current_yaw() const; + + /** + * @brief Gets the desired surge velocity. + * @return Desired surge velocity. + */ + double get_u_desired() const; + + private: + types::Inputs path_inputs_{}; + double u_desired_{}; + double goal_reached_tol_{}; + double max_pitch_angle_{}; + double current_yaw_{}; + double time_step_s_{}; + types::ActiveLosMethod method_{}; + double nearest_been_to_goal_{std::numeric_limits::max()}; + double time_since_nearest_goal_{}; + double missed_goal_distance_margin_{}; + double missed_goal_timeout_{}; + bool has_active_segment_{false}; + + std::unique_ptr adaptive_los_{}; + std::unique_ptr integral_los_{}; + std::unique_ptr proportional_los_{}; + std::unique_ptr vector_field_los_{}; + + mutable std::mutex mutex_; +}; + +} // namespace vortex::guidance::los + +#endif // LOS_GUIDANCE__LIB__GUIDANCE_MANAGER_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/ros/los_guidance_ros.hpp similarity index 60% rename from guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp rename to guidance/los_guidance/include/los_guidance/ros/los_guidance_ros.hpp index ae93f5b7a..82f6eada7 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/ros/los_guidance_ros.hpp @@ -3,16 +3,20 @@ * @brief The LosGuidanceNode class initializes ROS interfaces, loads * configuration parameters, and runs the LOS guidance node. */ -#ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ -#define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#ifndef LOS_GUIDANCE__ROS__LOS_GUIDANCE_ROS_HPP_ +#define LOS_GUIDANCE__ROS__LOS_GUIDANCE_ROS_HPP_ #include #include #include #include #include + #include #include + +#include + #include #include #include @@ -20,15 +24,7 @@ #include #include -#include -#include -#include - -#include "los_guidance/lib/adaptive_los.hpp" -#include "los_guidance/lib/integral_los.hpp" -#include "los_guidance/lib/proportional_los.hpp" -#include "los_guidance/lib/types.hpp" -#include "los_guidance/lib/vector_field_los.hpp" +#include "los_guidance/lib/guidance_manager.hpp" namespace vortex::guidance::los { @@ -67,47 +63,6 @@ class LosGuidanceNode : public rclcpp::Node { */ void set_service_server(); - /** - * @brief Initializes the adaptive LOS guidance module from configuration. - * @param config YAML configuration node containing adaptive LOS parameters. - */ - void set_adaptive_los_guidance(YAML::Node config); - - /** - * @brief Initializes the proportional LOS guidance module from - * configuration. - * @param config YAML configuration node containing proportional LOS - * parameters. - */ - void set_proportional_los_guidance(YAML::Node config); - - /** - * @brief Initializes the integral LOS guidance module from configuration. - * @param config YAML configuration node containing integral LOS parameters. - */ - void set_integral_los_guidance(YAML::Node config); - - /** - * @brief Initializes the vector field LOS guidance module from - * configuration. - * @param config YAML configuration node containing vector field LOS - * parameters. - */ - void set_vector_field_guidance(YAML::Node config); - - /** - * @brief Loads the LOS guidance YAML configuration file. - * @param yaml_file_path Path to the YAML configuration file. - * @return YAML::Node Parsed YAML configuration. - */ - YAML::Node get_los_config(std::string yaml_file_path); - - /** - * @brief Parses common guidance parameters shared by all LOS methods. - * @param common_config YAML node containing common guidance parameters. - */ - void parse_common_config(YAML::Node common_config); - /** * @brief Callback for receiving waypoint updates. * @param msg Received waypoint message. @@ -181,49 +136,18 @@ class LosGuidanceNode : public rclcpp::Node { std::shared_ptr response); /** - * @brief Publishes debug information about the current vehicle state. - * @param current_pose Current vehicle pose used for debug publishing. - */ - void publish_state_debug( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr - current_pose); - - /** - * @brief Fills a LOS guidance reference message from computed outputs and - * inputs. - * @param output Calculated LOS guidance outputs. - * @param inputs Current LOS guidance inputs. + * @brief Fills a LOS guidance reference message from computed outputs. + * @param outputs Calculated LOS guidance outputs. * @return vortex_msgs::msg::LOSGuidance Populated LOS guidance reference * message. */ - vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); - - /** - * @brief Checks if the given LOS guidance goal is feasible based on the - * provided inputs. - * @param inputs Current LOS guidance inputs. - * @return true if the goal is feasible, false otherwise. - */ - bool is_goal_feasible( - const types::Inputs& inputs, - std::shared_ptr - goal); - - /** - * @brief Checks if the LOS guidance goal has been missed based on the - * provided inputs. - * @param inputs Current LOS guidance inputs. - * @return true if the goal is missed, false otherwise. - */ - bool is_goal_missed(const types::Inputs& inputs); - - bool has_active_segment_{false}; + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs outputs); + // ROS interfaces rclcpp_action::Server::SharedPtr action_server_; rclcpp::Service::SharedPtr los_mode_service_; rclcpp::Publisher::SharedPtr reference_pub_; - rclcpp::Publisher::SharedPtr los_debug_pub_; rclcpp::Publisher::SharedPtr state_debug_pub_; rclcpp::Subscription::SharedPtr @@ -233,36 +157,19 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr message_pub_sub_; - rclcpp::TimerBase::SharedPtr reference_pub_timer_; rclcpp::CallbackGroup::SharedPtr cb_group_; + // State manager + std::unique_ptr state_manager_; + + // Node-specific state std::chrono::milliseconds time_step_; std::mutex mutex_; - rclcpp_action::GoalUUID preempted_goal_id_; - std::shared_ptr goal_handle_; - - types::Inputs path_inputs_{}; - double u_desired_{}; - double goal_reached_tol_{}; - double max_pitch_angle_{}; - double current_yaw_{}; - types::ActiveLosMethod method_{}; - - double nearest_been_to_goal_{std::numeric_limits::max()}; - double time_since_nearest_goal_{}; - double missed_goal_distance_margin_{}; - double missed_goal_timeout_{}; - - std::unique_ptr adaptive_los_{}; - std::unique_ptr integral_los_{}; - std::unique_ptr proportional_los_{}; - std::unique_ptr vector_field_los_{}; - nav_msgs::msg::Odometry::SharedPtr debug_current_odom_{}; }; } // namespace vortex::guidance::los -#endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#endif // LOS_GUIDANCE__ROS__LOS_GUIDANCE_ROS_HPP_ diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index edd0c2b1d..89a507a67 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -14,7 +14,7 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) - los_config = os.path.join( + los_config_yaml = os.path.join( get_package_share_directory('los_guidance'), 'config', 'guidance_params.yaml', @@ -36,7 +36,7 @@ def launch_setup(context, *args, **kwargs): parameters=[ drone_params, { - 'los_config_file': los_config, + 'los_config_file_path': los_config_yaml, }, ], output='screen', diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index aabd47a6f..479608b20 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -17,6 +17,7 @@ vortex_msgs nav_msgs vortex_utils_ros + vortex_utility_nodes eigen yaml-cpp diff --git a/guidance/los_guidance/src/lib/guidance_manager.cpp b/guidance/los_guidance/src/lib/guidance_manager.cpp new file mode 100644 index 000000000..8459f352e --- /dev/null +++ b/guidance/los_guidance/src/lib/guidance_manager.cpp @@ -0,0 +1,310 @@ +#include "los_guidance/lib/guidance_manager.hpp" +#include +#include +#include +#include + +namespace vortex::guidance::los { + +// Constructor +LosGuidanceStateManager::LosGuidanceStateManager( + const std::string& yaml_file_path) { + time_step_s_ = 0.1; + + YAML::Node config = get_los_config(yaml_file_path); + + parse_common_config(config["common"]); + set_adaptive_los_guidance(config); + set_proportional_los_guidance(config); + set_integral_los_guidance(config); + set_vector_field_guidance(config); +} + +// Adaptive LOS setup +void LosGuidanceStateManager::set_adaptive_los_guidance(YAML::Node config) { + auto adaptive_los_config = config["adaptive_los"]; + auto params = AdaptiveLosParams{}; + + try { + params.lookahead_distance_h = + adaptive_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + adaptive_los_config["lookahead_distance_v"].as(); + params.adaptation_gain_h = + adaptive_los_config["adaptation_gain_h"].as(); + params.adaptation_gain_v = + adaptive_los_config["adaptation_gain_v"].as(); + params.time_step = time_step_s_; + + adaptive_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load adaptive_los parameters: ") + e.what()); + } +} + +// Proportional LOS setup +void LosGuidanceStateManager::set_proportional_los_guidance(YAML::Node config) { + auto proportional_los_config = config["prop_los"]; + auto params = ProportionalLosParams{}; + + try { + params.lookahead_distance_h = + proportional_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + proportional_los_config["lookahead_distance_v"].as(); + + proportional_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load proportional_los parameters: ") + + e.what()); + } +} + +// Integral LOS setup +void LosGuidanceStateManager::set_integral_los_guidance(YAML::Node config) { + auto integral_los_config = config["integer_los"]; + auto params = IntegralLosParams{}; + + try { + params.proportional_gain_h = + integral_los_config["proportional_gain_h"].as(); + params.proportional_gain_v = + integral_los_config["proportional_gain_v"].as(); + params.integral_gain_h = + integral_los_config["integral_gain_h"].as(); + params.integral_gain_v = + integral_los_config["integral_gain_v"].as(); + params.time_step = time_step_s_; + + integral_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load integral_los parameters: ") + e.what()); + } +} + +// Vector field LOS setup +void LosGuidanceStateManager::set_vector_field_guidance(YAML::Node config) { + auto vector_field_config = config["vector_field_los"]; + auto params = VectorFieldLosParams{}; + + try { + params.max_approach_angle_h = + vector_field_config["max_approach_angle_h"].as(); + params.max_approach_angle_v = + vector_field_config["max_approach_angle_v"].as(); + params.proportional_gain_h = + vector_field_config["proportional_gain_h"].as(); + params.proportional_gain_v = + vector_field_config["proportional_gain_v"].as(); + params.time_step = time_step_s_; + + vector_field_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load vector_field_los parameters: ") + + e.what()); + } +} + +// Load LOS config +YAML::Node LosGuidanceStateManager::get_los_config(std::string yaml_file_path) { + try { + YAML::Node config = YAML::LoadFile(yaml_file_path); + return config; + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load LOS config file '") + yaml_file_path + + "': " + e.what()); + } +} + +// Parse common config +void LosGuidanceStateManager::parse_common_config(YAML::Node common_config) { + try { + std::unique_lock lock(mutex_); + + u_desired_ = common_config["u_desired"].as(); + max_pitch_angle_ = common_config["max_pitch_angle"].as(); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); + missed_goal_timeout_ = + common_config["missed_goal_timeout"].as(); + missed_goal_distance_margin_ = + common_config["missed_goal_distance_margin"].as(); + + method_ = static_cast( + common_config["active_los_method"].as()); + + lock.unlock(); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load common parameters: ") + e.what()); + } +} + +// Update waypoint +void LosGuidanceStateManager::update_waypoint(const types::Point& new_wp) { + std::unique_lock lock(mutex_); + + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } + + lock.unlock(); +} + +// Update position +void LosGuidanceStateManager::update_position(const types::Point& position) { + std::unique_lock lock(mutex_); + path_inputs_.current_position = position; + lock.unlock(); +} + +// Update yaw +void LosGuidanceStateManager::update_yaw(double yaw) { + std::unique_lock lock(mutex_); + current_yaw_ = yaw; + lock.unlock(); +} + +// Set LOS method +void LosGuidanceStateManager::set_los_method(types::ActiveLosMethod method) { + std::unique_lock lock(mutex_); + method_ = method; + lock.unlock(); +} + +// Initialize goal +void LosGuidanceStateManager::initialize_goal(const types::Point& new_wp) { + std::unique_lock lock(mutex_); + + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } + + lock.unlock(); + + adaptive_los_->reset(); + + nearest_been_to_goal_ = std::numeric_limits::infinity(); + time_since_nearest_goal_ = 0.0; +} + +// Check if goal is feasible +bool LosGuidanceStateManager::is_goal_feasible( + std::shared_ptr goal) { + std::unique_lock lock(mutex_); + types::Inputs inputs_copy = path_inputs_; + double max_pitch_angle_copy = max_pitch_angle_; + lock.unlock(); + + const auto& current_position = inputs_copy.current_position; + const auto& goal_point = goal->waypoint.pose.position; + + const double dx = goal_point.x - current_position.x; + const double dy = goal_point.y - current_position.y; + const double dz = goal_point.z - current_position.z; + + const double horizontal_distance = std::sqrt(dx * dx + dy * dy); + const double required_pitch = std::atan2(-dz, horizontal_distance); + + return std::abs(required_pitch) <= max_pitch_angle_copy; +} + +// Check if goal is missed +bool LosGuidanceStateManager::is_goal_missed() { + std::unique_lock lock(mutex_); + types::Inputs inputs_copy = path_inputs_; + lock.unlock(); + + const double distance_to_goal = + (inputs_copy.current_position - inputs_copy.next_point) + .as_vector() + .norm(); + + if (distance_to_goal < nearest_been_to_goal_) { + nearest_been_to_goal_ = distance_to_goal; + time_since_nearest_goal_ = 0.0; + return false; + } + + if (distance_to_goal > + nearest_been_to_goal_ + missed_goal_distance_margin_) { + time_since_nearest_goal_ += time_step_s_; + } else { + time_since_nearest_goal_ = 0.0; + } + + return time_since_nearest_goal_ >= missed_goal_timeout_; +} + +// Check if goal is reached +bool LosGuidanceStateManager::is_goal_reached(double tolerance) { + std::unique_lock lock(mutex_); + types::Inputs inputs_copy = path_inputs_; + lock.unlock(); + + return (inputs_copy.current_position - inputs_copy.next_point) + .as_vector() + .norm() < tolerance; +} + +// Calculate outputs +types::Outputs LosGuidanceStateManager::calculate_outputs() { + std::unique_lock lock(mutex_); + types::Inputs inputs_copy = path_inputs_; + types::ActiveLosMethod method_copy = method_; + lock.unlock(); + + types::Outputs outputs; + + switch (method_copy) { + case types::ActiveLosMethod::ADAPTIVE: + outputs = adaptive_los_->calculate_outputs(inputs_copy); + break; + case types::ActiveLosMethod::PROPORTIONAL: + outputs = proportional_los_->calculate_outputs(inputs_copy); + break; + case types::ActiveLosMethod::INTEGRAL: + outputs = integral_los_->calculate_outputs(inputs_copy); + break; + case types::ActiveLosMethod::VECTOR_FIELD: + outputs = vector_field_los_->calculate_outputs(inputs_copy); + break; + default: + spdlog::error("Invalid LOS method selected"); + break; + } + + return outputs; +} + +// Getters +double LosGuidanceStateManager::get_max_pitch_angle() const { + std::unique_lock lock(mutex_); + return max_pitch_angle_; +} + +double LosGuidanceStateManager::get_current_yaw() const { + std::unique_lock lock(mutex_); + return current_yaw_; +} + +double LosGuidanceStateManager::get_u_desired() const { + std::unique_lock lock(mutex_); + return u_desired_; +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/ros/los_guidance_ros.cpp similarity index 54% rename from guidance/los_guidance/src/los_guidance_ros.cpp rename to guidance/los_guidance/src/ros/los_guidance_ros.cpp index 3017e7ba4..3ba7a60a6 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/ros/los_guidance_ros.cpp @@ -1,14 +1,11 @@ -#include "los_guidance/los_guidance_ros.hpp" +#include "los_guidance/ros/los_guidance_ros.hpp" #include #include -#include #include #include #include #include -#include "los_guidance/lib/types.hpp" - #ifdef NDEBUG constexpr bool debug = false; #else @@ -34,18 +31,14 @@ LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) std::chrono::milliseconds(static_cast(time_step_s * 1000)); const std::string yaml_path = - this->declare_parameter("los_config_file"); + this->declare_parameter("los_config_file_path"); - YAML::Node config = get_los_config(yaml_path); + // Initialize the state manager + state_manager_ = std::make_unique(yaml_path); - parse_common_config(config["common"]); set_subscribers_and_publisher(); set_action_server(); set_service_server(); - set_adaptive_los_guidance(config); - set_proportional_los_guidance(config); - set_integral_los_guidance(config); - set_vector_field_guidance(config); spdlog::info(start_message); } @@ -131,112 +124,12 @@ void LosGuidanceNode::set_service_server() { std::placeholders::_1, std::placeholders::_2)); } -// Adaptive LOS setup -void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { - auto adaptive_los_config = config["adaptive_los"]; - auto params = AdaptiveLosParams{}; - - try { - params.lookahead_distance_h = - adaptive_los_config["lookahead_distance_h"].as(); - params.lookahead_distance_v = - adaptive_los_config["lookahead_distance_v"].as(); - params.adaptation_gain_h = - adaptive_los_config["adaptation_gain_h"].as(); - params.adaptation_gain_v = - adaptive_los_config["adaptation_gain_v"].as(); - params.time_step = static_cast(time_step_.count()) / 1000.0; - - adaptive_los_ = std::make_unique(params); - } catch (const YAML::Exception& e) { - throw std::runtime_error( - std::string("Failed to load adaptive_los parameters: ") + e.what()); - } -} - -// Proportional LOS setup -void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { - auto proportional_los_config = config["prop_los"]; - auto params = ProportionalLosParams{}; - - try { - params.lookahead_distance_h = - proportional_los_config["lookahead_distance_h"].as(); - params.lookahead_distance_v = - proportional_los_config["lookahead_distance_v"].as(); - - proportional_los_ = std::make_unique(params); - } catch (const YAML::Exception& e) { - throw std::runtime_error( - std::string("Failed to load proportional_los parameters: ") + - e.what()); - } -} - -// Integral LOS setup -void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { - auto integral_los_config = config["integer_los"]; - auto params = IntegralLosParams{}; - - try { - params.proportional_gain_h = - integral_los_config["proportional_gain_h"].as(); - params.proportional_gain_v = - integral_los_config["proportional_gain_v"].as(); - params.integral_gain_h = - integral_los_config["integral_gain_h"].as(); - params.integral_gain_v = - integral_los_config["integral_gain_v"].as(); - params.time_step = static_cast(time_step_.count()) / 1000.0; - - integral_los_ = std::make_unique(params); - } catch (const YAML::Exception& e) { - throw std::runtime_error( - std::string("Failed to load integral_los parameters: ") + e.what()); - } -} - -// Vector field LOS setup -void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { - auto vector_field_config = config["vector_field_los"]; - auto params = VectorFieldLosParams{}; - - try { - params.max_approach_angle_h = - vector_field_config["max_approach_angle_h"].as(); - params.max_approach_angle_v = - vector_field_config["max_approach_angle_v"].as(); - params.proportional_gain_h = - vector_field_config["proportional_gain_h"].as(); - params.proportional_gain_v = - vector_field_config["proportional_gain_v"].as(); - params.time_step = static_cast(time_step_.count()) / 1000.0; - - vector_field_los_ = std::make_unique(params); - } catch (const YAML::Exception& e) { - throw std::runtime_error( - std::string("Failed to load vector_field_los parameters: ") + - e.what()); - } -} - // Waypoint callback void LosGuidanceNode::waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { - std::unique_lock lock(mutex_); - const auto new_wp = types::Point::point_from_ros(wp_msg->point); - if (!has_active_segment_) { - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = new_wp; - has_active_segment_ = true; - } else { - path_inputs_.prev_point = path_inputs_.next_point; - path_inputs_.next_point = new_wp; - } - - lock.unlock(); + state_manager_->update_waypoint(new_wp); spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, new_wp.z); @@ -246,10 +139,10 @@ void LosGuidanceNode::waypoint_callback( void LosGuidanceNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { - std::unique_lock lock(mutex_); - path_inputs_.current_position = + types::Point position = types::Point::point_from_ros(current_pose->pose.pose.position); - lock.unlock(); + + state_manager_->update_position(position); } // Odometry callback @@ -263,24 +156,14 @@ void LosGuidanceNode::odom_callback( // Euler (yaw) callback void LosGuidanceNode::odom_msg_callback( const vortex_msgs::msg::PoseEulerStamped::SharedPtr msg) { - std::unique_lock lock(mutex_); - current_yaw_ = msg->yaw; - lock.unlock(); + state_manager_->update_yaw(msg->yaw); } // Goal handler rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { - types::Inputs inputs_copy; - - { - std::unique_lock lock(mutex_); - inputs_copy = path_inputs_; - lock.unlock(); - } - - if (!is_goal_feasible(inputs_copy, goal)) { + if (!state_manager_->is_goal_feasible(goal)) { RCLCPP_WARN(this->get_logger(), "Rejected goal request: waypoint is not reachable with " "current pitch limit"); @@ -319,11 +202,8 @@ void LosGuidanceNode::handle_accepted( void LosGuidanceNode::set_los_mode( const std::shared_ptr request, std::shared_ptr response) { - { - std::unique_lock lock(mutex_); - method_ = static_cast(request->mode); - lock.unlock(); - } + state_manager_->set_los_method( + static_cast(request->mode)); spdlog::info("LOS mode set to {}", static_cast(request->mode)); response->success = true; @@ -334,110 +214,27 @@ vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( types::Outputs outputs) { vortex_msgs::msg::LOSGuidance reference_msg; - double max_pitch_angle_copy; - double current_yaw_copy; - double u_desired_copy; - - { - std::unique_lock lock(mutex_); - max_pitch_angle_copy = max_pitch_angle_; - current_yaw_copy = current_yaw_; - u_desired_copy = u_desired_; - } + double max_pitch_angle = state_manager_->get_max_pitch_angle(); + double current_yaw = state_manager_->get_current_yaw(); + double u_desired = state_manager_->get_u_desired(); - const double clamped_pitch = std::clamp( - outputs.theta_d, -max_pitch_angle_copy, max_pitch_angle_copy); + const double clamped_pitch = + std::clamp(outputs.theta_d, -max_pitch_angle, max_pitch_angle); reference_msg.pitch = clamped_pitch; reference_msg.yaw = outputs.psi_d; - double yaw_error = - vortex::utils::math::ssa(outputs.psi_d - current_yaw_copy); + double yaw_error = vortex::utils::math::ssa(outputs.psi_d - current_yaw); double abs_err = std::abs(yaw_error); - double u_cmd = u_desired_copy / (1.0 + 0.5 * abs_err); - u_cmd = std::clamp(u_cmd, 0.15, u_desired_copy); + double u_cmd = u_desired / (1.0 + 0.5 * abs_err); + u_cmd = std::clamp(u_cmd, 0.15, u_desired); reference_msg.surge = u_cmd; return reference_msg; } -// Check if goal is feasible -bool LosGuidanceNode::is_goal_feasible( - const types::Inputs& inputs, - std::shared_ptr goal) { - const auto& current_position = inputs.current_position; - const auto& goal_point = goal->waypoint.pose.position; - - const double dx = goal_point.x - current_position.x; - const double dy = goal_point.y - current_position.y; - const double dz = goal_point.z - current_position.z; - - const double horizontal_distance = std::sqrt(dx * dx + dy * dy); - const double required_pitch = std::atan2(-dz, horizontal_distance); - - return std::abs(required_pitch) <= max_pitch_angle_; -} - -// Check if goal is missed -bool LosGuidanceNode::is_goal_missed(const types::Inputs& inputs) { - const double distance_to_goal = - (inputs.current_position - inputs.next_point).as_vector().norm(); - - const double dt = static_cast(time_step_.count()) / 1000.0; - - if (distance_to_goal < nearest_been_to_goal_) { - nearest_been_to_goal_ = distance_to_goal; - time_since_nearest_goal_ = 0.0; - return false; - } - - if (distance_to_goal > - nearest_been_to_goal_ + missed_goal_distance_margin_) { - time_since_nearest_goal_ += dt; - } else { - time_since_nearest_goal_ = 0.0; - } - - return time_since_nearest_goal_ >= missed_goal_timeout_; -} - -// Load LOS config -YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { - try { - YAML::Node config = YAML::LoadFile(yaml_file_path); - return config; - } catch (const YAML::Exception& e) { - throw std::runtime_error( - std::string("Failed to load LOS config file '") + yaml_file_path + - "': " + e.what()); - } -} - -// Parse common config -void LosGuidanceNode::parse_common_config(YAML::Node common_config) { - try { - std::unique_lock lock(mutex_); - - u_desired_ = common_config["u_desired"].as(); - max_pitch_angle_ = common_config["max_pitch_angle"].as(); - goal_reached_tol_ = common_config["goal_reached_tol"].as(); - missed_goal_timeout_ = - common_config["missed_goal_timeout"].as(); - missed_goal_distance_margin_ = - common_config["missed_goal_distance_margin"].as(); - - method_ = static_cast( - common_config["active_los_method"].as()); - - lock.unlock(); - } catch (const YAML::Exception& e) { - throw std::runtime_error( - std::string("Failed to load common parameters: ") + e.what()); - } -} - // Execute action void LosGuidanceNode::execute( const std::shared_ptr goal_handle) { @@ -454,27 +251,11 @@ void LosGuidanceNode::execute( const auto new_wp = types::Point::point_from_ros(los_waypoint); - { - std::unique_lock lock(mutex_); - if (!has_active_segment_) { - path_inputs_.prev_point = path_inputs_.current_position; - path_inputs_.next_point = new_wp; - has_active_segment_ = true; - } else { - path_inputs_.prev_point = path_inputs_.next_point; - path_inputs_.next_point = new_wp; - } - lock.unlock(); - } - - adaptive_los_->reset(); + state_manager_->initialize_goal(new_wp); auto result = std::make_shared(); - nearest_been_to_goal_ = std::numeric_limits::infinity(); - time_since_nearest_goal_ = 0.0; - rclcpp::Rate loop_rate(1000.0 / time_step_.count()); while (rclcpp::ok()) { @@ -495,56 +276,30 @@ void LosGuidanceNode::execute( return; } - types::Inputs inputs_copy; - types::ActiveLosMethod method_copy; nav_msgs::msg::Odometry::SharedPtr odom_copy; double goal_reached_tol_copy; { std::unique_lock lock(mutex_); - inputs_copy = path_inputs_; - method_copy = method_; odom_copy = debug_current_odom_; goal_reached_tol_copy = goal_handle->get_goal()->convergence_threshold; lock.unlock(); } - if (is_goal_missed(inputs_copy)) { + if (state_manager_->is_goal_missed()) { result->success = false; goal_handle->abort(result); spdlog::info("Aborting goal: waypoint missed"); return; } - types::Outputs outputs; - - switch (method_copy) { - case types::ActiveLosMethod::ADAPTIVE: - outputs = adaptive_los_->calculate_outputs(inputs_copy); - break; - case types::ActiveLosMethod::PROPORTIONAL: - outputs = proportional_los_->calculate_outputs(inputs_copy); - break; - case types::ActiveLosMethod::INTEGRAL: - outputs = integral_los_->calculate_outputs(inputs_copy); - break; - case types::ActiveLosMethod::VECTOR_FIELD: - outputs = vector_field_los_->calculate_outputs(inputs_copy); - break; - default: - spdlog::error("Invalid LOS method selected"); - result->success = false; - goal_handle->abort(result); - return; - } + types::Outputs outputs = state_manager_->calculate_outputs(); auto reference_msg = std::make_unique( fill_los_reference(outputs)); - if ((inputs_copy.current_position - inputs_copy.next_point) - .as_vector() - .norm() < goal_reached_tol_copy) { + if (state_manager_->is_goal_reached(goal_reached_tol_copy)) { reference_msg->pitch = 0.0; reference_msg->surge = 0.0; From 5562c69a029a7428dc520f3ce83a55900e5d2552 Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 7 May 2026 18:13:46 +0200 Subject: [PATCH 86/87] Changes from PR commnets --- guidance/los_guidance/CMakeLists.txt | 1 + .../los_guidance/config/guidance_params.yaml | 1 + .../los_guidance/lib/guidance_manager.hpp | 13 +-- .../include/los_guidance/lib/types.hpp | 42 ++++++++ .../include/los_guidance/lib/utils.hpp | 23 +++++ .../los_guidance/ros/los_guidance_ros.hpp | 3 - .../los_guidance/src/lib/guidance_manager.cpp | 46 +++++---- guidance/los_guidance/src/lib/utils.cpp | 17 ++++ .../los_guidance/src/ros/los_guidance_ros.cpp | 95 +++++++++---------- 9 files changed, 156 insertions(+), 85 deletions(-) create mode 100644 guidance/los_guidance/include/los_guidance/lib/utils.hpp create mode 100644 guidance/los_guidance/src/lib/utils.cpp diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index f4cc6c115..ca1328f99 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(${LIB_NAME} SHARED src/lib/vector_field_los.cpp src/lib/guidance_manager.cpp src/ros/los_guidance_ros.cpp + src/lib/utils.cpp ) ament_target_dependencies(${LIB_NAME} diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index 698a5562b..a9e949f3a 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -46,6 +46,7 @@ vector_field_los: proportional_gain_v: 0.9 common: + time_step_s: 0.1 active_los_method: 2 u_desired: 0.2 goal_reached_tol: 0.20 diff --git a/guidance/los_guidance/include/los_guidance/lib/guidance_manager.hpp b/guidance/los_guidance/include/los_guidance/lib/guidance_manager.hpp index 59259ff13..361274211 100644 --- a/guidance/los_guidance/include/los_guidance/lib/guidance_manager.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/guidance_manager.hpp @@ -59,18 +59,11 @@ class LosGuidanceStateManager { */ void set_vector_field_guidance(YAML::Node config); - /** - * @brief Loads the LOS guidance YAML configuration file. - * @param yaml_file_path Path to the YAML configuration file. - * @return YAML::Node Parsed YAML configuration. - */ - YAML::Node get_los_config(std::string yaml_file_path); - /** * @brief Parses common guidance parameters shared by all LOS methods. * @param common_config YAML node containing common guidance parameters. */ - void parse_common_config(YAML::Node common_config); + void parse_common_config(const YAML::Node& common_config); /** * @brief Checks if the given LOS guidance goal is feasible based on the @@ -159,9 +152,9 @@ class LosGuidanceStateManager { double time_step_s_{}; types::ActiveLosMethod method_{}; double nearest_been_to_goal_{std::numeric_limits::max()}; - double time_since_nearest_goal_{}; + double time_since_nearest_goal_s_{}; double missed_goal_distance_margin_{}; - double missed_goal_timeout_{}; + double missed_goal_timeout_s_{}; bool has_active_segment_{false}; std::unique_ptr adaptive_los_{}; diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp index 76f8fa296..3f6af11f3 100644 --- a/guidance/los_guidance/include/los_guidance/lib/types.hpp +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -10,6 +10,7 @@ #include #include #include +#include namespace vortex::guidance::los::types { @@ -92,6 +93,47 @@ struct Inputs { */ enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; +/** + * @brief Converts a string to its corresponding ActiveLosMethod enum value. + * @param str String representation of the LOS method (case-insensitive variants + * accepted, e.g. "ADAPTIVE" or "adaptive"). + * @return ActiveLosMethod The corresponding enum value. + * @throws std::runtime_error if the string does not match a known LOS method. + */ +inline ActiveLosMethod string_to_active_los_method(const std::string& str) { + if (str == "ADAPTIVE" || str == "adaptive") + return ActiveLosMethod::ADAPTIVE; + if (str == "PROPORTIONAL" || str == "proportional") + return ActiveLosMethod::PROPORTIONAL; + if (str == "INTEGRAL" || str == "integral") + return ActiveLosMethod::INTEGRAL; + if (str == "VECTOR_FIELD" || str == "vector_field") + return ActiveLosMethod::VECTOR_FIELD; + throw std::runtime_error("Unknown ActiveLosMethod string: '" + str + "'"); +} + +/** + * @brief Converts an integer to its corresponding ActiveLosMethod enum value. + * @param value Integer representation of the LOS method. + * @return ActiveLosMethod The corresponding enum value. + * @throws std::runtime_error if the integer does not match a known LOS method. + */ +inline ActiveLosMethod int_to_active_los_method(int value) { + switch (value) { + case static_cast(ActiveLosMethod::ADAPTIVE): + return ActiveLosMethod::ADAPTIVE; + case static_cast(ActiveLosMethod::PROPORTIONAL): + return ActiveLosMethod::PROPORTIONAL; + case static_cast(ActiveLosMethod::INTEGRAL): + return ActiveLosMethod::INTEGRAL; + case static_cast(ActiveLosMethod::VECTOR_FIELD): + return ActiveLosMethod::VECTOR_FIELD; + default: + throw std::runtime_error("Unknown ActiveLosMethod numeric value: " + + std::to_string(value)); + } +} + } // namespace vortex::guidance::los::types #endif // LOS_GUIDANCE__LIB__TYPES_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/utils.hpp b/guidance/los_guidance/include/los_guidance/lib/utils.hpp new file mode 100644 index 000000000..0d25263e6 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/utils.hpp @@ -0,0 +1,23 @@ +/** + * @file yaml_utils.hpp + * @brief Utility functions for loading YAML configuration files. + */ +#ifndef LOS_GUIDANCE__LIB__UTILS_HPP_ +#define LOS_GUIDANCE__LIB__UTILS_HPP_ + +#include +#include + +namespace vortex::guidance::los::utils { + +/** + * @brief Loads a YAML configuration file from the given path. + * @param yaml_file_path Path to the YAML configuration file. + * @return YAML::Node Parsed YAML configuration. + * @throws std::runtime_error if the file cannot be loaded or parsed. + */ +YAML::Node load_yaml_config(const std::string& yaml_file_path); + +} // namespace vortex::guidance::los::utils + +#endif // LOS_GUIDANCE__LIB__UTILS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/ros/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/ros/los_guidance_ros.hpp index 82f6eada7..54fbcd89c 100644 --- a/guidance/los_guidance/include/los_guidance/ros/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/ros/los_guidance_ros.hpp @@ -155,9 +155,6 @@ class LosGuidanceNode : public rclcpp::Node { rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr - message_pub_sub_; - rclcpp::CallbackGroup::SharedPtr cb_group_; // State manager std::unique_ptr state_manager_; diff --git a/guidance/los_guidance/src/lib/guidance_manager.cpp b/guidance/los_guidance/src/lib/guidance_manager.cpp index 8459f352e..91219a309 100644 --- a/guidance/los_guidance/src/lib/guidance_manager.cpp +++ b/guidance/los_guidance/src/lib/guidance_manager.cpp @@ -3,15 +3,14 @@ #include #include #include +#include "los_guidance/lib/utils.hpp" namespace vortex::guidance::los { // Constructor LosGuidanceStateManager::LosGuidanceStateManager( const std::string& yaml_file_path) { - time_step_s_ = 0.1; - - YAML::Node config = get_los_config(yaml_file_path); + YAML::Node config = utils::load_yaml_config(yaml_file_path); parse_common_config(config["common"]); set_adaptive_los_guidance(config); @@ -109,33 +108,32 @@ void LosGuidanceStateManager::set_vector_field_guidance(YAML::Node config) { } } -// Load LOS config -YAML::Node LosGuidanceStateManager::get_los_config(std::string yaml_file_path) { - try { - YAML::Node config = YAML::LoadFile(yaml_file_path); - return config; - } catch (const YAML::Exception& e) { - throw std::runtime_error( - std::string("Failed to load LOS config file '") + yaml_file_path + - "': " + e.what()); - } -} - // Parse common config -void LosGuidanceStateManager::parse_common_config(YAML::Node common_config) { +void LosGuidanceStateManager::parse_common_config( + const YAML::Node& common_config) { try { std::unique_lock lock(mutex_); u_desired_ = common_config["u_desired"].as(); max_pitch_angle_ = common_config["max_pitch_angle"].as(); goal_reached_tol_ = common_config["goal_reached_tol"].as(); - missed_goal_timeout_ = + missed_goal_timeout_s_ = common_config["missed_goal_timeout"].as(); missed_goal_distance_margin_ = common_config["missed_goal_distance_margin"].as(); - method_ = static_cast( - common_config["active_los_method"].as()); + const auto m = common_config["active_los_method"]; + if (!m) { + throw std::runtime_error( + "Missing required field 'active_los_method'"); + } + try { + method_ = types::int_to_active_los_method(m.as()); + } catch (const YAML::BadConversion&) { + method_ = types::string_to_active_los_method(m.as()); + } + + time_step_s_ = common_config["time_step_s"].as(); lock.unlock(); } catch (const YAML::Exception& e) { @@ -199,7 +197,7 @@ void LosGuidanceStateManager::initialize_goal(const types::Point& new_wp) { adaptive_los_->reset(); nearest_been_to_goal_ = std::numeric_limits::infinity(); - time_since_nearest_goal_ = 0.0; + time_since_nearest_goal_s_ = 0.0; } // Check if goal is feasible @@ -236,18 +234,18 @@ bool LosGuidanceStateManager::is_goal_missed() { if (distance_to_goal < nearest_been_to_goal_) { nearest_been_to_goal_ = distance_to_goal; - time_since_nearest_goal_ = 0.0; + time_since_nearest_goal_s_ = 0.0; return false; } if (distance_to_goal > nearest_been_to_goal_ + missed_goal_distance_margin_) { - time_since_nearest_goal_ += time_step_s_; + time_since_nearest_goal_s_ += time_step_s_; } else { - time_since_nearest_goal_ = 0.0; + time_since_nearest_goal_s_ = 0.0; } - return time_since_nearest_goal_ >= missed_goal_timeout_; + return time_since_nearest_goal_s_ >= missed_goal_timeout_s_; } // Check if goal is reached diff --git a/guidance/los_guidance/src/lib/utils.cpp b/guidance/los_guidance/src/lib/utils.cpp new file mode 100644 index 000000000..809fbf765 --- /dev/null +++ b/guidance/los_guidance/src/lib/utils.cpp @@ -0,0 +1,17 @@ +#include "los_guidance/lib/utils.hpp" +#include + +namespace vortex::guidance::los::utils { + +YAML::Node load_yaml_config(const std::string& yaml_file_path) { + try { + YAML::Node config = YAML::LoadFile(yaml_file_path); + return config; + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load LOS config file '") + yaml_file_path + + "': " + e.what()); + } +} + +} // namespace vortex::guidance::los::utils diff --git a/guidance/los_guidance/src/ros/los_guidance_ros.cpp b/guidance/los_guidance/src/ros/los_guidance_ros.cpp index 3ba7a60a6..e2b4622c5 100644 --- a/guidance/los_guidance/src/ros/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/ros/los_guidance_ros.cpp @@ -45,51 +45,54 @@ LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) // Subscribers + publishers void LosGuidanceNode::set_subscribers_and_publisher() { - this->declare_parameter("topics.pose"); - this->declare_parameter("topics.guidance.los"); - this->declare_parameter("topics.waypoint"); - this->declare_parameter("topics.odom"); - this->declare_parameter( + const std::string pose_topic = + this->declare_parameter("topics.pose"); + const std::string guidance_topic = + this->declare_parameter("topics.guidance.los"); + const std::string waypoint_topic = + this->declare_parameter("topics.waypoint"); + const std::string odom_topic = + this->declare_parameter("topics.odom"); + const std::string odom_tf_rpy_topic = this->declare_parameter( "topics.odom_tf_rpy", "/utils/message_publisher/odom_tf_rpy"); - std::string pose_topic = this->get_parameter("topics.pose").as_string(); - std::string guidance_topic = - this->get_parameter("topics.guidance.los").as_string(); - std::string waypoint_topic = - this->get_parameter("topics.waypoint").as_string(); - std::string odom_topic = this->get_parameter("topics.odom").as_string(); - std::string odom_tf_rpy_topic = - this->get_parameter("topics.odom_tf_rpy").as_string(); - auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); reference_pub_ = this->create_publisher( guidance_topic, qos_sensor_data); - state_debug_pub_ = this->create_publisher( - "state_debug", qos_sensor_data); - waypoint_sub_ = this->create_subscription( waypoint_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::waypoint_callback, this, - std::placeholders::_1)); + [this](const geometry_msgs::msg::PointStamped::SharedPtr msg) { + const auto new_wp = types::Point::point_from_ros(msg->point); + state_manager_->update_waypoint(new_wp); + spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, + new_wp.z); + }); pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( pose_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::pose_callback, this, - std::placeholders::_1)); + [this](const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + msg) { + types::Point position = + types::Point::point_from_ros(msg->pose.pose.position); + state_manager_->update_position(position); + }); odom_sub_ = this->create_subscription( odom_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::odom_callback, this, - std::placeholders::_1)); - - message_pub_sub_ = - this->create_subscription( - odom_tf_rpy_topic, qos_sensor_data, - std::bind(&LosGuidanceNode::odom_msg_callback, this, - std::placeholders::_1)); + [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + const Eigen::Vector3d euler = + vortex::utils::math::quat_to_euler(Eigen::Quaterniond( + msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z)); + state_manager_->update_yaw(euler.z()); + + std::lock_guard lock(mutex_); + debug_current_odom_ = msg; + }); } // Action server setup @@ -98,19 +101,14 @@ void LosGuidanceNode::set_action_server() { std::string action_server_name = this->get_parameter("action_servers.los").as_string(); - cb_group_ = - this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - action_server_ = rclcpp_action::create_server( this, action_server_name, - std::bind(&LosGuidanceNode::handle_goal, this, - std::placeholders::_1, std::placeholders::_2), - std::bind(&LosGuidanceNode::handle_cancel, this, - std::placeholders::_1), - std::bind(&LosGuidanceNode::handle_accepted, this, - std::placeholders::_1), - rcl_action_server_get_default_options(), cb_group_); + [this](const auto& uuid, auto goal) { + return handle_goal(uuid, std::move(goal)); + }, + [this](auto goal_handle) { return handle_cancel(goal_handle); }, + [this](auto goal_handle) { handle_accepted(goal_handle); }); } // Service server setup @@ -120,8 +118,13 @@ void LosGuidanceNode::set_service_server() { this->get_parameter("services.los_mode").as_string(); los_mode_service_ = this->create_service( - service_name, std::bind(&LosGuidanceNode::set_los_mode, this, - std::placeholders::_1, std::placeholders::_2)); + service_name, + [this]( + const std::shared_ptr + request, + std::shared_ptr response) { + set_los_mode(request, response); + }); } // Waypoint callback @@ -153,12 +156,6 @@ void LosGuidanceNode::odom_callback( lock.unlock(); } -// Euler (yaw) callback -void LosGuidanceNode::odom_msg_callback( - const vortex_msgs::msg::PoseEulerStamped::SharedPtr msg) { - state_manager_->update_yaw(msg->yaw); -} - // Goal handler rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, @@ -311,6 +308,7 @@ void LosGuidanceNode::execute( reference_pub_->publish(std::move(reference_msg)); + /* if (debug && odom_copy) { const auto& v = odom_copy->twist.twist.linear; double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); @@ -327,7 +325,8 @@ void LosGuidanceNode::execute( state_debug_msg.surge = surge; state_debug_pub_->publish(state_debug_msg); - } + + }*/ loop_rate.sleep(); } From be7d537819f9e32f84492b37d8d76320338892b6 Mon Sep 17 00:00:00 2001 From: Anbit Date: Thu, 7 May 2026 18:15:15 +0200 Subject: [PATCH 87/87] Comment update --- guidance/los_guidance/src/ros/los_guidance_ros.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/guidance/los_guidance/src/ros/los_guidance_ros.cpp b/guidance/los_guidance/src/ros/los_guidance_ros.cpp index e2b4622c5..806283e55 100644 --- a/guidance/los_guidance/src/ros/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/ros/los_guidance_ros.cpp @@ -308,10 +308,10 @@ void LosGuidanceNode::execute( reference_pub_->publish(std::move(reference_msg)); - /* - if (debug && odom_copy) { - const auto& v = odom_copy->twist.twist.linear; - double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); + /* will shortly be moved to Vortex_utils message publisher node for + general debugging purposes if (debug && odom_copy) { const auto& v = + odom_copy->twist.twist.linear; double surge = std::sqrt(v.x * v.x + v.y + * v.y + v.z * v.z); vortex_msgs::msg::LOSGuidance state_debug_msg; Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(