diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp
new file mode 100644
index 000000000..6fa179291
--- /dev/null
+++ b/guidance/reference_filter_dp/include/reference_filter_dp/reference_filter_ros.hpp
@@ -0,0 +1,131 @@
+#ifndef REFERENCE_FILTER_DP__REFERENCE_FILTER_ROS_HPP_
+#define REFERENCE_FILTER_DP__REFERENCE_FILTER_ROS_HPP_
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <vortex_msgs/action/reference_filter_waypoint.hpp>
+#include <vortex_msgs/msg/reference_filter.hpp>
+#include "reference_filter_dp/eigen_typedefs.hpp"
+#include "reference_filter_dp/reference_filter.hpp"
+
+namespace vortex::guidance {
+
+class ReferenceFilterNode : public rclcpp::Node {
+   public:
+    explicit ReferenceFilterNode(
+        const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
+   private:
+    // @brief Set the subscribers and publishers
+    void set_subscribers_and_publisher();
+
+    // @brief Set the action server
+    void set_action_server();
+
+    // @brief Initializes the reference filter with ROS parameters.
+    void set_refererence_filter();
+
+    // @brief Callback for the reference topic
+    // @param msg The reference message
+    void reference_callback(
+        const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+
+    // @brief Callback for the pose topic
+    // @param msg The pose message
+    void pose_callback(
+        const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+
+    // @brief Callback for the twist topic
+    // @param msg The twist message
+    void twist_callback(
+        const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
+
+    // @brief Handle the goal request
+    // @param uuid The goal UUID
+    // @param goal The goal message
+    // @return The goal response
+    rclcpp_action::GoalResponse handle_goal(
+        const rclcpp_action::GoalUUID& uuid,
+        std::shared_ptr<
+            const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal);
+
+    // @brief Handle the cancel request
+    // @param goal_handle The goal handle
+    // @return The cancel response
+    rclcpp_action::CancelResponse handle_cancel(
+        const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+            vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle);
+
+    // @brief Handle the accepted request
+    // @param goal_handle The goal handle
+    void handle_accepted(
+        const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+            vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle);
+
+    // @brief Execute the goal
+    // @param goal_handle The goal handle
+    void execute(
+        const std::shared_ptr<rclcpp_action::ServerGoalHandle<
+            vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle);
+
+    Eigen::Vector18d fill_reference_state();
+
+    Eigen::Vector6d fill_reference_goal(const geometry_msgs::msg::Pose& goal);
+
+    Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& r_in, uint8_t mode);
+
+    void publish_hold_reference();
+
+    vortex_msgs::msg::ReferenceFilter fill_reference_msg();
+
+    rclcpp_action::Server<
+        vortex_msgs::action::ReferenceFilterWaypoint>::SharedPtr action_server_;
+
+    std::unique_ptr<ReferenceFilter> reference_filter_{};
+
+    rclcpp::Publisher<vortex_msgs::msg::ReferenceFilter>::SharedPtr
+        reference_pub_;
+
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
+        reference_sub_;
+
+    rclcpp::Subscription<
+        geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
+
+    rclcpp::Subscription<
+        geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
+
+    rclcpp::TimerBase::SharedPtr reference_pub_timer_;
+
+    std::chrono::milliseconds time_step_{};
+
+    geometry_msgs::msg::PoseWithCovarianceStamped current_pose_;
+
+    geometry_msgs::msg::TwistWithCovarianceStamped current_twist_;
+
+    // x is [eta, eta_dot, eta_dot_dot] (ref. page 337 in Fossen, 2021
+    // nu and eta are 6 degrees of freedom (position and orientation in 3D
+    // space)
+    Eigen::Vector18d x_ = Eigen::Vector18d::Zero();
+
+    // The reference signal vector with 6 degrees of freedom [eta]
+    Eigen::Vector6d r_ = Eigen::Vector6d::Zero();
+
+    std::mutex mutex_;
+
+    rclcpp_action::GoalUUID preempted_goal_id_;
+
+    std::shared_ptr<rclcpp_action::ServerGoalHandle<
+        vortex_msgs::action::ReferenceFilterWaypoint>>
+        goal_handle_;
+
+    rclcpp::CallbackGroup::SharedPtr cb_group_;
+};
+
+}  // namespace vortex::guidance
+
+#endif  // REFERENCE_FILTER_DP__REFERENCE_FILTER_ROS_HPP_
diff --git a/tests/ros_node_tests/gripper_reference_filter_node_test.sh b/tests/ros_node_tests/gripper_reference_filter_node_test.sh
new file mode 100755
index 000000000..f03b0c749
--- /dev/null
+++ b/tests/ros_node_tests/gripper_reference_filter_node_test.sh
@@ -0,0 +1,135 @@
+#!/bin/bash
+set -e
+set -o pipefail
+
+# Goal mode argument (GripperWaypoint mode constraints)
+#   0 = ROLL_AND_PINCH
+#   1 = ONLY_ROLL
+#   2 = ONLY_PINCH
+MODE_ARG=${1:-0}
+
+# Goal values can be overridden from env without editing the file.
+ROLL_TARGET=${ROLL_TARGET:-1.57}
+PINCH_TARGET=${PINCH_TARGET:--0.10}
+CONVERGENCE_THRESHOLD=${CONVERGENCE_THRESHOLD:-0.05}
+
+ACTION_NAME="/vortex/gripper/reference_filter"
+ACTION_TYPE="vortex_msgs/action/GripperReferenceFilterWaypoint"
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+WORKSPACE_DIR=""
+
+MODE_NAME=""
+case "$MODE_ARG" in
+	0) MODE_NAME="ROLL_AND_PINCH" ;;
+	1) MODE_NAME="ONLY_ROLL" ;;
+	2) MODE_NAME="ONLY_PINCH" ;;
+	*)
+		echo "Invalid mode '$MODE_ARG'. Valid values: 0 (ROLL_AND_PINCH), 1 (ONLY_ROLL), 2 (ONLY_PINCH)"
+		exit 1
+		;;
+esac
+
+# Valid pinch range is [-0.333, 0.0].
+if ! awk "BEGIN {exit !($PINCH_TARGET >= -0.333 && $PINCH_TARGET <= 0.0)}"; then
+	echo "Invalid pinch target '$PINCH_TARGET'. Expected range is [-0.333, 0.0]."
+	exit 1
+fi
+
+GOAL_PAYLOAD="{waypoint: {roll: {roll: $ROLL_TARGET}, pinch: {pinch: $PINCH_TARGET}, mode: $MODE_ARG}, convergence_threshold: $CONVERGENCE_THRESHOLD}"
+
+FILTER_PID=""
+ACTION_LOG_FILE=""
+ACTION_PID=""
+
+find_workspace_dir() {
+	local start_dir=""
+	local dir=""
+	local -a start_points=()
+
+	[[ -n "${WORKSPACE:-}" ]] && start_points+=("$WORKSPACE")
+	start_points+=("$PWD" "$SCRIPT_DIR")
+
+	for start_dir in "${start_points[@]}"; do
+		dir="$start_dir"
+		while true; do
+			if [[ -f "$dir/install/setup.bash" ]]; then
+				echo "$dir"
+				return 0
+			fi
+			[[ "$dir" == "/" ]] && break
+			dir="$(dirname "$dir")"
+		done
+	done
+
+	return 1
+}
+
+cleanup() {
+	echo "Error detected. Cleaning up..."
+	[[ -n "$ACTION_PID" ]] && kill -TERM "$ACTION_PID" 2>/dev/null || true
+	kill -TERM -"$FILTER_PID" || true
+	[[ -n "$ACTION_LOG_FILE" && -f "$ACTION_LOG_FILE" ]] && rm -f "$ACTION_LOG_FILE"
+	exit 1
+}
+trap cleanup ERR
+
+# Load ROS 2 environment
+echo "Setting up ROS 2 environment..."
+. /opt/ros/humble/setup.sh
+WORKSPACE_DIR="$(find_workspace_dir)" || {
+	echo "Unable to locate workspace root containing install/setup.bash."
+	echo "Set WORKSPACE to your ROS 2 workspace path and re-run."
+	exit 1
+}
+. "$WORKSPACE_DIR/install/setup.bash"
+echo "Detected workspace: $WORKSPACE_DIR"
+
+echo "Using goal config: mode=$MODE_ARG ($MODE_NAME), roll=$ROLL_TARGET, pinch=$PINCH_TARGET, convergence_threshold=$CONVERGENCE_THRESHOLD"
+
+# Launch gripper reference filter node
+echo "Launching gripper reference filter..."
+setsid ros2 launch gripper_reference_filter gripper_reference_filter.launch.py &
+FILTER_PID=$!
+echo "Launched gripper reference filter with PID: $FILTER_PID"
+
+# Check for ROS errors before continuing
+if journalctl -u ros2 | grep -i "error"; then
+	echo "Error detected in ROS logs. Exiting..."
+	exit 1
+fi
+
+# Seed current gripper state so the filter has a valid initial state.
+echo "Publishing initial gripper state..."
+ros2 topic pub /vortex/gripper/state vortex_msgs/msg/GripperState "{roll: 0.0, pinch: -0.333}" --once >/dev/null
+
+ACTION_LOG_FILE=$(mktemp)
+
+echo "Sending gripper goal with feedback..."
+ros2 action send_goal "$ACTION_NAME" "$ACTION_TYPE" "$GOAL_PAYLOAD" --feedback >"$ACTION_LOG_FILE" 2>&1 &
+ACTION_PID=$!
+
+# Check if node correctly publishes guidance
+echo "Waiting for guidance data..."
+timeout 15s ros2 topic echo /vortex/gripper/guidance --once --qos-reliability best_effort
+echo "Got guidance data"
+
+wait "$ACTION_PID"
+
+echo "Action output:"
+cat "$ACTION_LOG_FILE"
+
+if ! grep -Eiq "Goal finished with status: SUCCEEDED|Goal succeeded" "$ACTION_LOG_FILE"; then
+	echo "Goal did not report SUCCEEDED status."
+	exit 1
+fi
+
+if ! grep -Eiq "success:[[:space:]]*(true|True)" "$ACTION_LOG_FILE"; then
+	echo "Action result did not report success=true."
+	exit 1
+fi
+
+# Terminate process
+kill -TERM -"$FILTER_PID"
+rm -f "$ACTION_LOG_FILE"
+
+echo "Test completed successfully."
diff --git a/tests/simulator_tests/gripper_test/check_goal.py b/tests/simulator_tests/gripper_test/check_goal.py
new file mode 100644
index 000000000..e3ce323e5
--- /dev/null
+++ b/tests/simulator_tests/gripper_test/check_goal.py
@@ -0,0 +1,107 @@
+#!/usr/bin/env python3
+import argparse
+import math
+import os
+import sys
+
+import yaml
+from action_msgs.msg import GoalStatus
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="Validate gripper action result and feedback convergence"
+    )
+    parser.add_argument(
+        "--output-dir", default=os.path.dirname(os.path.abspath(__file__))
+    )
+    parser.add_argument("--margin", type=float, default=0.02)
+    return parser.parse_args()
+
+
+def read_yaml(path: str) -> dict:
+    with open(path, encoding="utf-8") as file_handle:
+        return yaml.safe_load(file_handle)
+
+
+def main() -> int:
+    args = parse_args()
+
+    output_dir = os.path.abspath(args.output_dir)
+    goal_file = os.path.join(output_dir, "gripper_goal.yaml")
+    result_file = os.path.join(output_dir, "gripper_result.yaml")
+
+    if not os.path.exists(goal_file):
+        print(f"Missing goal file: {goal_file}")
+        return 1
+
+    if not os.path.exists(result_file):
+        print(f"Missing result file: {result_file}")
+        return 1
+
+    goal = read_yaml(goal_file)
+    result = read_yaml(result_file)
+
+    action_success = bool(result.get("action_success", False))
+    goal_status = int(result.get("goal_status", GoalStatus.STATUS_UNKNOWN))
+    goal_status_text = result.get("goal_status_text", "UNKNOWN")
+
+    if not action_success:
+        print(f"Action result reported success=false (status={goal_status_text})")
+        return 1
+
+    if goal_status != GoalStatus.STATUS_SUCCEEDED:
+        print(f"Action status is not SUCCEEDED: {goal_status_text}")
+        return 1
+
+    feedback = result.get("last_feedback")
+    if not isinstance(feedback, dict):
+        print(
+            "No feedback captured from action execution; relying on action success/status only"
+        )
+        print("Goal check passed")
+        return 0
+
+    mode = int(goal["mode"])
+    target_roll = float(goal["roll"])
+    target_pinch = float(goal["pinch"])
+    threshold = float(goal["convergence_threshold"])
+
+    feedback_roll = float(feedback["roll"])
+    feedback_pinch = float(feedback["pinch"])
+
+    margin = float(args.margin)
+    tolerance = threshold + margin
+
+    if mode == 0:
+        error = math.hypot(feedback_roll - target_roll, feedback_pinch - target_pinch)
+        mode_name = "ROLL_AND_PINCH"
+    elif mode == 1:
+        error = abs(feedback_roll - target_roll)
+        mode_name = "ONLY_ROLL"
+    elif mode == 2:
+        error = abs(feedback_pinch - target_pinch)
+        mode_name = "ONLY_PINCH"
+    else:
+        print(f"Unsupported mode in goal file: {mode}")
+        return 1
+
+    print(
+        "Goal check:\n"
+        f"  mode={mode} ({mode_name})\n"
+        f"  target_roll={target_roll:.6f}, target_pinch={target_pinch:.6f}\n"
+        f"  feedback_roll={feedback_roll:.6f}, feedback_pinch={feedback_pinch:.6f}\n"
+        f"  threshold={threshold:.6f}, margin={margin:.6f}, tolerance={tolerance:.6f}\n"
+        f"  error={error:.6f}"
+    )
+
+    if error > tolerance:
+        print("Goal check failed: feedback did not converge within tolerance")
+        return 1
+
+    print("Goal check passed")
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())
diff --git a/tests/simulator_tests/gripper_test/gripper_goal.yaml b/tests/simulator_tests/gripper_test/gripper_goal.yaml
new file mode 100644
index 000000000..b132fe3c7
--- /dev/null
+++ b/tests/simulator_tests/gripper_test/gripper_goal.yaml
@@ -0,0 +1,5 @@
+action_name: /vortex/gripper/reference_filter
+mode: 0
+roll: 1.57
+pinch: -0.1
+convergence_threshold: 0.05
diff --git a/tests/simulator_tests/gripper_test/send_goal.py b/tests/simulator_tests/gripper_test/send_goal.py
new file mode 100644
index 000000000..ad2d83759
--- /dev/null
+++ b/tests/simulator_tests/gripper_test/send_goal.py
@@ -0,0 +1,179 @@
+#!/usr/bin/env python3
+import argparse
+import os
+import sys
+
+import rclpy
+import yaml
+from action_msgs.msg import GoalStatus
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from vortex_msgs.action import GripperReferenceFilterWaypoint
+
+
+def goal_status_to_text(status: int) -> str:
+    mapping = {
+        GoalStatus.STATUS_UNKNOWN: "UNKNOWN",
+        GoalStatus.STATUS_ACCEPTED: "ACCEPTED",
+        GoalStatus.STATUS_EXECUTING: "EXECUTING",
+        GoalStatus.STATUS_CANCELING: "CANCELING",
+        GoalStatus.STATUS_SUCCEEDED: "SUCCEEDED",
+        GoalStatus.STATUS_CANCELED: "CANCELED",
+        GoalStatus.STATUS_ABORTED: "ABORTED",
+    }
+    return mapping.get(status, f"UNRECOGNIZED({status})")
+
+
+def write_yaml(path: str, payload: dict) -> None:
+    with open(path, "w", encoding="utf-8") as file_handle:
+        yaml.safe_dump(payload, file_handle, sort_keys=False)
+
+
+class GripperGoalClient(Node):
+    def __init__(self, action_name: str):
+        super().__init__("gripper_reference_filter_waypoint_client")
+        self.action_client = ActionClient(
+            self,
+            GripperReferenceFilterWaypoint,
+            action_name,
+        )
+        self.latest_feedback = None
+
+    def feedback_callback(self, feedback_msg):
+        reference = feedback_msg.feedback.reference
+        self.latest_feedback = {
+            "roll": float(reference.roll),
+            "pinch": float(reference.pinch),
+        }
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="Send gripper reference filter goal and persist result for check_goal.py"
+    )
+    parser.add_argument(
+        "--mode",
+        type=int,
+        default=0,
+        help="0=ROLL_AND_PINCH, 1=ONLY_ROLL, 2=ONLY_PINCH",
+    )
+    parser.add_argument("--roll", type=float, default=1.57)
+    parser.add_argument("--pinch", type=float, default=-0.10)
+    parser.add_argument("--convergence-threshold", type=float, default=0.05)
+    parser.add_argument("--action-name", default="/vortex/gripper/reference_filter")
+    parser.add_argument(
+        "--output-dir", default=os.path.dirname(os.path.abspath(__file__))
+    )
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+
+    if args.mode not in (0, 1, 2):
+        print(
+            "Invalid mode. Valid values: 0 (ROLL_AND_PINCH), 1 (ONLY_ROLL), 2 (ONLY_PINCH)"
+        )
+        return 1
+
+    if not (-0.333 <= args.pinch <= 0.0):
+        print("Invalid pinch target. Expected range is [-0.333, 0.0].")
+        return 1
+
+    output_dir = os.path.abspath(args.output_dir)
+    os.makedirs(output_dir, exist_ok=True)
+
+    goal_file = os.path.join(output_dir, "gripper_goal.yaml")
+    result_file = os.path.join(output_dir, "gripper_result.yaml")
+
+    goal_payload = {
+        "action_name": args.action_name,
+        "mode": int(args.mode),
+        "roll": float(args.roll),
+        "pinch": float(args.pinch),
+        "convergence_threshold": float(args.convergence_threshold),
+    }
+    write_yaml(goal_file, goal_payload)
+
+    result_payload = {
+        "action_success": False,
+        "goal_status": int(GoalStatus.STATUS_UNKNOWN),
+        "goal_status_text": goal_status_to_text(int(GoalStatus.STATUS_UNKNOWN)),
+        "last_feedback": None,
+    }
+
+    rclpy.init()
+    node = GripperGoalClient(args.action_name)
+
+    try:
+        if not node.action_client.wait_for_server(timeout_sec=20.0):
+            print(f"Timed out waiting for action server: {args.action_name}")
+            write_yaml(result_file, result_payload)
+            return 1
+
+        goal_msg = GripperReferenceFilterWaypoint.Goal()
+        goal_msg.waypoint.roll.roll = float(args.roll)
+        goal_msg.waypoint.pinch.pinch = float(args.pinch)
+        goal_msg.waypoint.mode = int(args.mode)
+        goal_msg.convergence_threshold = float(args.convergence_threshold)
+
+        print(
+            f"Sending goal: mode={args.mode}, roll={args.roll}, pinch={args.pinch}, "
+            f"convergence_threshold={args.convergence_threshold}"
+        )
+
+        send_goal_future = node.action_client.send_goal_async(
+            goal_msg,
+            feedback_callback=node.feedback_callback,
+        )
+
+        while rclpy.ok() and not send_goal_future.done():
+            rclpy.spin_once(node, timeout_sec=0.1)
+
+        goal_handle = send_goal_future.result()
+        if goal_handle is None or not goal_handle.accepted:
+            print("Goal rejected")
+            write_yaml(result_file, result_payload)
+            return 1
+
+        print("Goal accepted")
+        get_result_future = goal_handle.get_result_async()
+
+        while rclpy.ok() and not get_result_future.done():
+            rclpy.spin_once(node, timeout_sec=0.1)
+
+        wrapped_result = get_result_future.result()
+        if wrapped_result is None:
+            print("Failed to retrieve action result")
+            write_yaml(result_file, result_payload)
+            return 1
+
+        goal_status = int(wrapped_result.status)
+        action_success = bool(wrapped_result.result.success)
+
+        result_payload = {
+            "action_success": action_success,
+            "goal_status": goal_status,
+            "goal_status_text": goal_status_to_text(goal_status),
+            "last_feedback": node.latest_feedback,
+        }
+        write_yaml(result_file, result_payload)
+
+        print(
+            f"Action finished with status={result_payload['goal_status_text']} "
+            f"and success={action_success}"
+        )
+
+        if action_success and goal_status == GoalStatus.STATUS_SUCCEEDED:
+            return 0
+
+        return 1
+
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    sys.exit(main())
diff --git a/tests/simulator_tests/gripper_test/simulator_test.sh b/tests/simulator_tests/gripper_test/simulator_test.sh
new file mode 100755
index 000000000..b92a494c3
--- /dev/null
+++ b/tests/simulator_tests/gripper_test/simulator_test.sh
@@ -0,0 +1,218 @@
+#!/bin/bash
+set -e
+set -o pipefail
+
+DRONE="${1:-nautilus}"
+MODE_ARG="${2:-0}"
+
+# Match manual behavior by default when a display exists; allow CI/headless override.
+if [[ -z "${RENDERING:-}" ]]; then
+    if [[ -n "${DISPLAY:-}" ]]; then
+        RENDERING="true"
+    else
+        RENDERING="false"
+    fi
+fi
+
+# Goal values can be overridden from env without editing the script.
+ROLL_TARGET="${ROLL_TARGET:-1.57}"
+PINCH_TARGET="${PINCH_TARGET:--0.10}"
+CONVERGENCE_THRESHOLD="${CONVERGENCE_THRESHOLD:-0.05}"
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+WORKSPACE_DIR=""
+
+SIM_PID=""
+GRIPPER_CONTROLLER_PID=""
+GRIPPER_REF_FILTER_PID=""
+GRIPPER_SIM_BRIDGE_PID=""
+GUIDANCE_WATCH_PID=""
+CONTROL_WATCH_PID=""
+
+find_workspace_dir() {
+    local start_dir=""
+    local dir=""
+    local -a start_points=()
+
+    [[ -n "${WORKSPACE:-}" ]] && start_points+=("$WORKSPACE")
+    start_points+=("$PWD" "$SCRIPT_DIR")
+
+    for start_dir in "${start_points[@]}"; do
+        dir="$start_dir"
+        while true; do
+            if [[ -f "$dir/install/setup.bash" ]]; then
+                echo "$dir"
+                return 0
+            fi
+            [[ "$dir" == "/" ]] && break
+            dir="$(dirname "$dir")"
+        done
+    done
+
+    return 1
+}
+
+wait_for_topic() {
+    local topic_name="$1"
+    local timeout_s="$2"
+
+    echo "Waiting for topic: $topic_name"
+    timeout "${timeout_s}s" bash -c 'until ros2 topic list | grep -Fxq "$1"; do sleep 1; done' _ "$topic_name"
+}
+
+wait_for_service() {
+    local service_name="$1"
+    local timeout_s="$2"
+
+    echo "Waiting for service: $service_name"
+    timeout "${timeout_s}s" bash -c 'until ros2 service list | grep -Fxq "$1"; do sleep 1; done' _ "$service_name"
+}
+
+run_in_workspace_shell() {
+    local cmd="$1"
+    bash -lc "cd \"$WORKSPACE_DIR\" && source /opt/ros/humble/setup.sh && source \"$WORKSPACE_DIR/install/setup.bash\" && $cmd"
+}
+
+cleanup() {
+    echo "Error detected. Cleaning up..."
+
+    [[ -n "$GUIDANCE_WATCH_PID" ]] && kill -TERM "$GUIDANCE_WATCH_PID" 2>/dev/null || true
+    [[ -n "$CONTROL_WATCH_PID" ]] && kill -TERM "$CONTROL_WATCH_PID" 2>/dev/null || true
+
+    [[ -n "$GRIPPER_SIM_BRIDGE_PID" ]] && kill -TERM -"$GRIPPER_SIM_BRIDGE_PID" 2>/dev/null || true
+    [[ -n "$GRIPPER_REF_FILTER_PID" ]] && kill -TERM -"$GRIPPER_REF_FILTER_PID" 2>/dev/null || true
+    [[ -n "$GRIPPER_CONTROLLER_PID" ]] && kill -TERM -"$GRIPPER_CONTROLLER_PID" 2>/dev/null || true
+    [[ -n "$SIM_PID" ]] && kill -TERM -"$SIM_PID" 2>/dev/null || true
+
+    rm -f "$SCRIPT_DIR/gripper_goal.yaml" "$SCRIPT_DIR/gripper_result.yaml"
+
+    exit 1
+}
+trap cleanup ERR
+
+case "$MODE_ARG" in
+    0) MODE_NAME="ROLL_AND_PINCH" ;;
+    1) MODE_NAME="ONLY_ROLL" ;;
+    2) MODE_NAME="ONLY_PINCH" ;;
+    *)
+        echo "Invalid mode '$MODE_ARG'. Valid values: 0 (ROLL_AND_PINCH), 1 (ONLY_ROLL), 2 (ONLY_PINCH)"
+        exit 1
+        ;;
+esac
+
+# Valid pinch range is [-0.333, 0.0].
+if ! awk "BEGIN {exit !($PINCH_TARGET >= -0.333 && $PINCH_TARGET <= 0.0)}"; then
+    echo "Invalid pinch target '$PINCH_TARGET'. Expected range is [-0.333, 0.0]."
+    exit 1
+fi
+
+if [[ "$DRONE" != "nautilus" ]]; then
+    echo "gripper_sim_interface currently bridges nautilus topics only."
+    echo "Use DRONE=nautilus for this simulator test."
+    exit 1
+fi
+
+echo "Setting up ROS 2 environment..."
+. /opt/ros/humble/setup.sh
+WORKSPACE_DIR="$(find_workspace_dir)" || {
+    echo "Unable to locate workspace root containing install/setup.bash."
+    echo "Set WORKSPACE to your ROS 2 workspace path and re-run."
+    exit 1
+}
+. "$WORKSPACE_DIR/install/setup.bash"
+export LD_LIBRARY_PATH="${LD_LIBRARY_PATH:+$LD_LIBRARY_PATH:}/usr/local/lib"
+
+echo "Detected workspace: $WORKSPACE_DIR"
+echo "Using goal config: drone=$DRONE mode=$MODE_ARG ($MODE_NAME), roll=$ROLL_TARGET, pinch=$PINCH_TARGET, convergence_threshold=$CONVERGENCE_THRESHOLD, rendering=$RENDERING"
+
+# Match launch_drone.sh stack; rendering can be toggled with RENDERING=true/false.
+echo "Launching simulator..."
+setsid ros2 launch stonefish_sim vortex_sim_launch.py drone:=${DRONE} rendering:=${RENDERING} &
+SIM_PID=$!
+echo "Launched simulator with PID: $SIM_PID"
+
+echo "Launching gripper controller..."
+setsid ros2 launch gripper_controller gripper_controller.launch.py &
+GRIPPER_CONTROLLER_PID=$!
+echo "Launched gripper controller with PID: $GRIPPER_CONTROLLER_PID"
+
+echo "Launching gripper reference filter..."
+setsid ros2 launch gripper_reference_filter gripper_reference_filter.launch.py &
+GRIPPER_REF_FILTER_PID=$!
+echo "Launched gripper reference filter with PID: $GRIPPER_REF_FILTER_PID"
+
+echo "Launching gripper sim interface..."
+setsid ros2 launch gripper_sim_interface gripper_sim_interface.launch.py &
+GRIPPER_SIM_BRIDGE_PID=$!
+echo "Launched gripper sim interface with PID: $GRIPPER_SIM_BRIDGE_PID"
+
+echo "Waiting for simulator to start..."
+wait_for_topic "/${DRONE}/odom" 60
+timeout 15s ros2 topic echo "/${DRONE}/odom" --once >/dev/null
+echo "Simulator online"
+
+echo "Waiting for gripper pipeline readiness..."
+# Bridge input from stonefish sim
+wait_for_topic "/${DRONE}/servo_state" 60
+timeout 20s ros2 topic echo "/${DRONE}/servo_state" --once >/dev/null
+# Bridge output from gripper_sim_interface
+timeout 30s ros2 topic echo /vortex/gripper/state --once >/dev/null
+
+# Check for ROS errors before continuing
+if journalctl -u ros2 | grep -i "error"; then
+    echo "Error detected in ROS logs. Exiting..."
+    exit 1
+fi
+
+# Manual workflow parity:
+# - Wait ~5 seconds after sim appears online.
+# - Emulate keyboard '2' then '3' through service calls.
+echo "Sleeping 5 seconds for sim spool-up..."
+sleep 5
+
+echo "Emulating keyboard input: key '2' (killswitch false)"
+wait_for_service "/${DRONE}/set_killswitch" 30
+run_in_workspace_shell "timeout 20s ros2 service call /${DRONE}/set_killswitch vortex_msgs/srv/SetKillswitch '{killswitch_on: false}'" >/dev/null
+
+echo "Emulating keyboard input: key '3' (autonomous mode)"
+wait_for_service "/${DRONE}/set_operation_mode" 30
+run_in_workspace_shell "timeout 20s ros2 service call /${DRONE}/set_operation_mode vortex_msgs/srv/SetOperationMode '{requested_operation_mode: {operation_mode: 1}}'" >/dev/null
+
+# Let operation mode propagate before sending action goal.
+sleep 2
+
+echo "Waiting for guidance output from reference filter..."
+timeout 40s ros2 topic echo /vortex/gripper/guidance --once --qos-reliability best_effort >/dev/null &
+GUIDANCE_WATCH_PID=$!
+
+echo "Waiting for controller output command..."
+timeout 40s ros2 topic echo /vortex/gripper/control --once --qos-reliability best_effort >/dev/null &
+CONTROL_WATCH_PID=$!
+
+echo "Sending goal (terminal-like subshell)"
+run_in_workspace_shell "python3 \"$SCRIPT_DIR/send_goal.py\" \
+    --mode "$MODE_ARG" \
+    --roll "$ROLL_TARGET" \
+    --pinch "$PINCH_TARGET" \
+    --convergence-threshold "$CONVERGENCE_THRESHOLD" \
+    --output-dir \"$SCRIPT_DIR\""
+
+wait "$GUIDANCE_WATCH_PID"
+echo "Got guidance data"
+
+wait "$CONTROL_WATCH_PID"
+echo "Got controller output"
+
+echo "Checking if goal reached"
+if ! run_in_workspace_shell "python3 \"$SCRIPT_DIR/check_goal.py\" --output-dir \"$SCRIPT_DIR\""; then
+    echo "Test failed: Gripper did not reach goal."
+    exit 1
+fi
+echo "Test passed: Gripper reached goal."
+
+echo "Terminating launched processes..."
+kill -TERM -"$GRIPPER_SIM_BRIDGE_PID" -"$GRIPPER_REF_FILTER_PID" -"$GRIPPER_CONTROLLER_PID" -"$SIM_PID" || true
+
+rm -f "$SCRIPT_DIR/gripper_goal.yaml" "$SCRIPT_DIR/gripper_result.yaml"
+
+echo "Gripper simulator test completed successfully."