added gripper driver for service control

CMakeLists.txt

@@ -25,12 +25,36 @@ find_package(catkin REQUIRED COMPONENTS
         sas_clock
         sas_robot_driver
         sas_patient_side_manager
+        message_generation
         pybind11_catkin
 )
 
+
+add_service_files(
+        DIRECTORY srv
+        FILES
+        Move.srv
+        Grasp.srv
+)
+
+add_message_files(
+        DIRECTORY msg
+        FILES
+        GripperState.msg
+)
+
+catkin_python_setup()
+
+generate_messages(
+        DEPENDENCIES
+        std_msgs
+
+)
+
+
 catkin_package(
         INCLUDE_DIRS include
-        CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 pybind11_catkin
+        CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 pybind11_catkin message_runtime std_msgs
 )
 
 find_package(Franka REQUIRED)
@@ -121,9 +145,10 @@ target_link_libraries(sas_robot_driver_franka
         dqrobotics-interface-json11
         robot_interface_franka)
 
-add_library(sas_robot_driver_franka_hand src/hand/sas_robot_driver_franka_hand.cpp)
+add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp)
-target_link_libraries(sas_robot_driver_franka_hand
+target_link_libraries(qros_effector_driver_franka_hand
         dqrobotics
+#        robot_interface_hand
         Franka::Franka)
 
 
@@ -134,6 +159,8 @@ target_link_libraries(sas_robot_driver_coppelia
 
 add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(sas_robot_driver_coppelia ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(qros_effector_driver_franka_hand ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 
 add_executable(sas_robot_driver_coppelia_node src/sas_robot_driver_coppelia_node.cpp)
 target_link_libraries(sas_robot_driver_coppelia_node
@@ -148,7 +175,7 @@ target_link_libraries(sas_robot_driver_franka_node
 
 add_executable(sas_robot_driver_franka_hand_node src/sas_robot_driver_franka_hand_node.cpp)
 target_link_libraries(sas_robot_driver_franka_hand_node
-        sas_robot_driver_franka_hand
+        qros_effector_driver_franka_hand
         ${catkin_LIBRARIES})
 
 
@@ -210,4 +237,3 @@ install(TARGETS _qros_robot_dynamic
         LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}
 )
 
-catkin_python_setup()

cfg/sas_robot_driver_franka_hand_1.yaml

@@ -1,7 +1,3 @@
 "robot_ip_address": "172.16.0.2"
-"robot_mode": "PositionControl"
+"thread_sampeling_time_ns": 100000000
-"robot_speed": 20.0
-"thread_sampling_time_nsec": 40000000
-"q_min": [0.00]
-"q_max": [0.04]
 

include/Grasp.h

@@ -0,0 +1,123 @@
+// Generated by gencpp from file sas_robot_driver_franka/Grasp.msg
+// DO NOT EDIT!
+
+
+#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASP_H
+#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASP_H
+
+#include <ros/service_traits.h>
+
+
+#include <sas_robot_driver_franka/GraspRequest.h>
+#include <sas_robot_driver_franka/GraspResponse.h>
+
+
+namespace sas_robot_driver_franka
+{
+
+struct Grasp
+{
+
+typedef GraspRequest Request;
+typedef GraspResponse Response;
+Request request;
+Response response;
+
+typedef Request RequestType;
+typedef Response ResponseType;
+
+}; // struct Grasp
+} // namespace sas_robot_driver_franka
+
+
+namespace ros
+{
+namespace service_traits
+{
+
+
+template<>
+struct MD5Sum< ::sas_robot_driver_franka::Grasp > {
+  static const char* value()
+  {
+    return "6752ec080e002a60682f31654d420583";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::Grasp&) { return value(); }
+};
+
+template<>
+struct DataType< ::sas_robot_driver_franka::Grasp > {
+  static const char* value()
+  {
+    return "sas_robot_driver_franka/Grasp";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::Grasp&) { return value(); }
+};
+
+
+// service_traits::MD5Sum< ::sas_robot_driver_franka::GraspRequest> should match
+// service_traits::MD5Sum< ::sas_robot_driver_franka::Grasp >
+template<>
+struct MD5Sum< ::sas_robot_driver_franka::GraspRequest>
+{
+  static const char* value()
+  {
+    return MD5Sum< ::sas_robot_driver_franka::Grasp >::value();
+  }
+  static const char* value(const ::sas_robot_driver_franka::GraspRequest&)
+  {
+    return value();
+  }
+};
+
+// service_traits::DataType< ::sas_robot_driver_franka::GraspRequest> should match
+// service_traits::DataType< ::sas_robot_driver_franka::Grasp >
+template<>
+struct DataType< ::sas_robot_driver_franka::GraspRequest>
+{
+  static const char* value()
+  {
+    return DataType< ::sas_robot_driver_franka::Grasp >::value();
+  }
+  static const char* value(const ::sas_robot_driver_franka::GraspRequest&)
+  {
+    return value();
+  }
+};
+
+// service_traits::MD5Sum< ::sas_robot_driver_franka::GraspResponse> should match
+// service_traits::MD5Sum< ::sas_robot_driver_franka::Grasp >
+template<>
+struct MD5Sum< ::sas_robot_driver_franka::GraspResponse>
+{
+  static const char* value()
+  {
+    return MD5Sum< ::sas_robot_driver_franka::Grasp >::value();
+  }
+  static const char* value(const ::sas_robot_driver_franka::GraspResponse&)
+  {
+    return value();
+  }
+};
+
+// service_traits::DataType< ::sas_robot_driver_franka::GraspResponse> should match
+// service_traits::DataType< ::sas_robot_driver_franka::Grasp >
+template<>
+struct DataType< ::sas_robot_driver_franka::GraspResponse>
+{
+  static const char* value()
+  {
+    return DataType< ::sas_robot_driver_franka::Grasp >::value();
+  }
+  static const char* value(const ::sas_robot_driver_franka::GraspResponse&)
+  {
+    return value();
+  }
+};
+
+} // namespace service_traits
+} // namespace ros
+
+#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASP_H

include/GraspRequest.h

@@ -0,0 +1,235 @@
+// Generated by gencpp from file sas_robot_driver_franka/GraspRequest.msg
+// DO NOT EDIT!
+
+
+#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPREQUEST_H
+#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPREQUEST_H
+
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+
+namespace sas_robot_driver_franka
+{
+template <class ContainerAllocator>
+struct GraspRequest_
+{
+  typedef GraspRequest_<ContainerAllocator> Type;
+
+  GraspRequest_()
+    : width(0.0)
+    , speed(0.0)
+    , force(0.0)
+    , epsilon_inner(0.0)
+    , epsilon_outer(0.0)  {
+    }
+  GraspRequest_(const ContainerAllocator& _alloc)
+    : width(0.0)
+    , speed(0.0)
+    , force(0.0)
+    , epsilon_inner(0.0)
+    , epsilon_outer(0.0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef float _width_type;
+  _width_type width;
+
+   typedef float _speed_type;
+  _speed_type speed;
+
+   typedef float _force_type;
+  _force_type force;
+
+   typedef float _epsilon_inner_type;
+  _epsilon_inner_type epsilon_inner;
+
+   typedef float _epsilon_outer_type;
+  _epsilon_outer_type epsilon_outer;
+
+
+
+
+
+  typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> const> ConstPtr;
+
+}; // struct GraspRequest_
+
+typedef ::sas_robot_driver_franka::GraspRequest_<std::allocator<void> > GraspRequest;
+
+typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspRequest > GraspRequestPtr;
+typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspRequest const> GraspRequestConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator==(const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator2> & rhs)
+{
+  return lhs.width == rhs.width &&
+    lhs.speed == rhs.speed &&
+    lhs.force == rhs.force &&
+    lhs.epsilon_inner == rhs.epsilon_inner &&
+    lhs.epsilon_outer == rhs.epsilon_outer;
+}
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator!=(const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator2> & rhs)
+{
+  return !(lhs == rhs);
+}
+
+
+} // namespace sas_robot_driver_franka
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+
+
+template <class ContainerAllocator>
+struct IsMessage< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "337f46ba15e58c568d47e27881cf893c";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0x337f46ba15e58c56ULL;
+  static const uint64_t static_value2 = 0x8d47e27881cf893cULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "sas_robot_driver_franka/GraspRequest";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "float32 width\n"
+"float32 speed\n"
+"float32 force\n"
+"float32 epsilon_inner\n"
+"float32 epsilon_outer\n"
+;
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.width);
+      stream.next(m.speed);
+      stream.next(m.force);
+      stream.next(m.epsilon_inner);
+      stream.next(m.epsilon_outer);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct GraspRequest_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator>& v)
+  {
+    s << indent << "width: ";
+    Printer<float>::stream(s, indent + "  ", v.width);
+    s << indent << "speed: ";
+    Printer<float>::stream(s, indent + "  ", v.speed);
+    s << indent << "force: ";
+    Printer<float>::stream(s, indent + "  ", v.force);
+    s << indent << "epsilon_inner: ";
+    Printer<float>::stream(s, indent + "  ", v.epsilon_inner);
+    s << indent << "epsilon_outer: ";
+    Printer<float>::stream(s, indent + "  ", v.epsilon_outer);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPREQUEST_H

include/GraspResponse.h

@@ -0,0 +1,195 @@
+// Generated by gencpp from file sas_robot_driver_franka/GraspResponse.msg
+// DO NOT EDIT!
+
+
+#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPRESPONSE_H
+#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPRESPONSE_H
+
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+
+namespace sas_robot_driver_franka
+{
+template <class ContainerAllocator>
+struct GraspResponse_
+{
+  typedef GraspResponse_<ContainerAllocator> Type;
+
+  GraspResponse_()
+    : success(false)  {
+    }
+  GraspResponse_(const ContainerAllocator& _alloc)
+    : success(false)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint8_t _success_type;
+  _success_type success;
+
+
+
+
+
+  typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> const> ConstPtr;
+
+}; // struct GraspResponse_
+
+typedef ::sas_robot_driver_franka::GraspResponse_<std::allocator<void> > GraspResponse;
+
+typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspResponse > GraspResponsePtr;
+typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspResponse const> GraspResponseConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator==(const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator2> & rhs)
+{
+  return lhs.success == rhs.success;
+}
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator!=(const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator2> & rhs)
+{
+  return !(lhs == rhs);
+}
+
+
+} // namespace sas_robot_driver_franka
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+
+
+template <class ContainerAllocator>
+struct IsMessage< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "358e233cde0c8a8bcfea4ce193f8fc15";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0x358e233cde0c8a8bULL;
+  static const uint64_t static_value2 = 0xcfea4ce193f8fc15ULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "sas_robot_driver_franka/GraspResponse";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "bool success\n"
+;
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.success);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct GraspResponse_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator>& v)
+  {
+    s << indent << "success: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.success);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPRESPONSE_H

include/GripperState.h

@@ -0,0 +1,235 @@
+// Generated by gencpp from file sas_robot_driver_franka/GripperState.msg
+// DO NOT EDIT!
+
+
+#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRIPPERSTATE_H
+#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRIPPERSTATE_H
+
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+
+namespace sas_robot_driver_franka
+{
+template <class ContainerAllocator>
+struct GripperState_
+{
+  typedef GripperState_<ContainerAllocator> Type;
+
+  GripperState_()
+    : width(0.0)
+    , max_width(0.0)
+    , is_grasped(false)
+    , temperature(0)
+    , duration_ms(0)  {
+    }
+  GripperState_(const ContainerAllocator& _alloc)
+    : width(0.0)
+    , max_width(0.0)
+    , is_grasped(false)
+    , temperature(0)
+    , duration_ms(0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef float _width_type;
+  _width_type width;
+
+   typedef float _max_width_type;
+  _max_width_type max_width;
+
+   typedef uint8_t _is_grasped_type;
+  _is_grasped_type is_grasped;
+
+   typedef uint16_t _temperature_type;
+  _temperature_type temperature;
+
+   typedef uint64_t _duration_ms_type;
+  _duration_ms_type duration_ms;
+
+
+
+
+
+  typedef boost::shared_ptr< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> const> ConstPtr;
+
+}; // struct GripperState_
+
+typedef ::sas_robot_driver_franka::GripperState_<std::allocator<void> > GripperState;
+
+typedef boost::shared_ptr< ::sas_robot_driver_franka::GripperState > GripperStatePtr;
+typedef boost::shared_ptr< ::sas_robot_driver_franka::GripperState const> GripperStateConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::GripperState_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator==(const ::sas_robot_driver_franka::GripperState_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GripperState_<ContainerAllocator2> & rhs)
+{
+  return lhs.width == rhs.width &&
+    lhs.max_width == rhs.max_width &&
+    lhs.is_grasped == rhs.is_grasped &&
+    lhs.temperature == rhs.temperature &&
+    lhs.duration_ms == rhs.duration_ms;
+}
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator!=(const ::sas_robot_driver_franka::GripperState_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GripperState_<ContainerAllocator2> & rhs)
+{
+  return !(lhs == rhs);
+}
+
+
+} // namespace sas_robot_driver_franka
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+
+
+template <class ContainerAllocator>
+struct IsMessage< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "53f8669159aaded70b1783087f07679d";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::GripperState_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0x53f8669159aaded7ULL;
+  static const uint64_t static_value2 = 0x0b1783087f07679dULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "sas_robot_driver_franka/GripperState";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::GripperState_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "float32 width\n"
+"float32 max_width\n"
+"bool is_grasped\n"
+"uint16 temperature\n"
+"uint64 duration_ms\n"
+;
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::GripperState_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.width);
+      stream.next(m.max_width);
+      stream.next(m.is_grasped);
+      stream.next(m.temperature);
+      stream.next(m.duration_ms);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct GripperState_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::GripperState_<ContainerAllocator>& v)
+  {
+    s << indent << "width: ";
+    Printer<float>::stream(s, indent + "  ", v.width);
+    s << indent << "max_width: ";
+    Printer<float>::stream(s, indent + "  ", v.max_width);
+    s << indent << "is_grasped: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.is_grasped);
+    s << indent << "temperature: ";
+    Printer<uint16_t>::stream(s, indent + "  ", v.temperature);
+    s << indent << "duration_ms: ";
+    Printer<uint64_t>::stream(s, indent + "  ", v.duration_ms);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRIPPERSTATE_H

include/Move.h

@@ -0,0 +1,123 @@
+// Generated by gencpp from file sas_robot_driver_franka/Move.msg
+// DO NOT EDIT!
+
+
+#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVE_H
+#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVE_H
+
+#include <ros/service_traits.h>
+
+
+#include <sas_robot_driver_franka/MoveRequest.h>
+#include <sas_robot_driver_franka/MoveResponse.h>
+
+
+namespace sas_robot_driver_franka
+{
+
+struct Move
+{
+
+typedef MoveRequest Request;
+typedef MoveResponse Response;
+Request request;
+Response response;
+
+typedef Request RequestType;
+typedef Response ResponseType;
+
+}; // struct Move
+} // namespace sas_robot_driver_franka
+
+
+namespace ros
+{
+namespace service_traits
+{
+
+
+template<>
+struct MD5Sum< ::sas_robot_driver_franka::Move > {
+  static const char* value()
+  {
+    return "73e650ba1526b28d9e3f1be7ee33a441";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::Move&) { return value(); }
+};
+
+template<>
+struct DataType< ::sas_robot_driver_franka::Move > {
+  static const char* value()
+  {
+    return "sas_robot_driver_franka/Move";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::Move&) { return value(); }
+};
+
+
+// service_traits::MD5Sum< ::sas_robot_driver_franka::MoveRequest> should match
+// service_traits::MD5Sum< ::sas_robot_driver_franka::Move >
+template<>
+struct MD5Sum< ::sas_robot_driver_franka::MoveRequest>
+{
+  static const char* value()
+  {
+    return MD5Sum< ::sas_robot_driver_franka::Move >::value();
+  }
+  static const char* value(const ::sas_robot_driver_franka::MoveRequest&)
+  {
+    return value();
+  }
+};
+
+// service_traits::DataType< ::sas_robot_driver_franka::MoveRequest> should match
+// service_traits::DataType< ::sas_robot_driver_franka::Move >
+template<>
+struct DataType< ::sas_robot_driver_franka::MoveRequest>
+{
+  static const char* value()
+  {
+    return DataType< ::sas_robot_driver_franka::Move >::value();
+  }
+  static const char* value(const ::sas_robot_driver_franka::MoveRequest&)
+  {
+    return value();
+  }
+};
+
+// service_traits::MD5Sum< ::sas_robot_driver_franka::MoveResponse> should match
+// service_traits::MD5Sum< ::sas_robot_driver_franka::Move >
+template<>
+struct MD5Sum< ::sas_robot_driver_franka::MoveResponse>
+{
+  static const char* value()
+  {
+    return MD5Sum< ::sas_robot_driver_franka::Move >::value();
+  }
+  static const char* value(const ::sas_robot_driver_franka::MoveResponse&)
+  {
+    return value();
+  }
+};
+
+// service_traits::DataType< ::sas_robot_driver_franka::MoveResponse> should match
+// service_traits::DataType< ::sas_robot_driver_franka::Move >
+template<>
+struct DataType< ::sas_robot_driver_franka::MoveResponse>
+{
+  static const char* value()
+  {
+    return DataType< ::sas_robot_driver_franka::Move >::value();
+  }
+  static const char* value(const ::sas_robot_driver_franka::MoveResponse&)
+  {
+    return value();
+  }
+};
+
+} // namespace service_traits
+} // namespace ros
+
+#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVE_H

include/MoveRequest.h

@@ -0,0 +1,205 @@
+// Generated by gencpp from file sas_robot_driver_franka/MoveRequest.msg
+// DO NOT EDIT!
+
+
+#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVEREQUEST_H
+#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVEREQUEST_H
+
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+
+namespace sas_robot_driver_franka
+{
+template <class ContainerAllocator>
+struct MoveRequest_
+{
+  typedef MoveRequest_<ContainerAllocator> Type;
+
+  MoveRequest_()
+    : width(0.0)
+    , speed(0.0)  {
+    }
+  MoveRequest_(const ContainerAllocator& _alloc)
+    : width(0.0)
+    , speed(0.0)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef float _width_type;
+  _width_type width;
+
+   typedef float _speed_type;
+  _speed_type speed;
+
+
+
+
+
+  typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> const> ConstPtr;
+
+}; // struct MoveRequest_
+
+typedef ::sas_robot_driver_franka::MoveRequest_<std::allocator<void> > MoveRequest;
+
+typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveRequest > MoveRequestPtr;
+typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveRequest const> MoveRequestConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator==(const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator2> & rhs)
+{
+  return lhs.width == rhs.width &&
+    lhs.speed == rhs.speed;
+}
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator!=(const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator2> & rhs)
+{
+  return !(lhs == rhs);
+}
+
+
+} // namespace sas_robot_driver_franka
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+
+
+template <class ContainerAllocator>
+struct IsMessage< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "b2d4f46fe020a06d64128c90310c767d";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0xb2d4f46fe020a06dULL;
+  static const uint64_t static_value2 = 0x64128c90310c767dULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "sas_robot_driver_franka/MoveRequest";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "float32 width\n"
+"float32 speed\n"
+;
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.width);
+      stream.next(m.speed);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct MoveRequest_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator>& v)
+  {
+    s << indent << "width: ";
+    Printer<float>::stream(s, indent + "  ", v.width);
+    s << indent << "speed: ";
+    Printer<float>::stream(s, indent + "  ", v.speed);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVEREQUEST_H

include/MoveResponse.h

@@ -0,0 +1,195 @@
+// Generated by gencpp from file sas_robot_driver_franka/MoveResponse.msg
+// DO NOT EDIT!
+
+
+#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVERESPONSE_H
+#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVERESPONSE_H
+
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+
+namespace sas_robot_driver_franka
+{
+template <class ContainerAllocator>
+struct MoveResponse_
+{
+  typedef MoveResponse_<ContainerAllocator> Type;
+
+  MoveResponse_()
+    : success(false)  {
+    }
+  MoveResponse_(const ContainerAllocator& _alloc)
+    : success(false)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint8_t _success_type;
+  _success_type success;
+
+
+
+
+
+  typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> const> ConstPtr;
+
+}; // struct MoveResponse_
+
+typedef ::sas_robot_driver_franka::MoveResponse_<std::allocator<void> > MoveResponse;
+
+typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveResponse > MoveResponsePtr;
+typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveResponse const> MoveResponseConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator==(const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator2> & rhs)
+{
+  return lhs.success == rhs.success;
+}
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator!=(const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator2> & rhs)
+{
+  return !(lhs == rhs);
+}
+
+
+} // namespace sas_robot_driver_franka
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+
+
+template <class ContainerAllocator>
+struct IsMessage< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "358e233cde0c8a8bcfea4ce193f8fc15";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0x358e233cde0c8a8bULL;
+  static const uint64_t static_value2 = 0xcfea4ce193f8fc15ULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "sas_robot_driver_franka/MoveResponse";
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "bool success\n"
+;
+  }
+
+  static const char* value(const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.success);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct MoveResponse_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator>& v)
+  {
+    s << indent << "success: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.success);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVERESPONSE_H

launch/sas_robot_driver_franka_hand_1.launch

@@ -6,12 +6,5 @@
 <rosparam file="$(find sas_robot_driver_franka)/cfg/sas_robot_driver_franka_hand_1.yaml" command="load"
 />
 
-
-
 </node>
-
-
-
-
-
 </launch>

msg/GripperState.msg

@@ -0,0 +1,5 @@
+float32 width
+float32 max_width
+bool is_grasped
+uint16 temperature
+uint64 duration_ms

package.xml

@@ -60,6 +60,7 @@
   <build_depend>sas_common</build_depend>
   <build_depend>sas_patient_side_manager</build_depend>
   <build_depend>pybind11_catkin</build_depend>
+  <build_depend>message_generation</build_depend>
 
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
@@ -82,6 +83,7 @@
   <exec_depend>sas_robot_driver</exec_depend>
   <exec_depend>sas_common</exec_depend>
   <exec_depend>sas_patient_side_manager</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/hand/qros_effector_driver_franka_hand.cpp

@@ -0,0 +1,292 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include "hand/qros_effector_driver_franka_hand.h"
+
+#include <franka/exception.h>
+
+
+namespace qros {
+
+    //const static int SLAVE_MODE_JOINT_CONTROL;
+    //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
+
+    EffectorDriverFrankaHand::~EffectorDriverFrankaHand()
+    {
+        if(_is_connected())
+        {
+            disconnect();
+        }
+    }
+
+    EffectorDriverFrankaHand::EffectorDriverFrankaHand(
+        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)
+    {
+        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<sas_robot_driver_franka::GripperState>(driver_node_prefix_+"/gripper_status", 1);
+    }
+
+    bool EffectorDriverFrankaHand::_is_connected() const
+    {
+#ifdef IN_TESTING
+        return true;
+#endif
+        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_))
+        {
+            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.");
+                break_loops_->store(true);
+                break;
+            }
+
+            clock.update_and_sleep();
+            ros::spinOnce();
+        }
+        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");
+        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.");
+
+    }
+    void EffectorDriverFrankaHand::disconnect() noexcept
+    {
+#ifdef IN_TESTING
+        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...");
+        gripper_sptr_->~Gripper();
+        gripper_sptr_ = nullptr;
+    }
+
+    /**
+     * @brief robot_driver_franka::gripper_homing
+     */
+    void EffectorDriverFrankaHand::gripper_homing()
+    {
+#ifdef IN_TESTING
+        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.");
+        auto ret = gripper_sptr_->homing();
+        if(!ret)
+        {
+            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.");
+        gripper_homing();
+        // start gripper status loop
+        status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
+
+    }
+
+    void EffectorDriverFrankaHand::deinitialize()
+    {
+#ifdef IN_TESTING
+        ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::deinitialize::In testing mode.");
+        return;
+#endif
+        if(_is_connected())
+        {
+            franka::GripperState gripper_state = gripper_sptr_->readOnce();
+            if(gripper_state.is_grasped)
+            {
+                gripper_sptr_->stop();
+            }
+        }
+    }
+
+
+    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));
+        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));
+        bool ret = false;
+        bool function_ret = true;
+        gripper_in_use_.lock();
+#ifdef IN_TESTING
+        ret = true;
+#else
+        try
+        {
+            ret = gripper_sptr_->grasp(req.width, speed, force, epsilon_inner, epsilon_outer);
+        }catch(franka::CommandException& e)
+        {
+            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::CommandException::" + e.what());
+            function_ret = false;
+        }catch(franka::NetworkException& e)
+        {
+            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::NetworkException::" + e.what());
+            function_ret = false;
+        }
+#endif
+        gripper_in_use_.unlock();
+        res.success = ret;
+        return function_ret;
+    }
+
+
+
+    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));
+        bool ret = false;
+        bool function_ret = true;
+        gripper_in_use_.lock();
+#ifdef IN_TESTING
+        ret = true;
+#else
+        try
+        {
+            ret = gripper_sptr_->move(req.width, speed);
+        }catch(franka::CommandException& e)
+        {
+            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::CommandException::" + e.what());
+            function_ret = false;
+        }catch(franka::NetworkException& e)
+        {
+            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::NetworkException::" + e.what());
+            function_ret = false;
+        }
+#endif
+        gripper_in_use_.unlock();
+        res.success = ret;
+        return function_ret;
+    }
+
+
+    void EffectorDriverFrankaHand::_gripper_status_loop()
+    {
+        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.");
+        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.");
+                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.");
+#endif
+                    gripper_in_use_.lock();
+                    should_unlock = true;
+                    franka::GripperState gripper_state = gripper_sptr_->readOnce();
+                    gripper_in_use_.unlock();
+                    sas_robot_driver_franka::GripperState msg;
+                    msg.width = static_cast<float>(gripper_state.width);
+                    msg.max_width = static_cast<float>(gripper_state.max_width);
+                    msg.is_grasped = gripper_state.is_grasped;
+                    msg.temperature = gripper_state.temperature;
+                    msg.duration_ms = gripper_state.time.toMSec();
+                    gripper_status_publisher_.publish(msg);
+                }catch(...)
+                {
+                    if (should_unlock) gripper_in_use_.unlock();
+                    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);
+                }
+
+                clock.update_and_sleep();
+            }
+            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;
+        }
+        catch (...)
+        {
+            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.");
+    }
+
+
+
+
+} // qros

