diff --git a/sas_robot_driver_franka/__init__.py b/sas_robot_driver_franka/__init__.py index 50237d5..adfa035 100644 --- a/sas_robot_driver_franka/__init__.py +++ b/sas_robot_driver_franka/__init__.py @@ -7,7 +7,7 @@ from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamics 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_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown from sas_robot_driver_franka_interfaces.srv import Move, Grasp from std_srvs.srv import Trigger from sas_robot_driver_franka_interfaces.msg import GripperState @@ -31,12 +31,10 @@ class FrankaGripperInterface: self.last_message = None self.robot_prefix = robot_prefix 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._moving = False 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._homing = False self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX), self._status_callback, 10) @@ -50,7 +48,7 @@ class FrankaGripperInterface: self.spin_handler = self._default_spin_handler 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): try: @@ -75,6 +73,7 @@ class FrankaGripperInterface: self.state_max_width = msg.max_width self.state_temperature = msg.temperature self.state_is_grasped = msg.is_grasped + def home(self): """ Reinitialize the gripper @@ -93,7 +92,7 @@ class FrankaGripperInterface: """ if self.is_busy(): 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): """ @@ -107,7 +106,7 @@ class FrankaGripperInterface: """ if self.is_busy(): 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): """ Get the maximum width of the gripper """ @@ -135,21 +134,25 @@ class FrankaGripperInterface: def is_moving(self): """ Check if the gripper is currently moving """ - return self._moving + return self._current_action == "moving" def is_grasping(self): """ 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): """ 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): """ Check if the gripper is done moving or grasping """ if not self.is_busy(): self.node.get_logger().warn("Gripper is not moving or grasping") - return False + return True else: if self.service_future is not None: if self.service_future.done(): @@ -167,11 +170,11 @@ class FrankaGripperInterface: if self.service_future.done(): response = self.service_future.result() if isinstance(response, Move.Response): - self._moving = False + self._current_action = None elif isinstance(response, Grasp.Response): - self._grasping = False + self._current_action = None elif isinstance(response, Trigger.Response): - self._homing = False + self._current_action = None else: raise Exception("Invalid response type") self.service_future = None @@ -181,6 +184,7 @@ class FrankaGripperInterface: raise Exception("No result available") else: raise Exception("No result available") + def get_last_message(self): return self.last_message @@ -189,31 +193,36 @@ class FrankaGripperInterface: Wait until the gripper is done moving or grasping :return: success """ + rate = self.node.create_rate(100) # 100 Hz if not self.is_enabled(): raise Exception("Gripper is not enabled") if not self.is_busy(): return while not self.is_done(): self.spin_handler() - time.sleep(0.01) + rate.sleep() + return self.get_result() + def _home(self): - self._homing = True + self._current_action = "homing" # self.node.get_logger().info("Homing gripper") request = Trigger.Request() # self.node.get_logger().info("Calling home service") self.service_future = self.home_service.call_async(request) + return self.service_future 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)) 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) + return self.service_future 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)) request = Grasp.Request() request.width = float(width) @@ -223,7 +232,7 @@ class FrankaGripperInterface: request.epsilon_outer = float(epsilon_outer) # self.node.get_logger().info("Calling grasp service") self.service_future = self.grasp_service.call_async(request) - + return self.service_future def set_spin_handler(self, spin_handler): self.spin_handler = spin_handler