Feat/altitude reference

guidance/reference_filter_dp/config/reference_filter_params.yaml

@@ -3,3 +3,5 @@
     zeta: [1., 1., 1., 1., 1., 1.]
     omega: [0.125, 0.125, 0.125, 0.125, 0.125, 0.125]
     time_step_ms: 10
+    altitude_control_enabled: false  # Set true to subscribe to DVL altitude and enable keep_altitude waypoints
+    altitude_lp_alpha: 0.2           # IIR coefficient for DVL altitude low-pass filter [0,1)

guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp

@@ -58,6 +58,25 @@ class WaypointFollower {
      */
     bool within_convergance(const Eigen::Vector6d& measured_pose) const;
 
+    /**
+     * @brief Convergence check that excludes z from the position error.
+     *
+     * Use during altitude-hold mode: the z goal tracks a noisy altitude
+     * measurement, so including it in the convergence criterion would prevent
+     * the action from ever succeeding.
+     */
+    bool within_convergance_ignore_z(
+        const Eigen::Vector6d& measured_pose) const;
+
+    /**
+     * @brief Update only the z component of the reference goal.
+     *
+     * Used during altitude-hold mode to continuously track seafloor distance
+     * without disturbing x/y/orientation targets.
+     * @param target_ned_z The desired NED z coordinate for the AUV.
+     */
+    void update_z_goal(double target_ned_z);
+
     /**
      * @brief Update the reference goal pose mid-sequence.
      * @param reference_goal_pose The new reference pose.

guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp

@@ -31,6 +31,8 @@ enum class WaypointMode : uint8_t {
 struct Waypoint {
     PoseEuler pose{};
     WaypointMode mode = WaypointMode::FULL_POSE;
+    bool keep_altitude{false};
+    double desired_altitude{0.0};
 };
 
 }  // namespace vortex::guidance

guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp

@@ -11,6 +11,7 @@
 #include <std_msgs/msg/empty.hpp>
 #include <vortex/utils/types.hpp>
 #include <vortex_msgs/action/guidance_waypoint.hpp>
+#include <vortex_msgs/msg/dvl_altitude.hpp>
 #include <vortex_msgs/msg/reference_filter.hpp>
 #include <vortex_msgs/msg/waypoint.hpp>
 #include "reference_filter_dp/lib/waypoint_follower.hpp"
@@ -69,12 +70,20 @@ class ReferenceFilterNode : public rclcpp::Node {
     rclcpp::Subscription<
         geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
 
+    rclcpp::Subscription<vortex_msgs::msg::DVLAltitude>::SharedPtr
+        altitude_sub_;
+
     std::chrono::milliseconds time_step_{};
 
     vortex::utils::types::PoseEuler current_pose_;
 
     vortex::utils::types::Twist current_twist_;
 
+    bool altitude_control_enabled_{false};
+    double current_altitude_{0.0};
+    bool altitude_valid_{false};
+    double altitude_lp_alpha_{0.9};
+
     std::mutex sensor_mutex_;
 
     std::atomic<bool> preempted_{false};

guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp

@@ -47,6 +47,8 @@ inline vortex::guidance::Waypoint waypoint_from_ros(
     wp.pose =
         vortex::utils::ros_conversions::ros_pose_to_pose_euler(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;
 }
 

guidance/reference_filter_dp/src/lib/waypoint_follower.cpp

@@ -58,6 +58,20 @@ bool WaypointFollower::within_convergance(
                          convergence_threshold_);
 }
 
+bool WaypointFollower::within_convergance_ignore_z(
+    const Eigen::Vector6d& measured_pose) const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    Eigen::Vector6d adjusted = measured_pose;
+    adjusted(2) = reference_goal_(2);
+    return has_converged(adjusted, reference_goal_, waypoint_mode_,
+                         convergence_threshold_);
+}
+
+void WaypointFollower::update_z_goal(double target_ned_z) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    reference_goal_(2) = target_ned_z;
+}
+
 void WaypointFollower::set_reference(const PoseEuler& reference_goal_pose) {
     std::lock_guard<std::mutex> lock(mutex_);
     reference_goal_ = apply_mode_logic(reference_goal_pose.to_vector(),

guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp

@@ -46,6 +46,8 @@ void ReferenceFilterNode::set_subscribers_and_publisher() {
     this->declare_parameter<std::string>("topics.twist");
     this->declare_parameter<std::string>("topics.guidance.dp_rpy");
     this->declare_parameter<std::string>("topics.reference_pose");
+    altitude_control_enabled_ =
+        this->declare_parameter<bool>("altitude_control_enabled", false);
 
     std::string pose_topic = this->get_parameter("topics.pose").as_string();
     std::string twist_topic = this->get_parameter("topics.twist").as_string();
@@ -86,6 +88,34 @@ void ReferenceFilterNode::set_subscribers_and_publisher() {
             current_twist_ = vortex::utils::ros_conversions::ros_twist_to_twist(
                 msg->twist.twist);
         });
+
+    if (altitude_control_enabled_) {
+        this->declare_parameter<std::string>("topics.dvl_altitude");
+        this->declare_parameter<double>("altitude_lp_alpha", 0.9);
+
+        std::string dvl_altitude_topic =
+            this->get_parameter("topics.dvl_altitude").as_string();
+        altitude_lp_alpha_ =
+            this->get_parameter("altitude_lp_alpha").as_double();
+
+        altitude_sub_ =
+            this->create_subscription<vortex_msgs::msg::DVLAltitude>(
+                dvl_altitude_topic, qos_sensor_data,
+                [this](const vortex_msgs::msg::DVLAltitude::SharedPtr msg) {
+                    std::lock_guard<std::mutex> lock(sensor_mutex_);
+                    if (!altitude_valid_) {
+                        current_altitude_ = msg->altitude;
+                        altitude_valid_ = true;
+                    } else {
+                        current_altitude_ =
+                            altitude_lp_alpha_ * current_altitude_ +
+                            (1.0 - altitude_lp_alpha_) * msg->altitude;
+                    }
+                });
+
+        spdlog::info("Altitude control enabled, subscribing to '{}'",
+                     dvl_altitude_topic);
+    }
 }
 
 void ReferenceFilterNode::setup_reset_subscription() {
@@ -182,7 +212,39 @@ void ReferenceFilterNode::execute(
             "ReferenceFilter: invalid convergence_threshold (<= 0), using 0.1");
     }
 
-    const auto wp = waypoint_from_ros(goal_handle->get_goal()->waypoint);
+    auto wp = waypoint_from_ros(goal_handle->get_goal()->waypoint);
+
+    if (wp.keep_altitude && altitude_control_enabled_ &&
+        wp.desired_altitude <= 0.0) {
+        executing_ = false;
+        auto result =
+            std::make_shared<vortex_msgs::action::GuidanceWaypoint::Result>();
+        result->success = false;
+        goal_handle->abort(result);
+        spdlog::error(
+            "ReferenceFilter: desired_altitude must be > 0, got {:.3f}",
+            wp.desired_altitude);
+        return;
+    }
+
+    if (wp.keep_altitude && altitude_control_enabled_) {
+        const auto [pose, current_alt, alt_valid] = [this] {
+            std::lock_guard lock(sensor_mutex_);
+            return std::tuple{current_pose_, current_altitude_,
+                              altitude_valid_};
+        }();
+        if (!alt_valid) {
+            spdlog::warn(
+                "ReferenceFilter: keep_altitude requested but no DVL altitude "
+                "received yet; proceeding with waypoint z as-is");
+        } else {
+            wp.pose.z = pose.z + current_alt - wp.desired_altitude;
+        }
+        spdlog::info(
+            "Altitude-hold mode: desired_altitude={:.2f} m, initial "
+            "z_goal={:.3f}",
+            wp.desired_altitude, wp.pose.z);
+    }
 
     if (retarget) {
         follower_->retarget(wp, threshold);
@@ -196,6 +258,9 @@ void ReferenceFilterNode::execute(
         spdlog::info("Executing goal (cold start)");
     }
 
+    const bool keep_altitude = wp.keep_altitude && altitude_control_enabled_;
+    const double desired_altitude = wp.desired_altitude;
+
     auto result =
         std::make_shared<vortex_msgs::action::GuidanceWaypoint::Result>();
 
@@ -220,14 +285,28 @@ void ReferenceFilterNode::execute(
 
         Eigen::Vector18d filter_state = follower_->step();
 
+        if (keep_altitude) {
+            const auto [current_z, current_alt] = [this] {
+                std::lock_guard lock(sensor_mutex_);
+                return std::pair{current_pose_.z, current_altitude_};
+            }();
+            follower_->update_z_goal(current_z + current_alt -
+                                     desired_altitude);
+        }
+
         reference_pub_->publish(fill_reference_msg(filter_state));
 
         const auto current_pose_vector = [this] {
             std::lock_guard lock(sensor_mutex_);
             return current_pose_.to_vector();
         }();
 
-        if (follower_->within_convergance(current_pose_vector)) {
+        const bool converged =
+            keep_altitude
+                ? follower_->within_convergance_ignore_z(current_pose_vector)
+                : follower_->within_convergance(current_pose_vector);
+
+        if (converged) {
             follower_->snap_state_to_reference();
 
             reference_pub_->publish(fill_reference_msg(follower_->state()));

guidance/reference_filter_dp_quat/config/reference_filter_params.yaml

@@ -4,3 +4,5 @@
     omega: [0.125, 0.125, 0.125, 0.125, 0.125, 0.125]
     time_step_ms: 10
     publish_rpy_debug: false
+    altitude_control_enabled: true  # Set true to subscribe to DVL altitude and enable keep_altitude waypoints
+    altitude_lp_alpha: 0.2           # IIR coefficient for DVL altitude low-pass filter [0,1)

guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp

@@ -66,12 +66,33 @@ class WaypointFollower {
      */
     bool within_convergance(const Pose& measured_pose) const;
 
+    /**
+     * @brief Convergence check that excludes z from the position error.
+     *
+     * Use this during altitude-hold mode: the z goal tracks a noisy altitude
+     * measurement, so including it in the convergence criterion would prevent
+     * the action from ever succeeding.
+     * @param measured_pose Current measured pose.
+     * @return True if x/y/orientation error is within the convergence
+     * threshold.
+     */
+    bool within_convergance_ignore_z(const Pose& measured_pose) const;
+
     /**
      * @brief Update the reference goal pose mid-sequence.
      * @param reference_goal_pose The new reference pose.
      */
     void set_reference(const Pose& reference_goal_pose);
 
+    /**
+     * @brief Update only the z component of the waypoint goal.
+     *
+     * Used during altitude-hold mode to continuously track seafloor distance
+     * without disturbing the x/y/orientation targets.
+     * @param target_ned_z The desired NED z coordinate for the AUV.
+     */
+    void update_z_goal(double target_ned_z);
+
     /**
      * @brief Snap the nominal pose to the waypoint goal and zero
      * errors/velocity.

guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp

@@ -11,6 +11,7 @@
 #include <std_msgs/msg/empty.hpp>
 #include <vortex/utils/types.hpp>
 #include <vortex_msgs/action/guidance_waypoint.hpp>
+#include <vortex_msgs/msg/dvl_altitude.hpp>
 #include <vortex_msgs/msg/reference_filter.hpp>
 #include <vortex_msgs/msg/reference_filter_quat.hpp>
 #include <vortex_msgs/msg/waypoint.hpp>
@@ -77,12 +78,20 @@ class ReferenceFilterNode : public rclcpp::Node {
     rclcpp::Subscription<
         geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
 
+    rclcpp::Subscription<vortex_msgs::msg::DVLAltitude>::SharedPtr
+        altitude_sub_;
+
     std::chrono::milliseconds time_step_{};
 
     vortex::utils::types::Pose current_pose_;
 
     vortex::utils::types::Twist current_twist_;
 
+    bool altitude_control_enabled_{false};
+    double current_altitude_{0.0};
+    bool altitude_valid_{false};
+    double altitude_lp_alpha_{0.9};
+
     std::mutex sensor_mutex_;
 
     std::atomic<bool> preempted_{false};

guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp

@@ -85,12 +85,26 @@ bool WaypointFollower::within_convergance(const Pose& measured_pose) const {
         measured_pose, waypoint_goal_, waypoint_mode_, convergence_threshold_);
 }
 
+bool WaypointFollower::within_convergance_ignore_z(
+    const Pose& measured_pose) const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    Pose adjusted = measured_pose;
+    adjusted.z = waypoint_goal_.z;
+    return vortex::utils::waypoints::has_converged(
+        adjusted, waypoint_goal_, waypoint_mode_, convergence_threshold_);
+}
+
 void WaypointFollower::set_reference(const Pose& reference_goal_pose) {
     std::lock_guard<std::mutex> lock(mutex_);
     waypoint_goal_ = vortex::utils::waypoints::compute_waypoint_goal(
         reference_goal_pose, waypoint_mode_, nominal_pose_);
 }
 
+void WaypointFollower::update_z_goal(double target_ned_z) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    waypoint_goal_.z = target_ned_z;
+}
+
 void WaypointFollower::snap_state_to_reference() {
     std::lock_guard<std::mutex> lock(mutex_);
     nominal_pose_ = waypoint_goal_;