src/hand/qros_effector_driver_franka_hand.h

@@ -0,0 +1,125 @@
+#pragma once
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include <exception>
+#include <tuple>
+#include <atomic>
+#include <vector>
+#include <memory>
+#include <franka/robot.h>
+#include <franka/gripper.h>
+#include <thread>
+#include <mutex>
+
+// #define IN_TESTING
+
+// #include <sas_robot_driver/sas_robot_driver.h>
+#include <sas_clock/sas_clock.h>
+#include <ros/ros.h>
+#include <ros/service.h>
+#include <sas_robot_driver_franka/Grasp.h>
+#include <sas_robot_driver_franka/Move.h>
+#include <sas_robot_driver_franka/GripperState.h>
+#ifdef IN_TESTING
+#include <Move.h> // dummy include
+#include <Grasp.h>  // dummy include
+#include <GripperState.h> // dummy include
+#endif
+
+
+// using namespace DQ_robotics;
+// using namespace Eigen;
+
+
+namespace qros {
+
+struct EffectorDriverFrankaHandConfiguration
+{
+    std::string robot_ip;
+    int thread_sampeling_time_ns = 1e8;  // 10Hz
+    double default_force = 3.0;
+    double default_speed = 0.1;
+    double default_epsilon_inner = 0.005;
+    double default_epsilon_outer = 0.005;
+};
+
+class EffectorDriverFrankaHand{
+private:
+    std::string driver_node_prefix_;
+    EffectorDriverFrankaHandConfiguration configuration_;
+    ros::NodeHandle& node_handel_;
+
+    std::shared_ptr<franka::Gripper> gripper_sptr_;
+
+
+    std::atomic_bool* break_loops_;
+
+    bool _is_connected() const;
+
+    // thread specific functions
+    void _gripper_status_loop();
+    std::thread status_loop_thread_;
+    std::atomic_bool status_loop_running_{false};
+    ros::Publisher gripper_status_publisher_;
+
+    std::mutex gripper_in_use_;
+    ros::ServiceServer grasp_server_;
+    ros::ServiceServer move_server_;
+
+public:
+
+    bool _grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req, sas_robot_driver_franka::Grasp::Response& res);
+
+    bool _move_srv_callback(sas_robot_driver_franka::Move::Request& req, sas_robot_driver_franka::Move::Response& res);
+
+    EffectorDriverFrankaHand(const EffectorDriverFrankaHand&)=delete;
+    EffectorDriverFrankaHand()=delete;
+    ~EffectorDriverFrankaHand();
+
+    EffectorDriverFrankaHand(
+        const std::string &driver_node_prefix,
+        const EffectorDriverFrankaHandConfiguration& configuration,
+        ros::NodeHandle& node_handel,
+        std::atomic_bool* break_loops);
+
+    void start_control_loop();
+
+    void gripper_homing();
+
+
+    void connect() ;
+    void disconnect() noexcept;
+
+    void initialize() ;
+    void deinitialize() ;
+};
+
+} // qros
+

