From da362ebec11037526cd8983febcbd062a9620a82 Mon Sep 17 00:00:00 2001 From: qlin960618 Date: Sat, 5 Jul 2025 18:09:59 +0900 Subject: [PATCH] added python interface additional type hint and misc --- sas_robot_driver_franka/__init__.py | 38 +++++++++++++++++++---------- 1 file changed, 25 insertions(+), 13 deletions(-) diff --git a/sas_robot_driver_franka/__init__.py b/sas_robot_driver_franka/__init__.py index fc8cb4a..a384ca7 100644 --- a/sas_robot_driver_franka/__init__.py +++ b/sas_robot_driver_franka/__init__.py @@ -22,7 +22,7 @@ class FrankaGripperInterface: Non blocking interface to control the Franka gripper """ - def __init__(self, node: Node, robot_prefix): + def __init__(self, node: Node, robot_prefix: str): self.last_message = None self.robot_prefix = robot_prefix self.node = node @@ -45,7 +45,7 @@ class FrankaGripperInterface: def _default_spin_handler(self): rclpy.spin_once(self.node, timeout_sec=0.01) - def _is_service_ready(self, service: Client): + def _is_service_ready(self, service: Client) -> bool: try: # self.node.get_logger().info("Waiting for service: " + service.service_name) ret = service.wait_for_service(timeout_sec=0.1) @@ -54,7 +54,7 @@ class FrankaGripperInterface: self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e)) return False - def is_enabled(self): + def is_enabled(self) -> bool: if self.state_width is None: return False if not self._is_service_ready(self.move_service): @@ -69,7 +69,10 @@ class FrankaGripperInterface: self.state_temperature = msg.temperature self.state_is_grasped = msg.is_grasped - def home(self): + ############################################################################# + # Public methods to control the gripper (actions) + ############################################################################# + def home(self) -> rclpy.Future: """ Reinitialize the gripper :return: None @@ -78,7 +81,7 @@ class FrankaGripperInterface: raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished") self._home() - def move(self, width, speed=0): + def move(self, width, speed=0) -> rclpy.Future: """ Move the gripper to a specific width :param width: width in meters @@ -89,7 +92,7 @@ class FrankaGripperInterface: raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished") return self._move(width, speed) - def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0): + def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0) -> rclpy.Future: """ Grasp an object with the gripper :param width: @@ -103,30 +106,36 @@ class FrankaGripperInterface: raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished") return self._grasp(width, force, speed, epsilon_inner, epsilon_outer) - def get_max_width(self): + ############################################################################# + # Public methods to get the gripper state, from ros topics + ############################################################################# + def get_max_width(self) -> float: """ Get the maximum width of the gripper """ if not self.is_enabled(): raise Exception("Gripper is not enabled") return self.state_max_width - def get_width(self): + def get_width(self) -> float: """ Get the current width of the gripper """ if not self.is_enabled(): raise Exception("Gripper is not enabled") return self.state_width - def get_temperature(self): + def get_temperature(self) -> float: """ Get the temperature of the gripper """ if not self.is_enabled(): raise Exception("Gripper is not enabled") return self.state_temperature - def is_grasped(self): + def is_grasped(self) -> bool: """ Check if an object is grasped """ if not self.is_enabled(): raise Exception("Gripper is not enabled") return self.state_is_grasped + ############################################################################# + # relating to action state + ############################################################################# def is_moving(self): """ Check if the gripper is currently moving """ return self._current_action == "moving" @@ -156,6 +165,9 @@ class FrankaGripperInterface: else: return True + ############################################################################## + # Public methods to get the result of the last action + ############################################################################## def get_result(self): """ Get the result of the last action @@ -198,7 +210,7 @@ class FrankaGripperInterface: rate.sleep() return self.get_result() - def _home(self): + def _home(self) -> rclpy.Future: self._current_action = "homing" # self.node.get_logger().info("Homing gripper") request = Trigger.Request() @@ -206,7 +218,7 @@ class FrankaGripperInterface: self.service_future = self.home_service.call_async(request) return self.service_future - def _move(self, width, speed): + def _move(self, width, speed) -> rclpy.Future: self._current_action = "moving" # self.node.get_logger().info("Moving gripper to width: " + str(width) + " speed: " + str(speed)) request = Move.Request() @@ -216,7 +228,7 @@ class FrankaGripperInterface: self.service_future = self.move_service.call_async(request) return self.service_future - def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer): + def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer) -> rclpy.Future: self._current_action = "grasping" # self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed)) request = Grasp.Request()