From fd98ce22ef0fc077490f27f240bd86ddd689e1c1 Mon Sep 17 00:00:00 2001 From: qlin960618 Date: Wed, 18 Dec 2024 15:20:49 +0900 Subject: [PATCH] [sas_robot_driver_franka_hand_node] added gripper homing option to python module --- sas_robot_driver_franka/__init__.py | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/sas_robot_driver_franka/__init__.py b/sas_robot_driver_franka/__init__.py index 91e5636..50237d5 100644 --- a/sas_robot_driver_franka/__init__.py +++ b/sas_robot_driver_franka/__init__.py @@ -9,6 +9,7 @@ from rclpy.node import Node from rclpy.client import Client from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown from sas_robot_driver_franka_interfaces.srv import Move, Grasp +from std_srvs.srv import Trigger from sas_robot_driver_franka_interfaces.msg import GripperState import os import threading @@ -27,12 +28,15 @@ class FrankaGripperInterface: """ def __init__(self, node: Node, robot_prefix): + self.last_message = None self.robot_prefix = robot_prefix self.node = node self.move_service = node.create_client(Move, os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX)) self._moving = False self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX)) self._grasping = False + self.home_service = node.create_client(Trigger, os.path.join(robot_prefix, "homing")) + self._homing = False self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX), self._status_callback, 10) @@ -71,6 +75,14 @@ class FrankaGripperInterface: self.state_max_width = msg.max_width self.state_temperature = msg.temperature self.state_is_grasped = msg.is_grasped + def home(self): + """ + Reinitialize the gripper + :return: None + """ + if self.is_busy(): + raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished") + self._home() def move(self, width, speed=0): """ @@ -158,14 +170,19 @@ class FrankaGripperInterface: self._moving = False elif isinstance(response, Grasp.Response): self._grasping = False + elif isinstance(response, Trigger.Response): + self._homing = False else: raise Exception("Invalid response type") self.service_future = None + self.last_message = hasattr(response, "message") and response.message or "" return response.success else: raise Exception("No result available") else: raise Exception("No result available") + def get_last_message(self): + return self.last_message def wait_until_done(self): """ @@ -179,6 +196,12 @@ class FrankaGripperInterface: while not self.is_done(): self.spin_handler() time.sleep(0.01) + def _home(self): + self._homing = True + # self.node.get_logger().info("Homing gripper") + request = Trigger.Request() + # self.node.get_logger().info("Calling home service") + self.service_future = self.home_service.call_async(request) def _move(self, width, speed): self._moving = True