src/hand/sas_robot_driver_franka_hand.cpp

@@ -1,152 +0,0 @@
-/*
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Quenitin Lin
-#
-# ################################################################
-*/
-#include "sas_robot_driver_franka_hand.h"
-
-namespace sas {
-
-    //const static int SLAVE_MODE_JOINT_CONTROL;
-    //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
-
-    RobotDriverFrankaHand::~RobotDriverFrankaHand()
-    {
-        if(_is_connected())
-        {
-            disconnect();
-        }
-    }
-
-    RobotDriverFrankaHand::RobotDriverFrankaHand(
-        const RobotDriverFrankaHandConfiguration& configuration,
-        const RobotDriverROSConfiguration& ros_configuration,
-        std::atomic_bool* break_loops):
-    configuration_(configuration), ros_configuration_(ros_configuration), break_loops_(break_loops)
-    {
-        gripper_sptr_ = nullptr;
-
-    }
-
-    bool RobotDriverFrankaHand::_is_connected() const
-    {
-        if(gripper_sptr_ == nullptr) return false;
-        if(!gripper_sptr_) return false;
-        else return true;
-    }
-
-    VectorXd RobotDriverFrankaHand::get_joint_positions()
-    {
-        return joint_positions_;
-
-    }
-    void RobotDriverFrankaHand::set_target_joint_positions(const VectorXd& desired_joint_positions_rad)
-    {
-
-    }
-
-    VectorXd RobotDriverFrankaHand::get_joint_velocities()
-    {
-        return VectorXd::Zero(1);
-    }
-    void RobotDriverFrankaHand::set_target_joint_velocities(const VectorXd& desired_joint_velocities)
-    {
-
-    }
-
-    VectorXd RobotDriverFrankaHand::get_joint_forces()
-    {
-        return VectorXd::Zero(1);
-    }
-
-    void RobotDriverFrankaHand::start_control_loop()
-    {
-
-        Clock clock = Clock(ros_configuration_.thread_sampling_time_nsec);
-        clock.init();
-        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Starting control loop.");
-        while(!(*break_loops_))
-        {
-            if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Robot is not connected.");
-
-
-            clock.update_and_sleep();
-        }
-        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Exiting control loop.");
-
-    }
-
-
-    void RobotDriverFrankaHand::connect()
-    {
-        gripper_sptr_ = std::make_shared<franka::Gripper>(configuration_.robot_ip);
-        if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::connect::Could not connect to the robot.");
-
-    }
-    void RobotDriverFrankaHand::disconnect() noexcept
-    {
-        ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::disconnecting...");
-        gripper_sptr_->~Gripper();
-        gripper_sptr_ = nullptr;
-    }
-
-    /**
-     * @brief robot_driver_franka::gripper_homing
-     */
-    void RobotDriverFrankaHand::gripper_homing()
-    {
-        if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Robot is not connected.");
-        auto ret = gripper_sptr_->homing();
-        if(!ret)
-        {
-            throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
-        }
-        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Gripper homed.");
-    }
-
-    void RobotDriverFrankaHand::initialize()
-    {
-        if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::initialize::Robot is not connected.");
-        gripper_homing();
-    }
-
-    void RobotDriverFrankaHand::deinitialize()
-    {
-        if(_is_connected())
-        {
-            franka::GripperState gripper_state = gripper_sptr_->readOnce();
-            if(gripper_state.is_grasped)
-            {
-                gripper_sptr_->stop();
-            }
-        }
-    }
-
-
-
-
-} // sas

