altitude logic

vortex_utils/include/vortex/utils/types.hpp

@@ -448,8 +448,9 @@ enum class WaypointMode : uint8_t {
     FORWARD_HEADING = 2,   ///< Control x, y, z with yaw toward target.
     ONLY_ORIENTATION = 3,  ///< Control roll, pitch, yaw; hold current position.
     POSITION_AND_YAW = 4,  ///< Control x, y, z and yaw; force roll=pitch=0.
-    XY_AND_YAW = 5,        ///< Control x, y and yaw; hold z, force roll=pitch=0.
-    XY_FORWARD_DIR = 6,    ///< Control x, y; hold z; yaw auto-computed toward target.
+    XY_AND_YAW = 5,  ///< Control x, y and yaw; hold z, force roll=pitch=0.
+    XY_FORWARD_DIR =
+        6,  ///< Control x, y; hold z; yaw auto-computed toward target.
 };
 
 /**
@@ -458,6 +459,8 @@ enum class WaypointMode : uint8_t {
 struct Waypoint {
     Pose pose{};
     WaypointMode mode = WaypointMode::FULL_POSE;
+    bool keep_altitude{false};
+    double desired_altitude{0.0};
 };
 
 struct SonarInfo {

vortex_utils/include/vortex/utils/waypoint_utils.hpp

@@ -17,6 +17,8 @@ struct WaypointGoal {
     Pose pose;
     WaypointMode mode;
     double convergence_threshold{0.1};
+    bool keep_altitude{false};
+    double desired_altitude{0.0};
 };
 
 /**
@@ -123,6 +125,8 @@ Pose load_pose_from_yaml(const std::string& file_path,
  * ONLY_POSITION, FORWARD_HEADING x: 1.0 y: 0.0 z: -0.5 orientation:         #
  * Required for FULL_POSE, ONLY_ORIENTATION roll: 0.0 pitch: 0.0 yaw: 3.14159
  *   convergence_threshold: 0.1  # Optional, default is 0.1
+ *   keep_altitude: true          # Optional, default false
+ *   desired_altitude: 1.5        # Required when keep_altitude is true
  * @endcode
  *
  * @param file_path Path to the YAML file.

vortex_utils/src/waypoint_utils.cpp

@@ -269,9 +269,25 @@ WaypointGoal load_waypoint_goal_from_yaml(const std::string& file_path,
         convergence_threshold = wp["convergence_threshold"].as<double>();
     }
 
+    bool keep_altitude = false;
+    double desired_altitude = 0.0;
+    if (wp["keep_altitude"]) {
+        keep_altitude = wp["keep_altitude"].as<bool>();
+    }
+    if (keep_altitude) {
+        if (!wp["desired_altitude"]) {
+            throw std::runtime_error("Waypoint '" + identifier +
+                                     "' has keep_altitude=true but is missing "
+                                     "required field 'desired_altitude'");
+        }
+        desired_altitude = wp["desired_altitude"].as<double>();
+    }
+
     return WaypointGoal{.pose = pose,
                         .mode = mode,
-                        .convergence_threshold = convergence_threshold};
+                        .convergence_threshold = convergence_threshold,
+                        .keep_altitude = keep_altitude,
+                        .desired_altitude = desired_altitude};
 }
 
 LandmarkConvergenceGoal load_landmark_goal_from_yaml(

vortex_utils_ros/include/vortex/utils/ros/waypoint_ros_conversions.hpp

@@ -77,6 +77,8 @@ inline vortex::utils::types::Waypoint waypoint_from_ros(
     vortex::utils::types::Waypoint wp;
     wp.pose = vortex::utils::ros_conversions::ros_pose_to_pose(ros_wp.pose);
     wp.mode = waypoint_mode_from_ros(ros_wp.waypoint_mode);
+    wp.keep_altitude = ros_wp.keep_altitude;
+    wp.desired_altitude = ros_wp.desired_altitude;
     return wp;
 }