584 task gripper controller merge into main

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_

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."

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())

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