Anbit/guidance

.gitignore

@@ -9,7 +9,6 @@ install/
 log/
 build/
 bin/
-lib/
 msg_gen/
 srv_gen/
 msg/*Action.msg

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

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

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"

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 <cmath>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#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

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 <cmath>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#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

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 <cmath>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#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

guidance/los_guidance/include/los_guidance/lib/types.hpp

@@ -0,0 +1,58 @@
+#ifndef TYPES_HPP
+#define TYPES_HPP
+
+#include <cmath>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+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

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 <cmath>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry> 
+#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