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

added python interface additional type hint and misc

Browse Source
This commit is contained in:
qlin960618
2025-07-05 18:09:59 +09:00
parent 6ee5e85a64
commit da362ebec1
1 changed files with 25 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
sas_robot_driver_franka/__init__.py
View File

@@ -22,7 +22,7 @@ class FrankaGripperInterface:
Non blocking interface to control the Franka gripper 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.last_message = None
self.robot_prefix = robot_prefix self.robot_prefix = robot_prefix
self.node = node self.node = node
@@ -45,7 +45,7 @@ class FrankaGripperInterface:
def _default_spin_handler(self): def _default_spin_handler(self):
rclpy.spin_once(self.node, timeout_sec=0.01) 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: try:
# self.node.get_logger().info("Waiting for service: " + service.service_name) # self.node.get_logger().info("Waiting for service: " + service.service_name)
ret = service.wait_for_service(timeout_sec=0.1) 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)) self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e))
return False return False
def is_enabled(self): def is_enabled(self) -> bool:
if self.state_width is None: if self.state_width is None:
return False return False
if not self._is_service_ready(self.move_service): if not self._is_service_ready(self.move_service):
@@ -69,7 +69,10 @@ class FrankaGripperInterface:
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): #############################################################################
# Public methods to control the gripper (actions)
#############################################################################
def home(self) -> rclpy.Future:
""" """
Reinitialize the gripper Reinitialize the gripper
:return: None :return: None
@@ -78,7 +81,7 @@ class FrankaGripperInterface:
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._home() self._home()
def move(self, width, speed=0): def move(self, width, speed=0) -> rclpy.Future:
""" """
Move the gripper to a specific width Move the gripper to a specific width
:param width: width in meters :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") raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
return 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) -> rclpy.Future:
""" """
Grasp an object with the gripper Grasp an object with the gripper
:param width: :param width:
@@ -103,30 +106,36 @@ class FrankaGripperInterface:
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")
return self._grasp(width, force, speed, epsilon_inner, epsilon_outer) 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 """ """ Get the maximum width of the gripper """
if not self.is_enabled(): if not self.is_enabled():
raise Exception("Gripper is not enabled") raise Exception("Gripper is not enabled")
return self.state_max_width return self.state_max_width
def get_width(self): def get_width(self) -> float:
""" Get the current width of the gripper """ """ Get the current width of the gripper """
if not self.is_enabled(): if not self.is_enabled():
raise Exception("Gripper is not enabled") raise Exception("Gripper is not enabled")
return self.state_width return self.state_width
def get_temperature(self): def get_temperature(self) -> float:
""" Get the temperature of the gripper """ """ Get the temperature of the gripper """
if not self.is_enabled(): if not self.is_enabled():
raise Exception("Gripper is not enabled") raise Exception("Gripper is not enabled")
return self.state_temperature return self.state_temperature
def is_grasped(self): def is_grasped(self) -> bool:
""" Check if an object is grasped """ """ Check if an object is grasped """
if not self.is_enabled(): if not self.is_enabled():
raise Exception("Gripper is not enabled") raise Exception("Gripper is not enabled")
return self.state_is_grasped return self.state_is_grasped
#############################################################################
# relating to action state
#############################################################################
def is_moving(self): def is_moving(self):
""" Check if the gripper is currently moving """ """ Check if the gripper is currently moving """
return self._current_action == "moving" return self._current_action == "moving"
@@ -156,6 +165,9 @@ class FrankaGripperInterface:
else: else:
return True return True
##############################################################################
# Public methods to get the result of the last action
##############################################################################
def get_result(self): def get_result(self):
""" """
Get the result of the last action Get the result of the last action
@@ -198,7 +210,7 @@ class FrankaGripperInterface:
rate.sleep() rate.sleep()
return self.get_result() return self.get_result()
def _home(self): def _home(self) -> rclpy.Future:
self._current_action = "homing" self._current_action = "homing"
# self.node.get_logger().info("Homing gripper") # self.node.get_logger().info("Homing gripper")
request = Trigger.Request() request = Trigger.Request()
@@ -206,7 +218,7 @@ class FrankaGripperInterface:
self.service_future = self.home_service.call_async(request) self.service_future = self.home_service.call_async(request)
return self.service_future return self.service_future
def _move(self, width, speed): def _move(self, width, speed) -> rclpy.Future:
self._current_action = "moving" 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()
@@ -216,7 +228,7 @@ class FrankaGripperInterface:
self.service_future = self.move_service.call_async(request) self.service_future = self.move_service.call_async(request)
return self.service_future 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._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()