72 lines
2.0 KiB
Python
72 lines
2.0 KiB
Python
import threading
|
|
|
|
import rclpy
|
|
from sas_robot_driver_franka import FrankaGripperInterface
|
|
import time
|
|
|
|
|
|
def main_loop(node):
|
|
iteration_ = 0
|
|
node_name = node.get_name()
|
|
hand1 = FrankaGripperInterface(node, "/franka_hand_1")
|
|
logger = rclpy.node.get_logger(node_name)
|
|
|
|
while not hand1.is_enabled():
|
|
logger.info("Waiting for gripper to be enabled...")
|
|
time.sleep(0.1)
|
|
rclpy.node.get_logger(node_name).info("Gripper enabled!")
|
|
|
|
def spin_all(node_):
|
|
while rclpy.ok():
|
|
rclpy.spin_once(node_, timeout_sec=0.1)
|
|
time.sleep(0.1)
|
|
|
|
thread = threading.Thread(target=spin_all, args=(node, ), daemon=True)
|
|
thread.start()
|
|
|
|
rate = node.create_rate(2)
|
|
|
|
while rclpy.ok():
|
|
logger.info("Main loop running...")
|
|
|
|
# Get the temperature of the gripper
|
|
temperature = hand1.get_temperature()
|
|
logger.info(f"Temperature: {temperature}")
|
|
max_width = hand1.get_max_width()
|
|
logger.info(f"Max width: {max_width}")
|
|
width = hand1.get_width()
|
|
logger.info(f"Width: {width}")
|
|
is_grasped = hand1.is_grasped()
|
|
logger.info(f"Is grasped: {is_grasped}")
|
|
is_moving = hand1.is_moving()
|
|
logger.info(f"Is moving: {is_moving}")
|
|
is_grasping = hand1.is_grasping()
|
|
logger.info(f"Is grasping: {is_grasping}")
|
|
logger.warn("calling move(0.01)")
|
|
if not hand1.is_busy():
|
|
hand1.grasp(0.01)
|
|
else:
|
|
logger.warn("Gripper is busy. Waiting...")
|
|
result_ready = hand1.is_done()
|
|
if not result_ready:
|
|
logger.info("Waiting for gripper to finish moving...")
|
|
else:
|
|
result = hand1.get_result()
|
|
logger.info(f"Result: {result}")
|
|
|
|
|
|
# Check if there is a response in the queue
|
|
|
|
iteration_ += 1
|
|
rate.sleep()
|
|
|
|
rclpy.shutdown()
|
|
thread.join()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
rclpy.init()
|
|
node_name_ = "example_gripper_control_node"
|
|
NODE = rclpy.create_node(node_name_)
|
|
main_loop(NODE)
|