src/hand/sas_robot_driver_franka_hand.h

@@ -1,117 +0,0 @@
-#pragma once
-/*
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Quenitin Lin
-#
-# ################################################################
-*/
-#include <exception>
-#include <tuple>
-#include <atomic>
-#include <vector>
-#include <memory>
-#include <franka/robot.h>
-#include <franka/gripper.h>
-// #include <thread>
-
-#include <dqrobotics/DQ.h>
-
-#include <sas_robot_driver/sas_robot_driver.h>
-#include <sas_robot_driver/sas_robot_driver_ros.h>
-#include <sas_clock/sas_clock.h>
-#include <ros/common.h>
-
-using namespace DQ_robotics;
-using namespace Eigen;
-
-
-namespace sas {
-
-struct RobotDriverFrankaHandConfiguration
-{
-    std::string robot_ip;
-
-};
-
-class RobotDriverFrankaHand{
-private:
-    RobotDriverFrankaHandConfiguration configuration_;
-    RobotDriverROSConfiguration ros_configuration_;
-
-    std::shared_ptr<franka::Gripper> gripper_sptr_;
-
-    //Joint positions
-    VectorXd joint_positions_;
-    //VectorXd joint_velocities_;
-    //VectorXd end_effector_pose_;
-
-
-    // std::thread control_loop_thread_;
-    std::atomic_bool* break_loops_;
-
-    bool _is_connected() const;
-
-public:
-    //const static int SLAVE_MODE_JOINT_CONTROL;
-    //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
-
-    RobotDriverFrankaHand(const RobotDriverFrankaHand&)=delete;
-    RobotDriverFrankaHand()=delete;
-    ~RobotDriverFrankaHand();
-
-    RobotDriverFrankaHand(
-        const RobotDriverFrankaHandConfiguration& configuration,
-        const RobotDriverROSConfiguration& ros_configuration,
-        std::atomic_bool* break_loops);
-
-    void start_control_loop();
-
-
-
-    VectorXd get_joint_positions();
-    void set_target_joint_positions(const VectorXd& desired_joint_positions_rad);
-
-    VectorXd get_joint_velocities();
-    void set_target_joint_velocities(const VectorXd& desired_joint_velocities);
-
-    VectorXd get_joint_forces();
-
-    void gripper_homing();
-
-
-    void connect() ;
-    void disconnect() noexcept;
-
-    void initialize() ;
-    void deinitialize() ;
-
-    //bool set_end_effector_pose_dq(const DQ& pose);
-    //DQ get_end_effector_pose_dq();
-
-};
-
-} // sas
-

