diff --git a/scripts/example_gripper_control.py b/scripts/example_gripper_control.py new file mode 100644 index 0000000..e287a0a --- /dev/null +++ b/scripts/example_gripper_control.py @@ -0,0 +1,56 @@ +import rospy + +from sas_robot_driver_franka import FrankaGripperInterface + + +def main_loop(): + rate = rospy.Rate(10) # 10 Hz + iteration_ = 0 + + hand1 = FrankaGripperInterface("/franka_hand_1") + + while not hand1.is_enabled(): + rospy.loginfo("Waiting for gripper to be enabled...") + rate.sleep() + rospy.loginfo("Gripper enabled!") + + rate = rospy.Rate(2) # 0.5 Hz + + while not rospy.is_shutdown(): + rospy.loginfo("Main loop running...") + + # Get the temperature of the gripper + temperature = hand1.get_temperature() + rospy.loginfo(f"Temperature: {temperature}") + max_width = hand1.get_max_width() + rospy.loginfo(f"Max width: {max_width}") + width = hand1.get_width() + rospy.loginfo(f"Width: {width}") + is_grasped = hand1.is_grasped() + rospy.loginfo(f"Is grasped: {is_grasped}") + is_moving = hand1.is_moving() + rospy.loginfo(f"Is moving: {is_moving}") + is_grasping = hand1.is_grasping() + rospy.loginfo(f"Is grasping: {is_grasping}") + rospy.logwarn("calling move(0.01)") + if not hand1.is_busy(): + hand1.grasp(0.01) + else: + rospy.logwarn("Gripper is busy. Waiting...") + result_ready = hand1.is_done() + if not result_ready: + rospy.loginfo("Waiting for gripper to finish moving...") + else: + result = hand1.get_result() + rospy.loginfo(f"Result: {result}") + + + # Check if there is a response in the queue + + iteration_ += 1 + rate.sleep() + + +if __name__ == "__main__": + rospy.init_node("example_gripper_control_node") + main_loop() diff --git a/src/hand/qros_effector_driver_franka_hand.cpp b/src/hand/qros_effector_driver_franka_hand.cpp index ff275dd..74e839c 100644 --- a/src/hand/qros_effector_driver_franka_hand.cpp +++ b/src/hand/qros_effector_driver_franka_hand.cpp @@ -32,33 +32,36 @@ #include -namespace qros { - +namespace qros +{ //const static int SLAVE_MODE_JOINT_CONTROL; //const static int SLAVE_MODE_END_EFFECTOR_CONTROL; EffectorDriverFrankaHand::~EffectorDriverFrankaHand() { - if(_is_connected()) + if (_is_connected()) { disconnect(); } } EffectorDriverFrankaHand::EffectorDriverFrankaHand( - const std::string &driver_node_prefix, + const std::string& driver_node_prefix, const EffectorDriverFrankaHandConfiguration& configuration, ros::NodeHandle& node_handel, std::atomic_bool* break_loops): - driver_node_prefix_(driver_node_prefix), - configuration_(configuration), - node_handel_(node_handel), - break_loops_(break_loops) + driver_node_prefix_(driver_node_prefix), + configuration_(configuration), + node_handel_(node_handel), + break_loops_(break_loops) { gripper_sptr_ = nullptr; - grasp_server_ = node_handel_.advertiseService(driver_node_prefix_+"/grasp", &EffectorDriverFrankaHand::_grasp_srv_callback, this); - move_server_ = node_handel_.advertiseService(driver_node_prefix_+"/move", &EffectorDriverFrankaHand::_move_srv_callback, this); - gripper_status_publisher_ = node_handel_.advertise(driver_node_prefix_+"/gripper_status", 1); + grasp_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/grasp", + &EffectorDriverFrankaHand::_grasp_srv_callback, this); + move_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/move", + &EffectorDriverFrankaHand::_move_srv_callback, this); + gripper_status_publisher_ = node_handel_.advertise( + driver_node_prefix_ + "/gripper_status", 1); } bool EffectorDriverFrankaHand::_is_connected() const @@ -66,25 +69,29 @@ namespace qros { #ifdef IN_TESTING return true; #endif - if(gripper_sptr_ == nullptr) return false; - if(!gripper_sptr_) return false; + if (gripper_sptr_ == nullptr) return false; + if (!gripper_sptr_) return false; else return true; } - void EffectorDriverFrankaHand::start_control_loop() { - sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns); clock.init(); - ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop."); - while(!(*break_loops_)) + ROS_INFO_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop."); + while (!(*break_loops_)) { - if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected."); + if (!_is_connected()) throw std::runtime_error( + "[" + ros::this_node::getName() + + "]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected."); - if(!status_loop_running_){ - ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running."); + if (!status_loop_running_) + { + ROS_WARN_STREAM( + "["+ ros::this_node::getName()+ + "]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running."); break_loops_->store(true); break; } @@ -92,25 +99,29 @@ namespace qros { clock.update_and_sleep(); ros::spinOnce(); } - ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop."); - + ROS_INFO_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop."); } void EffectorDriverFrankaHand::connect() { #ifdef IN_TESTING - ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY"); + ROS_WARN_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY"); return; #endif gripper_sptr_ = std::make_shared(configuration_.robot_ip); - if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::connect::Could not connect to the robot."); - + if (!_is_connected()) throw std::runtime_error( + "[" + ros::this_node::getName() + + "]::[EffectorDriverFrankaHand]::connect::Could not connect to the robot."); } + void EffectorDriverFrankaHand::disconnect() noexcept { #ifdef IN_TESTING - ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY"); + ROS_WARN_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY"); return; #endif ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnecting..."); @@ -124,25 +135,29 @@ namespace qros { void EffectorDriverFrankaHand::gripper_homing() { #ifdef IN_TESTING - ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::In testing mode."); + ROS_WARN_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::In testing mode."); return; #endif - if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected."); + if (!_is_connected()) throw std::runtime_error( + "[" + ros::this_node::getName() + "]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected."); auto ret = gripper_sptr_->homing(); - if(!ret) + if (!ret) { - throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper."); + throw std::runtime_error( + "[" + ros::this_node::getName() + + "]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper."); } ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed."); } void EffectorDriverFrankaHand::initialize() { - if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::initialize::Robot is not connected."); + if (!_is_connected()) throw std::runtime_error( + "[" + ros::this_node::getName() + "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected."); gripper_homing(); // start gripper status loop status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this); - } void EffectorDriverFrankaHand::deinitialize() @@ -151,10 +166,10 @@ namespace qros { ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::deinitialize::In testing mode."); return; #endif - if(_is_connected()) + if (_is_connected()) { franka::GripperState gripper_state = gripper_sptr_->readOnce(); - if(gripper_state.is_grasped) + if (gripper_state.is_grasped) { gripper_sptr_->stop(); } @@ -162,24 +177,31 @@ namespace qros { } - bool EffectorDriverFrankaHand::_grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req, sas_robot_driver_franka::Grasp::Response& res) + bool EffectorDriverFrankaHand::_grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req, + sas_robot_driver_franka::Grasp::Response& res) { - ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping..."); - ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req.width)); + ROS_INFO_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping..."); + ROS_INFO_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std:: + to_string(req.width)); auto force = req.force; auto speed = req.speed; auto epsilon_inner = req.epsilon_inner; auto epsilon_outer = req.epsilon_outer; - if(force <= 0.0) force = configuration_.default_force; - if(speed<= 0.0) speed = configuration_.default_speed; - if(epsilon_inner <= 0.0) epsilon_inner = configuration_.default_epsilon_inner; - if(epsilon_outer <= 0.0) epsilon_outer = configuration_.default_epsilon_outer; - ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed)); + if (force <= 0.0) force = configuration_.default_force; + if (speed <= 0.0) speed = configuration_.default_speed; + if (epsilon_inner <= 0.0) epsilon_inner = configuration_.default_epsilon_inner; + if (epsilon_outer <= 0.0) epsilon_outer = configuration_.default_epsilon_outer; + ROS_INFO_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std:: + to_string(force) + " speed: " + std::to_string(speed)); bool ret = false; bool function_ret = true; gripper_in_use_.lock(); #ifdef IN_TESTING ret = true; + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); #else try { @@ -200,18 +222,21 @@ namespace qros { } - - bool EffectorDriverFrankaHand::_move_srv_callback(sas_robot_driver_franka::Move::Request& req, sas_robot_driver_franka::Move::Response& res) + bool EffectorDriverFrankaHand::_move_srv_callback(sas_robot_driver_franka::Move::Request& req, + sas_robot_driver_franka::Move::Response& res) { ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Moving..."); auto speed = req.speed; if (speed <= 0.0) speed = configuration_.default_speed; - ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req.width)); + ROS_INFO_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string + (speed) + " Width: " + std::to_string(req.width)); bool ret = false; bool function_ret = true; gripper_in_use_.lock(); #ifdef IN_TESTING ret = true; + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); #else try { @@ -236,24 +261,36 @@ namespace qros { { status_loop_running_ = true; sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns); - ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop."); + ROS_INFO_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.") + ; clock.init(); try { while (status_loop_running_) { bool should_unlock = false; - if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected."); + if (!_is_connected()) throw std::runtime_error( + "[" + ros::this_node::getName() + + "]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected."); try { #ifdef IN_TESTING - ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode."); - throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode."); + ROS_WARN_STREAM( + "["+ ros::this_node::getName()+ + "]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode."); + throw std::runtime_error( + "[" + ros::this_node::getName() + + "]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode."); #endif +#ifdef BLOCK_READ_IN_USED gripper_in_use_.lock(); should_unlock = true; +#endif franka::GripperState gripper_state = gripper_sptr_->readOnce(); +#ifdef BLOCK_READ_IN_USED gripper_in_use_.unlock(); +#endif sas_robot_driver_franka::GripperState msg; msg.width = static_cast(gripper_state.width); msg.max_width = static_cast(gripper_state.max_width); @@ -261,10 +298,16 @@ namespace qros { msg.temperature = gripper_state.temperature; msg.duration_ms = gripper_state.time.toMSec(); gripper_status_publisher_.publish(msg); - }catch(...) + } + catch (...) { +#ifdef BLOCK_READ_IN_USED if (should_unlock) gripper_in_use_.unlock(); - ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable."); +#endif + ROS_INFO_STREAM( + "["+ ros::this_node::getName()+ + "]::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.") + ; sas_robot_driver_franka::GripperState msg; msg.width = 0.0; gripper_status_publisher_.publish(msg); @@ -272,21 +315,23 @@ namespace qros { clock.update_and_sleep(); } - status_loop_running_=false; - }catch (std::exception& e) + status_loop_running_ = false; + } + catch (std::exception& e) { - ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e.what()); - status_loop_running_=false; + ROS_ERROR_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e. + what()); + status_loop_running_ = false; } catch (...) { - ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception."); - status_loop_running_=false; + ROS_ERROR_STREAM( + "["+ ros::this_node::getName()+ + "]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception."); + status_loop_running_ = false; } - ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop."); + ROS_INFO_STREAM( + "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop."); } - - - - -} // qros \ No newline at end of file +} // qros diff --git a/src/hand/qros_effector_driver_franka_hand.h b/src/hand/qros_effector_driver_franka_hand.h index 3a7cd29..60f1518 100644 --- a/src/hand/qros_effector_driver_franka_hand.h +++ b/src/hand/qros_effector_driver_franka_hand.h @@ -38,6 +38,7 @@ #include #include +// #define BLOCK_READ_IN_USED // #define IN_TESTING // #include diff --git a/src/sas_robot_driver_franka/__init__.py b/src/sas_robot_driver_franka/__init__.py index 4da9f7e..6cd8034 100644 --- a/src/sas_robot_driver_franka/__init__.py +++ b/src/sas_robot_driver_franka/__init__.py @@ -1,3 +1,4 @@ """ """ from _qros_robot_dynamic import RobotDynamicsInterface, RobotDynamicsProvider +from .franka_gripper_interface import FrankaGripperInterface \ No newline at end of file diff --git a/src/sas_robot_driver_franka/franka_gripper_interface.py b/src/sas_robot_driver_franka/franka_gripper_interface.py new file mode 100644 index 0000000..ef83e8a --- /dev/null +++ b/src/sas_robot_driver_franka/franka_gripper_interface.py @@ -0,0 +1,182 @@ +import rospy +from sas_robot_driver_franka.srv import Move, MoveRequest, MoveResponse, Grasp, GraspRequest, GraspResponse +from sas_robot_driver_franka.msg import GripperState +import os +import threading +from queue import Queue +import struct + +MOVE_TOPIC_SUFFIX = "move" +GRASP_TOPIC_SUFFIX = "grasp" +STATUS_TOPIC_SUFFIX = "gripper_status" + + +class FrankaGripperInterface: + """ + Non blocking interface to control the Franka gripper + """ + + def __init__(self, robot_prefix): + self.robot_prefix = robot_prefix + self.move_service = rospy.ServiceProxy(os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX), Move) + self._moving = False + self.grasp_service = rospy.ServiceProxy(os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX), Grasp) + self._grasping = False + self.status_subscriber = rospy.Subscriber(os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX), GripperState, + self._status_callback) + + self.result_queue = Queue() + + # gripper state + self.state_width = None + self.state_max_width = None + self.state_temperature = None + self.state_is_grasped = None + + def _is_service_ready(self, service): + try: + rospy.wait_for_service(service, timeout=0.1) + return True + except rospy.ROSException: + return False + + def is_enabled(self): + if self.state_width is None: + return False + if not self._is_service_ready(self.move_service.resolved_name): + return False + if not self._is_service_ready(self.grasp_service.resolved_name): + return False + return True + + def _status_callback(self, msg: GripperState): + self.state_width = msg.width + self.state_max_width = msg.max_width + self.state_temperature = msg.temperature + self.state_is_grasped = msg.is_grasped + + def move(self, width, speed=0): + """ + Move the gripper to a specific width + :param width: width in meters + :param speed: speed in meters per second + :return: None + """ + 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() + + def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0): + """ + Grasp an object with the gripper + :param width: + :param force: + :param speed: + :param epsilon_inner: + :param epsilon_outer: + :return: + """ + 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() + + def get_max_width(self): + """ Get the maximum width of the gripper """ + if not self.is_enabled(): + raise Exception("Gripper is not enabled") + return self.state_max_width + + def get_width(self): + """ Get the current width of the gripper """ + if not self.is_enabled(): + raise Exception("Gripper is not enabled") + return self.state_width + + def get_temperature(self): + """ Get the temperature of the gripper """ + if not self.is_enabled(): + raise Exception("Gripper is not enabled") + return self.state_temperature + + def is_grasped(self): + """ Check if an object is grasped """ + if not self.is_enabled(): + raise Exception("Gripper is not enabled") + return self.state_is_grasped + + def is_moving(self): + """ Check if the gripper is currently moving """ + return self._moving + + def is_grasping(self): + """ Check if the gripper is currently grasping """ + return self._grasping + + def is_busy(self): + """ Check if the gripper is currently moving or grasping """ + return self._moving or self._grasping + + def is_done(self): + """ Check if the gripper is done moving or grasping """ + if not self.is_busy(): + rospy.logwarn("Gripper is not moving or grasping") + return False + else: + if self.result_queue.empty(): + return False + else: + return True + + def get_result(self): + """ + 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 + else: + raise Exception("No result available") + + def wait_until_done(self): + """ + Wait until the gripper is done moving or grasping + :return: success + """ + 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") + while self._moving or self._grasping: + rospy.sleep(0.1) + if not self.result_queue.empty(): + response = self.result_queue.get() + if isinstance(response, MoveResponse): + self._moving = False + elif isinstance(response, GraspResponse): + self._grasping = False + else: + raise Exception("Invalid response type") + break + return response.success + + def _move(self, width, speed): + self._moving = True + request = MoveRequest() + request.width = width + request.speed = speed + response = self.move_service(request) + self.result_queue.put(response) + + def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer): + self._grasping = True + request = GraspRequest() + request.width = width + request.force = force + request.speed = speed + request.epsilon_inner = epsilon_inner + request.epsilon_outer = epsilon_outer + response = self.grasp_service(request) + self.result_queue.put(response)