diff --git a/.gitignore b/.gitignore index 9af41aca7..52ae0a3c0 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,6 @@ install/ log/ build/ bin/ -lib/ msg_gen/ srv_gen/ msg/*Action.msg 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/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 90b51de3a..1dbdbe0d9 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -15,22 +15,29 @@ 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) +find_package(yaml-cpp REQUIRED) + include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED - src/los_guidance.cpp + 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 ) ament_target_dependencies(${LIB_NAME} rclcpp rclcpp_action geometry_msgs + nav_msgs vortex_msgs vortex_utils_ros Eigen3 @@ -42,7 +49,18 @@ add_executable(los_guidance_node src/los_guidance_node.cpp ) -target_link_libraries(los_guidance_node ${LIB_NAME}) +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 +) install(TARGETS ${LIB_NAME} @@ -59,7 +77,7 @@ install(TARGETS install( DIRECTORY include/ - DESTINATION include + DESTINATION include/ ) install(DIRECTORY diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index a580cf57c..5d5d0b754 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,10 +1,34 @@ -/**: - 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 +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 + +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: + active_los_method: 0 # 0: Proportional, 1: Integral, 2: Adaptive + u_desired: 0.3 + goal_reached_tol: 1.0 + +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 new file mode 100644 index 000000000..f874db35f --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -0,0 +1,49 @@ +#ifndef ADAPTIVE_LOS_GUIDANCE_HPP +#define ADAPTIVE_LOS_GUIDANCE_HPP + +#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". + */ + +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 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 new file mode 100644 index 000000000..4af1b60ac --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -0,0 +1,45 @@ +#ifndef INTEGRAL_LOS_GUIDANCE_HPP +#define INTEGRAL_LOS_GUIDANCE_HPP + +#include +#include +#include +#include "los_guidance/lib/types.hpp" + +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()}; +}; +} // 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 new file mode 100644 index 000000000..2a5ea00a1 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -0,0 +1,40 @@ +#ifndef PROPORTIONAL_LOS_GUIDANCE_HPP +#define PROPORTIONAL_LOS_GUIDANCE_HPP + +#include +#include +#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()}; +}; + +} // 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 new file mode 100644 index 000000000..ca2492d10 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -0,0 +1,58 @@ +#ifndef TYPES_HPP +#define TYPES_HPP + +#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}; + } + + 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{}; + + 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 Inputs { + Point prev_point{}; + Point next_point{}; + Point current_position{}; +}; + +enum class ActiveLosMethod { + PROPORTIONAL, // 0 + INTEGRAL, // 1 + ADAPTIVE, // 2 + VECTOR_FIELD // 3 +}; + +} // namespace vortex::guidance::los::types + +#endif 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..9d3210ffd --- /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/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 623dac5da..5f79d75b5 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,33 +1,54 @@ #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 "los_guidance.hpp" - -namespace vortex::guidance { - -class LOSGuidanceNode : public rclcpp::Node { +#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" +#include "los_guidance/lib/types.hpp" +#include +#include +#include + +namespace vortex::guidance::los { + +class LosGuidanceNode : public rclcpp::Node { public: - LOSGuidanceNode(); + LosGuidanceNode(); 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(); + // @brief Determine the LOS mode service + void set_service_server(); + // @brief Set the adaptive LOS guidance parameters - void set_adaptive_los_guidance(); + 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); // @brief Callback for the waypoint topic // @param msg The reference message @@ -39,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 @@ -65,21 +89,37 @@ class LOSGuidanceNode : public rclcpp::Node { void execute(const std::shared_ptr> goal_handle); - // @brief Fill the lost waypoints - // @param goal The goal message - void fill_los_waypoints( - const geometry_msgs::msg::PointStamped& los_waypoint); + 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); - vortex_msgs::msg::LOSGuidance fill_los_reference(); + 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); rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Service::SharedPtr los_mode_service_; + rclcpp::Publisher::SharedPtr reference_pub_; + bool enable_debug_; + + std::string debug_topic_name_; + + rclcpp::Publisher::SharedPtr debug_pub_; + rclcpp::Subscription::SharedPtr waypoint_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; @@ -97,23 +137,19 @@ class LOSGuidanceNode : public rclcpp::Node { rclcpp::CallbackGroup::SharedPtr cb_group_; - LOS::Point eta_; - - LOS::Point last_point_; - - LOS::Point next_point_; - - std::unique_ptr adaptive_los_guidance_; - - double yaw_d_{}; - - double pitch_d_{}; + types::Inputs path_inputs_{}; double u_desired_{}; double goal_reached_tol_{}; -}; -} // namespace vortex::guidance + std::unique_ptr adaptive_los_{}; + std::unique_ptr integral_los_{}; + std::unique_ptr proportional_los_{}; + std::unique_ptr vector_field_los_{}; + types::ActiveLosMethod method_{}; +}; +} // namespace vortex::guidance::los + #endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ 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/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index 03366e897..859c0a255 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,8 +25,13 @@ def generate_launch_description(): namespace="orca", parameters=[ orca_params, - adapt_params, + {"los_config_file": los_params}, + {"time_step": 0.01}, ], output="screen", ) 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/package.xml b/guidance/los_guidance/package.xml index 5ff6218fe..5f696a725 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -13,9 +13,10 @@ rclcpp_action geometry_msgs vortex_msgs + nav_msgs vortex_utils_ros eigen - + yaml-cpp ament_cmake 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..f35d9a053 --- /dev/null +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -0,0 +1,66 @@ +#include +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& 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; + + 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()); +} + +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 Eigen::Vector3d cross_track_error = + rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + + 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 * + 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; + 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; +} + +types::Outputs AdaptiveLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(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); + 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}; +} + + + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp new file mode 100644 index 000000000..b1a4e3962 --- /dev/null +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -0,0 +1,47 @@ +#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 path_frame_error = rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * + diff_vec; + + return types::CrossTrackError::from_vector(path_frame_error); +} + +types::Outputs IntegralLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(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 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 diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp new file mode 100644 index 000000000..0bb65ba01 --- /dev/null +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -0,0 +1,54 @@ +#include + +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) { + 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 path_frame_error = rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * + diff_vec; + + return types::CrossTrackError::from_vector(path_frame_error); +} + +types::Outputs ProportionalLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(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; + + 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}; +} + +} // 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 new file mode 100644 index 000000000..52b4f1644 --- /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 diff --git a/guidance/los_guidance/src/los_guidance.cpp b/guidance/los_guidance/src/los_guidance.cpp deleted file mode 100644 index 3171d5442..000000000 --- a/guidance/los_guidance/src/los_guidance.cpp +++ /dev/null @@ -1,59 +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; -} - -} // namespace vortex::guidance 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 0b63f50c9..320eeea7d 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,40 +1,58 @@ #include "los_guidance/los_guidance_ros.hpp" +#include #include +#include #include +#include "los_guidance/lib/types.hpp" const auto start_message = R"( _ ___ ____ ____ _ _ | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ | | | | | \___ \ | | _| | | | |/ _` |/ _` | '_ \ / __/ _ \ | |__| |_| |___) | | |_| | |_| | | (_| | (_| | | | | (_| __/ - |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| + |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| )"; -namespace vortex::guidance { +namespace vortex::guidance::los { -LOSGuidanceNode::LOSGuidanceNode() : Node("los_guidance_node") { - time_step_ = std::chrono::milliseconds(10); +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"); - set_subscribers_and_publisher(); + const std::string yaml_path = + this->declare_parameter("los_config_file"); - set_action_server(); + YAML::Node config = get_los_config(yaml_path); - set_adaptive_los_guidance(); + parse_common_config(config["common"]); + set_subscribers_and_publisher(config); + 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); } -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("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(); + + std::string odom_topic = + this->get_parameter("nav_topics.odometry").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); @@ -43,17 +61,31 @@ void LOSGuidanceNode::set_subscribers_and_publisher() { waypoint_sub_ = this->create_subscription( waypoint_topic, qos_sensor_data, - std::bind(&LOSGuidanceNode::waypoint_callback, this, + 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::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_topic_name_, qos_sensor_data); + } } -void LOSGuidanceNode::set_action_server() { +void LosGuidanceNode::set_action_server() { this->declare_parameter("action_servers.los"); std::string action_server_name = this->get_parameter("action_servers.los").as_string(); @@ -63,52 +95,115 @@ void LOSGuidanceNode::set_action_server() { action_server_ = rclcpp_action::create_server( this, action_server_name, - std::bind(&LOSGuidanceNode::handle_goal, this, + std::bind(&LosGuidanceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&LOSGuidanceNode::handle_cancel, this, + std::bind(&LosGuidanceNode::handle_cancel, this, std::placeholders::_1), - std::bind(&LOSGuidanceNode::handle_accepted, this, + 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"); +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::Params params; + 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 params = AdaptiveLosParams{}; params.lookahead_distance_h = - this->get_parameter("los.lookahead_distance_h").as_double(); + adaptive_los_config["lookahead_distance_h"].as(); 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_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; + + adaptive_los_ = std::make_unique(params); +} - adaptive_los_guidance_ = std::make_unique(params); +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(); + 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::waypoint_callback( +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 = + 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; + + vector_field_los_ = 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"); + 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( +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; + 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( +rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { (void)goal; @@ -125,7 +220,7 @@ rclcpp_action::GoalResponse LOSGuidanceNode::handle_goal( return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -rclcpp_action::CancelResponse LOSGuidanceNode::handle_cancel( +rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { @@ -134,34 +229,83 @@ rclcpp_action::CancelResponse LOSGuidanceNode::handle_cancel( return rclcpp_action::CancelResponse::ACCEPT; } -void LOSGuidanceNode::handle_accepted( +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; +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_)); + response->success = true; } +vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( + types::Outputs outputs) { -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.pitch = outputs.theta_d; + reference_msg.yaw = outputs.psi_d; reference_msg.surge = u_desired_; return reference_msg; } -void LOSGuidanceNode::execute( +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; +} + +void LosGuidanceNode::parse_common_config(YAML::Node common_config) { + std::lock_guard lock(mutex_); + u_desired_ = common_config["u_desired"].as(); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); + method_ = static_cast( + 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) { @@ -170,17 +314,13 @@ void LOSGuidanceNode::execute( this->goal_handle_ = goal_handle; } - u_desired_ = this->get_parameter("los.u_desired").as_double(); - goal_reached_tol_ = this->get_parameter("los.goal_reached_tol").as_double(); - spdlog::info("Executing goal"); const geometry_msgs::msg::PointStamped los_waypoint = goal_handle->get_goal()->goal; - fill_los_waypoints(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); auto feedback = std::make_shared(); @@ -197,6 +337,7 @@ void LOSGuidanceNode::execute( return; } } + if (goal_handle->is_canceling()) { result->success = false; goal_handle->canceled(result); @@ -204,28 +345,40 @@ void LOSGuidanceNode::execute( return; } - LOS::CrossTrackError errors = - adaptive_los_guidance_->calculate_crosstrack_error(last_point_, - eta_); - - yaw_d_ = adaptive_los_guidance_->calculate_psi_d(errors.y_e); - pitch_d_ = adaptive_los_guidance_->calculate_theta_d(errors.z_e); - - adaptive_los_guidance_->update_adaptive_estimates(errors); - - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); + types::Outputs outputs; + + switch (method_) { + case types::ActiveLosMethod::ADAPTIVE: + outputs = adaptive_los_->calculate_outputs(path_inputs_); + break; + case types::ActiveLosMethod::PROPORTIONAL: + outputs = proportional_los_->calculate_outputs(path_inputs_); + break; + 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; + 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); - 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; } @@ -234,4 +387,4 @@ void LOSGuidanceNode::execute( } } -} // namespace vortex::guidance +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index 69c0dfa52..c0cdc1ae7 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -6,13 +6,21 @@ include(GoogleTest) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) add_executable( ${TEST_BINARY_NAME} - test_los_guidance.cpp + test_main.cpp + adaptive_los_test.cpp + proportional_los_test.cpp + integral_los_test.cpp + vector_field_los_test.cpp + ) -target_link_libraries( - ${TEST_BINARY_NAME} +target_link_libraries(${TEST_BINARY_NAME} PRIVATE ${LIB_NAME} + yaml-cpp + Eigen3::Eigen + spdlog::spdlog + fmt::fmt GTest::GTest ) 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..5fd71027a --- /dev/null +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -0,0 +1,173 @@ +#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}; + + 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 new file mode 100644 index 000000000..f94e93df1 --- /dev/null +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -0,0 +1,110 @@ +#include "los_guidance/lib/integral_los.hpp" +#include +#include "los_guidance/lib/adaptive_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 diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp new file mode 100644 index 000000000..c704dec68 --- /dev/null +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -0,0 +1,107 @@ +#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}; + + 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_los_guidance.cpp b/guidance/los_guidance/test/test_los_guidance.cpp deleted file mode 100644 index 78b3d4fe1..000000000 --- a/guidance/los_guidance/test/test_los_guidance.cpp +++ /dev/null @@ -1,182 +0,0 @@ -#include - -#include "los_guidance/los_guidance.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(); -} diff --git a/guidance/los_guidance/test/test_main.cpp b/guidance/los_guidance/test/test_main.cpp new file mode 100644 index 000000000..af89a87e6 --- /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(); +} 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