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

general bug fix

Browse Source
This commit is contained in:
qlin960618
2024-08-19 15:30:30 +09:00
parent 900f6aa2e1
commit 75e00b3111
5 changed files with 78 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

106
sas_robot_driver_franka/__init__.py
View File

@@ -1,11 +1,14 @@
"""
"""
from sas_robot_driver_franka._robot_dynamics import RobotDynamicsInterface, RobotDynamicsProvider
from typing import Union
from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamicsClient, RobotDynamicsServer
import rclpy
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, Move_Request, Move_Response, Grasp, Grasp_Request, Grasp_Response
from sas_robot_driver_franka_interfaces.srv import Move, Grasp
from sas_robot_driver_franka_interfaces.msg import GripperState
import os
import threading
@@ -31,21 +34,25 @@ class FrankaGripperInterface:
self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX))
self._grasping = False
self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
self._status_callback)
self._status_callback, 10)
self.result_queue = Queue()
self.service_future: Union[rclpy.Future, None] = None
# gripper state
self.state_width = None
self.state_max_width = None
self.state_temperature = None
self.state_is_grasped = None
self.spin_handler = self._default_spin_handler
def _is_service_ready(self, service):
def _default_spin_handler(self):
rclpy.spin_once(self.node)
def _is_service_ready(self, service: Client):
try:
self.node.get_logger().info("Waiting for service: " + service.service_name)
service.wait_for_service(timeout_sec=0.1)
return True
# self.node.get_logger().info("Waiting for service: " + service.service_name)
ret = service.wait_for_service(timeout_sec=0.1)
return ret
except Exception as e:
self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e))
return False
@@ -74,7 +81,7 @@ class FrankaGripperInterface:
"""
if self.is_busy():
raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
threading.Thread(target=self._move, args=(width, speed)).start()
self._move(width, speed)
def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
"""
@@ -88,7 +95,7 @@ class FrankaGripperInterface:
"""
if self.is_busy():
raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
threading.Thread(target=self._grasp, args=(width, force, speed, epsilon_inner, epsilon_outer)).start()
self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
def get_max_width(self):
""" Get the maximum width of the gripper """
@@ -124,7 +131,7 @@ class FrankaGripperInterface:
def is_busy(self):
""" Check if the gripper is currently moving or grasping """
return self._moving or self._grasping
return self._moving or self._grasping or self.service_future is not None
def is_done(self):
""" Check if the gripper is done moving or grasping """
@@ -132,7 +139,9 @@ class FrankaGripperInterface:
self.node.get_logger().warn("Gripper is not moving or grasping")
return False
else:
if self.result_queue.empty():
if self.service_future is not None:
if self.service_future.done():
return True
return False
else:
return True
@@ -142,11 +151,19 @@ class FrankaGripperInterface:
Get the result of the last action
:return:
"""
if not self.result_queue.empty():
response = self.result_queue.get()
self._moving = False
self._grasping = False
return response.success
if self.service_future is not None:
if self.service_future.done():
response = self.service_future.result()
if isinstance(response, Move.Response):
self._moving = False
elif isinstance(response, Grasp.Response):
self._grasping = False
else:
raise Exception("Invalid response type")
self.service_future = None
return response.success
else:
raise Exception("No result available")
else:
raise Exception("No result available")
@@ -157,40 +174,33 @@ class FrankaGripperInterface:
"""
if not self.is_enabled():
raise Exception("Gripper is not enabled")
if not self._moving and not self._grasping:
raise Exception("Gripper is not moving or grasping")
if not self._moving or self._grasping:
raise Exception("Gripper is not moving or grasping")
while self._moving or self._grasping:
rclcpp_spin_some(self.node)
time.sleep(0.1)
if not self.result_queue.empty():
response = self.result_queue.get()
if isinstance(response, Move_Response):
self._moving = False
elif isinstance(response, Grasp_Response):
self._grasping = False
else:
raise Exception("Invalid response type")
break
return response.success
if not self.is_busy():
return
while not self.is_done():
self.spin_handler()
time.sleep(0.01)
def _move(self, width, speed):
self._moving = True
request = Move_Request()
request.width = width
request.speed = speed
response = self.move_service.call(request)
self.result_queue.put(response)
# self.node.get_logger().info("Moving gripper to width: " + str(width) + " speed: " + str(speed))
request = Move.Request()
request.width = float(width)
request.speed = float(speed)
# self.node.get_logger().info("Calling move service")
self.service_future = self.move_service.call_async(request)
def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer):
self._grasping = True
request = Grasp_Request()
request.width = width
request.force = force
request.speed = speed
request.epsilon_inner = epsilon_inner
request.epsilon_outer = epsilon_outer
response = self.grasp_service.call(request)
self.result_queue.put(response)
# self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed))
request = Grasp.Request()
request.width = float(width)
request.force = float(force)
request.speed = float(speed)
request.epsilon_inner = float(epsilon_inner)
request.epsilon_outer = float(epsilon_outer)
# self.node.get_logger().info("Calling grasp service")
self.service_future = self.grasp_service.call_async(request)
def set_spin_handler(self, spin_handler):
self.spin_handler = spin_handler