From 8ccb6034ddb929b3914434de3a5b0d63321e71d0 Mon Sep 17 00:00:00 2001
From: Ruddick Lawrence <679360+mrjogo@users.noreply.github.com>
Date: Wed, 31 Jul 2024 16:21:48 -0700
Subject: [PATCH] Add SetHoldingTorque message. Has to be in different package
 because of https://github.com/ament/ament_cmake/issues/514

---
 ar_gripper/ar_gripper/scripts/ar_gripper.py   | 12 ++++-----
 ar_gripper/package.xml                        |  1 +
 ar_gripper_interfaces/CMakeLists.txt          | 16 ++++++++++++
 ar_gripper_interfaces/package.xml             | 25 +++++++++++++++++++
 .../srv/SetHoldingTorque.srv                  |  4 +++
 5 files changed, 52 insertions(+), 6 deletions(-)
 create mode 100644 ar_gripper_interfaces/CMakeLists.txt
 create mode 100644 ar_gripper_interfaces/package.xml
 create mode 100644 ar_gripper_interfaces/srv/SetHoldingTorque.srv

diff --git a/ar_gripper/ar_gripper/scripts/ar_gripper.py b/ar_gripper/ar_gripper/scripts/ar_gripper.py
index 28f514d..900d8f3 100755
--- a/ar_gripper/ar_gripper/scripts/ar_gripper.py
+++ b/ar_gripper/ar_gripper/scripts/ar_gripper.py
@@ -7,7 +7,7 @@
 from threading import Lock
 
 import rclpy
-from barbot_interfaces.srv import UseFloat64
+from ar_gripper_interfaces.srv import SetHoldingTorque
 from control_msgs.action import GripperCommand
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
 from rcl_interfaces.msg import ParameterDescriptor
@@ -44,7 +44,7 @@ def __init__(self, device, gripper_name, servo_id, servo_position_path, node):
             Empty, f"~/{gripper_name}/calibrate", self._handle_calibrate_srv
         )
         self._set_holding_torque = self._node.create_service(
-            UseFloat64,
+            SetHoldingTorque,
             f"~/{gripper_name}/set_holding_torque",
             self._handle_set_holding_torque,
         )
@@ -95,17 +95,17 @@ def _handle_calibrate_srv(self, _request, response):
         return response
 
     def _handle_set_holding_torque(self, request, response):
-        if request.arg < 0 or request.arg >= self.gripper.OVERLOAD_TORQUE:
+        if request.torque < 0 or request.torque >= self.gripper.OVERLOAD_TORQUE:
             response.success = False
             response.msg = (
-                f"Max holding torque {request.arg} must be between 0 and "
+                f"Max holding torque {request.torque} must be between 0 and "
                 f"{self.gripper.OVERLOAD_TORQUE}"
             )
             self._node.get_logger().error(response.msg)
             return response
 
-        self._holding_torque = request.arg
-        self._node.get_logger().info(f"Set holding torque to {request.arg}")
+        self._holding_torque = request.torque
+        self._node.get_logger().info(f"Set holding torque to {request.torque}")
         response.success = True
         return response
 
diff --git a/ar_gripper/package.xml b/ar_gripper/package.xml
index d43b0d7..6209993 100644
--- a/ar_gripper/package.xml
+++ b/ar_gripper/package.xml
@@ -20,6 +20,7 @@
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>rviz2</exec_depend>
   <exec_depend>python_qt_binding</exec_depend>
+  <exec_depend>ar_gripper_interfaces</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/ar_gripper_interfaces/CMakeLists.txt b/ar_gripper_interfaces/CMakeLists.txt
new file mode 100644
index 0000000..a78e6f2
--- /dev/null
+++ b/ar_gripper_interfaces/CMakeLists.txt
@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(ar_gripper_interfaces)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+# Build ROS messages, services and actions
+set(srv_files
+  "srv/SetHoldingTorque.srv"
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  ${srv_files}
+)
+
+ament_package()
diff --git a/ar_gripper_interfaces/package.xml b/ar_gripper_interfaces/package.xml
new file mode 100644
index 0000000..14a0519
--- /dev/null
+++ b/ar_gripper_interfaces/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ar_gripper_interfaces</name>
+  <version>0.1.0</version>
+  <description>ROS interfaces for the ARgripper driver</description>
+  <maintainer email="alex@machinekoder.com">Alexander Rössler</maintainer>
+  <license>BSD</license>
+
+  <url>TODO</url>
+  <author>Ruddick Lawrence</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/ar_gripper_interfaces/srv/SetHoldingTorque.srv b/ar_gripper_interfaces/srv/SetHoldingTorque.srv
new file mode 100644
index 0000000..57291b0
--- /dev/null
+++ b/ar_gripper_interfaces/srv/SetHoldingTorque.srv
@@ -0,0 +1,4 @@
+float64 torque
+---
+bool success
+string message # informational, e.g. for error messages
\ No newline at end of file