src/sas_robot_driver_franka_hand_node.cpp

@@ -36,7 +36,7 @@
 #include <sas_common/sas_common.h>
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "sas_robot_driver_franka_hand.h"
+#include "hand/qros_effector_driver_franka_hand.h"
 
 
 /*********************************************
@@ -49,6 +49,22 @@ void sig_int_handler(int)
     kill_this_process = true;
 }
 
+
+
+template<typename T>
+void get_optional_parameter(const std::string &node_prefix, ros::NodeHandle &nh, const std::string &param_name, T &param)
+{
+    if(nh.hasParam(node_prefix + param_name))
+    {
+        sas::get_ros_param(nh,param_name,param);
+    }else
+    {
+        ROS_INFO_STREAM(ros::this_node::getName() + "::Parameter " + param_name + " not found. Using default value. " + std::to_string(param));
+    }
+
+}
+
+
 int main(int argc, char **argv)
 {
     if(signal(SIGINT, sig_int_handler) == SIG_ERR)
@@ -60,32 +76,24 @@ int main(int argc, char **argv)
     ROS_WARN("---------------------------Quentin Lin-------------------------------");
     ROS_WARN("=====================================================================");
     ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
+    const std::string& effector_driver_provider_prefix = ros::this_node::getName();
 
 
     ros::NodeHandle nh;
-    sas::RobotDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
+    qros::EffectorDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
 
     sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
 
+    get_optional_parameter(effector_driver_provider_prefix,nh,"/thread_sampling_time_nsec",robot_driver_franka_hand_configuration.thread_sampeling_time_ns);
+    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_force",robot_driver_franka_hand_configuration.default_force);
+    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_speed",robot_driver_franka_hand_configuration.default_speed);
+    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_epsilon_inner",robot_driver_franka_hand_configuration.default_epsilon_inner);
+    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_epsilon_outer",robot_driver_franka_hand_configuration.default_epsilon_outer);
 
-    sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
+    qros::EffectorDriverFrankaHand franka_hand_driver(
-
+        effector_driver_provider_prefix,
-    sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
-    bool q_lim_override = false;
-    if(nh.hasParam("q_min") || nh.hasParam("q_max"))
-    {
-        sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
-        sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
-        q_lim_override = true;
-    }else
-    {
-
-    }
-    robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
-
-    sas::RobotDriverFrankaHand franka_hand_driver(
         robot_driver_franka_hand_configuration,
-        robot_driver_ros_configuration,
+        nh,
         &kill_this_process
     );
     try

srv/Grasp.srv

@@ -0,0 +1,7 @@
+float32 width
+float32 speed
+float32 force
+float32 epsilon_inner
+float32 epsilon_outer
+---
+bool success

srv/Move.srv

@@ -0,0 +1,4 @@
+float32 width
+float32 speed
+---
+bool success