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

minor bug fix for franka hand again

Browse Source
This commit is contained in:
qlin960618
2024-08-18 21:40:42 +09:00
parent 27b89070aa
commit 0358b851df
1 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
sas_robot_driver_franka/__init__.py
View File

@@ -4,7 +4,7 @@ from sas_robot_driver_franka._qros_franka_robot_dynamic import RobotDynamicsClie
import rclpy import rclpy
from rclpy.node import Node from rclpy.node import Node
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, Move_Request, Move_Response, Grasp, Grasp_Request, Grasp_Response from sas_robot_driver_franka_interfaces.srv import Move, Move_Request, Move_Response, Grasp, Grasp_Request, Grasp_Response
from sas_robot_driver_franka_interfaces.msg import GripperState from sas_robot_driver_franka_interfaces.msg import GripperState
import os import os
@@ -40,6 +40,13 @@ class FrankaGripperInterface:
self.state_max_width = None self.state_max_width = None
self.state_temperature = None self.state_temperature = None
self.state_is_grasped = None self.state_is_grasped = None
self.spin_handler = self._default_spin_handler
def _default_spin_handler(self):
rclpy.spin_once(self.node)
def set_spin_handler(self, spin_handler):
self.spin_handler = spin_handler
def _is_service_ready(self, service): def _is_service_ready(self, service):
try: try:
@@ -162,7 +169,7 @@ class FrankaGripperInterface:
if not self._moving or self._grasping: if not self._moving or self._grasping:
raise Exception("Gripper is not moving or grasping") raise Exception("Gripper is not moving or grasping")
while self._moving or self._grasping: while self._moving or self._grasping:
rclcpp_spin_some(self.node) self.spin_handler()
time.sleep(0.1) time.sleep(0.1)
if not self.result_queue.empty(): if not self.result_queue.empty():
response = self.result_queue.get() response = self.result_queue.get()