qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

updated minor optimization and bug fix for python gripper interface

Browse Source
This commit is contained in:
qlin960618
2025-07-05 18:02:13 +09:00
parent a54ddb14ea
commit 89e25af2d2
1 changed files with 28 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
sas_robot_driver_franka/__init__.py
View File

@@ -7,7 +7,7 @@ from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamics
import rclpy import rclpy
from rclpy.node import Node from rclpy.node import Node
from rclpy.client import Client from rclpy.client import Client
from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown # from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
from sas_robot_driver_franka_interfaces.srv import Move, Grasp from sas_robot_driver_franka_interfaces.srv import Move, Grasp
from std_srvs.srv import Trigger from std_srvs.srv import Trigger
from sas_robot_driver_franka_interfaces.msg import GripperState from sas_robot_driver_franka_interfaces.msg import GripperState
@@ -31,12 +31,10 @@ class FrankaGripperInterface:
self.last_message = None self.last_message = None
self.robot_prefix = robot_prefix self.robot_prefix = robot_prefix
self.node = node self.node = node
self._current_action = None # between moving, grasping, and homing
self.move_service = node.create_client(Move, os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX)) 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.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.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_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
self._status_callback, 10) self._status_callback, 10)
@@ -50,7 +48,7 @@ class FrankaGripperInterface:
self.spin_handler = self._default_spin_handler self.spin_handler = self._default_spin_handler
def _default_spin_handler(self): def _default_spin_handler(self):
rclpy.spin_once(self.node) rclpy.spin_once(self.node, timeout_sec=0.01)
def _is_service_ready(self, service: Client): def _is_service_ready(self, service: Client):
try: try:
@@ -75,6 +73,7 @@ class FrankaGripperInterface:
self.state_max_width = msg.max_width self.state_max_width = msg.max_width
self.state_temperature = msg.temperature self.state_temperature = msg.temperature
self.state_is_grasped = msg.is_grasped self.state_is_grasped = msg.is_grasped
def home(self): def home(self):
""" """
Reinitialize the gripper Reinitialize the gripper
@@ -93,7 +92,7 @@ class FrankaGripperInterface:
""" """
if self.is_busy(): if self.is_busy():
raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished") raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
self._move(width, speed) 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):
""" """
@@ -107,7 +106,7 @@ class FrankaGripperInterface:
""" """
if self.is_busy(): if self.is_busy():
raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished") raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
self._grasp(width, force, speed, epsilon_inner, epsilon_outer) return self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
def get_max_width(self): def get_max_width(self):
""" Get the maximum width of the gripper """ """ Get the maximum width of the gripper """
@@ -135,21 +134,25 @@ class FrankaGripperInterface:
def is_moving(self): def is_moving(self):
""" Check if the gripper is currently moving """ """ Check if the gripper is currently moving """
return self._moving return self._current_action == "moving"
def is_grasping(self): def is_grasping(self):
""" Check if the gripper is currently grasping """ """ Check if the gripper is currently grasping """
return self._grasping return self._current_action == "grasping"
def is_homing(self):
""" Check if the gripper is currently homing """
return self._current_action == "homing"
def is_busy(self): def is_busy(self):
""" Check if the gripper is currently moving or grasping """ """ Check if the gripper is currently moving or grasping """
return self._moving or self._grasping or self.service_future is not None return self._current_action is not None
def is_done(self): def is_done(self):
""" Check if the gripper is done moving or grasping """ """ Check if the gripper is done moving or grasping """
if not self.is_busy(): if not self.is_busy():
self.node.get_logger().warn("Gripper is not moving or grasping") self.node.get_logger().warn("Gripper is not moving or grasping")
return False return True
else: else:
if self.service_future is not None: if self.service_future is not None:
if self.service_future.done(): if self.service_future.done():
@@ -167,11 +170,11 @@ class FrankaGripperInterface:
if self.service_future.done(): if self.service_future.done():
response = self.service_future.result() response = self.service_future.result()
if isinstance(response, Move.Response): if isinstance(response, Move.Response):
self._moving = False self._current_action = None
elif isinstance(response, Grasp.Response): elif isinstance(response, Grasp.Response):
self._grasping = False self._current_action = None
elif isinstance(response, Trigger.Response): elif isinstance(response, Trigger.Response):
self._homing = False self._current_action = None
else: else:
raise Exception("Invalid response type") raise Exception("Invalid response type")
self.service_future = None self.service_future = None
@@ -181,6 +184,7 @@ class FrankaGripperInterface:
raise Exception("No result available") raise Exception("No result available")
else: else:
raise Exception("No result available") raise Exception("No result available")
def get_last_message(self): def get_last_message(self):
return self.last_message return self.last_message
@@ -189,31 +193,36 @@ class FrankaGripperInterface:
Wait until the gripper is done moving or grasping Wait until the gripper is done moving or grasping
:return: success :return: success
""" """
rate = self.node.create_rate(100) # 100 Hz
if not self.is_enabled(): if not self.is_enabled():
raise Exception("Gripper is not enabled") raise Exception("Gripper is not enabled")
if not self.is_busy(): if not self.is_busy():
return return
while not self.is_done(): while not self.is_done():
self.spin_handler() self.spin_handler()
time.sleep(0.01) rate.sleep()
return self.get_result()
def _home(self): def _home(self):
self._homing = True self._current_action = "homing"
# self.node.get_logger().info("Homing gripper") # self.node.get_logger().info("Homing gripper")
request = Trigger.Request() request = Trigger.Request()
# self.node.get_logger().info("Calling home service") # self.node.get_logger().info("Calling home service")
self.service_future = self.home_service.call_async(request) self.service_future = self.home_service.call_async(request)
return self.service_future
def _move(self, width, speed): def _move(self, width, speed):
self._moving = True self._current_action = "moving"
# self.node.get_logger().info("Moving gripper to width: " + str(width) + " speed: " + str(speed)) # self.node.get_logger().info("Moving gripper to width: " + str(width) + " speed: " + str(speed))
request = Move.Request() request = Move.Request()
request.width = float(width) request.width = float(width)
request.speed = float(speed) request.speed = float(speed)
# self.node.get_logger().info("Calling move service") # self.node.get_logger().info("Calling move service")
self.service_future = self.move_service.call_async(request) 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):
self._grasping = True self._current_action = "grasping"
# self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed)) # self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed))
request = Grasp.Request() request = Grasp.Request()
request.width = float(width) request.width = float(width)
@@ -223,7 +232,7 @@ class FrankaGripperInterface:
request.epsilon_outer = float(epsilon_outer) request.epsilon_outer = float(epsilon_outer)
# self.node.get_logger().info("Calling grasp service") # self.node.get_logger().info("Calling grasp service")
self.service_future = self.grasp_service.call_async(request) self.service_future = self.grasp_service.call_async(request)
return self.service_future
def set_spin_handler(self, spin_handler): def set_spin_handler(self, spin_handler):
self.spin_handler = spin_handler self.spin_handler = spin_handler