added python interface

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()

src/hand/qros_effector_driver_franka_hand.cpp

@@ -32,8 +32,8 @@
 #include <franka/exception.h>
 
 
-namespace qros {
+namespace qros
-
+{
     //const static int SLAVE_MODE_JOINT_CONTROL;
     //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
 
@@ -56,9 +56,12 @@ namespace qros {
         break_loops_(break_loops)
     {
         gripper_sptr_ = nullptr;
-        grasp_server_ = node_handel_.advertiseService(driver_node_prefix_+"/grasp", &EffectorDriverFrankaHand::_grasp_srv_callback, this);
+        grasp_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/grasp",
-        move_server_ = node_handel_.advertiseService(driver_node_prefix_+"/move", &EffectorDriverFrankaHand::_move_srv_callback, this);
+                                                      &EffectorDriverFrankaHand::_grasp_srv_callback, this);
-        gripper_status_publisher_ = node_handel_.advertise<sas_robot_driver_franka::GripperState>(driver_node_prefix_+"/gripper_status", 1);
+        move_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/move",
+                                                     &EffectorDriverFrankaHand::_move_srv_callback, this);
+        gripper_status_publisher_ = node_handel_.advertise<sas_robot_driver_franka::GripperState>(
+            driver_node_prefix_ + "/gripper_status", 1);
     }
 
     bool EffectorDriverFrankaHand::_is_connected() const
@@ -72,19 +75,23 @@ namespace qros {
     }
 
 
-
     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.");
+        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_){
+            if (!status_loop_running_)
-                ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not 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<franka::Gripper>(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)
         {
-            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()
@@ -162,10 +177,14 @@ 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_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req.width));
+            "["+ 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;
@@ -174,12 +193,15 @@ namespace qros {
         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));
+        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,
-    bool EffectorDriverFrankaHand::_move_srv_callback(sas_robot_driver_franka::Move::Request& req, sas_robot_driver_franka::Move::Response& res)
+                                                      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.");
+                    ROS_WARN_STREAM(
-                    throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
+                        "["+ 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<float>(gripper_state.width);
                     msg.max_width = static_cast<float>(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);
@@ -273,20 +316,22 @@ namespace qros {
                 clock.update_and_sleep();
             }
             status_loop_running_ = false;
-        }catch (std::exception& e)
+        }
+        catch (std::exception& e)
         {
-            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e.what());
+            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.");
+            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

src/hand/qros_effector_driver_franka_hand.h

@@ -38,6 +38,7 @@
 #include <thread>
 #include <mutex>
 
+// #define BLOCK_READ_IN_USED
 // #define IN_TESTING
 
 // #include <sas_robot_driver/sas_robot_driver.h>

src/sas_robot_driver_franka/__init__.py

@@ -1,3 +1,4 @@
 """
 """
 from _qros_robot_dynamic import RobotDynamicsInterface, RobotDynamicsProvider
+from .franka_gripper_interface import FrankaGripperInterface

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)