From 75f42ffb5bd17bf2136f7ed25d7c517890558116 Mon Sep 17 00:00:00 2001
From: Stefan Scherzinger <stefan.scherzinger@de.schunk.com>
Date: Mon, 26 Aug 2024 17:14:47 +0200
Subject: [PATCH] Add a test for the gripper's `gripper_control` action

Also add meaningful values for `max_grip_force`, `min_grip_force`,
`max_vel`, and `min_vel` (EGK 40).
---
 schunk_egu_egk_gripper_dummy/config/data.json |  8 ++++----
 .../test/test_actions.py                      | 20 +++++++++++++++++++
 2 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/schunk_egu_egk_gripper_dummy/config/data.json b/schunk_egu_egk_gripper_dummy/config/data.json
index 470c567..b28eda0 100644
--- a/schunk_egu_egk_gripper_dummy/config/data.json
+++ b/schunk_egu_egk_gripper_dummy/config/data.json
@@ -72,19 +72,19 @@
         "00000000"
     ],
     "0x0628": [
-        "40B00000"
+        "0000B040"
     ],
     "0x0630": [
-        "42E60000"
+        "0000E642"
     ],
     "0x0650": [
         "41B00000"
     ],
     "0x0658": [
-        "42960000"
+        "00009642"
     ],
     "0x0660": [
-        "43160000"
+        "0000B442"
     ],
     "0x06A8": [
         "00000000"
diff --git a/schunk_egu_egk_gripper_tests/test/test_actions.py b/schunk_egu_egk_gripper_tests/test/test_actions.py
index 29665d9..6524906 100644
--- a/schunk_egu_egk_gripper_tests/test/test_actions.py
+++ b/schunk_egu_egk_gripper_tests/test/test_actions.py
@@ -8,6 +8,7 @@
     MoveToRelativePosition,
     ReleaseWorkpiece,
 )
+from control_msgs.action import GripperCommand
 
 
 @pytest.mark.launch(fixture=launch_description)
@@ -57,6 +58,25 @@ def test_driver_grips_with_position(running_driver):
     assert action.result.workpiece_lost is False
 
 
+@pytest.mark.launch(fixture=launch_description)
+def test_driver_supports_gripper_control(running_driver):
+
+    runs = [
+        {"position": 0.0, "max_effort": 77.0},  # grip workpiece
+        {"position": 82.4, "max_effort": 0.0},  # move to abs. position
+        {"position": 66.3, "max_effort": 85.1},  # grip with position
+    ]
+
+    for run in runs:
+        goal = GripperCommand.Goal()
+        goal.command.position = run["position"]
+        goal.command.max_effort = run["max_effort"]
+        action = ActionReturnsResult("/gripper_control", GripperCommand, goal)
+        action.event.wait()
+        assert action.result.reached_goal
+        assert action.result.stalled
+
+
 @pytest.mark.launch(fixture=launch_description)
 def test_driver_moves_to_absolute_position(running_driver):
     test_positions = [43.55, 17.02, 38.55, 103.7]