From 101c45c2284bcee9e24dd2a17a3eaeda5a2c7d64 Mon Sep 17 00:00:00 2001 From: qlin1806 Date: Mon, 22 Jul 2024 05:25:28 -0700 Subject: [PATCH] added gripper driver for service control --- CMakeLists.txt | 36 ++- cfg/sas_robot_driver_franka_hand_1.yaml | 6 +- include/Grasp.h | 123 ++++++++ include/GraspRequest.h | 235 ++++++++++++++ include/GraspResponse.h | 195 ++++++++++++ include/GripperState.h | 235 ++++++++++++++ include/Move.h | 123 ++++++++ include/MoveRequest.h | 205 ++++++++++++ include/MoveResponse.h | 195 ++++++++++++ launch/sas_robot_driver_franka_hand_1.launch | 7 - msg/GripperState.msg | 5 + package.xml | 2 + src/hand/qros_effector_driver_franka_hand.cpp | 292 ++++++++++++++++++ src/hand/qros_effector_driver_franka_hand.h | 125 ++++++++ src/hand/sas_robot_driver_franka_hand.cpp | 152 --------- src/hand/sas_robot_driver_franka_hand.h | 117 ------- src/sas_robot_driver_franka_hand_node.cpp | 46 +-- srv/Grasp.srv | 7 + srv/Move.srv | 4 + 19 files changed, 1805 insertions(+), 305 deletions(-) create mode 100644 include/Grasp.h create mode 100644 include/GraspRequest.h create mode 100644 include/GraspResponse.h create mode 100644 include/GripperState.h create mode 100644 include/Move.h create mode 100644 include/MoveRequest.h create mode 100644 include/MoveResponse.h create mode 100644 msg/GripperState.msg create mode 100644 src/hand/qros_effector_driver_franka_hand.cpp create mode 100644 src/hand/qros_effector_driver_franka_hand.h delete mode 100644 src/hand/sas_robot_driver_franka_hand.cpp delete mode 100644 src/hand/sas_robot_driver_franka_hand.h create mode 100644 srv/Grasp.srv create mode 100644 srv/Move.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index d1184a9..8a21bce 100644 --- a/CMakeLists.txt +++ b/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) -target_link_libraries(sas_robot_driver_franka_hand +add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp) +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() diff --git a/cfg/sas_robot_driver_franka_hand_1.yaml b/cfg/sas_robot_driver_franka_hand_1.yaml index 1596e6b..2e572a2 100644 --- a/cfg/sas_robot_driver_franka_hand_1.yaml +++ b/cfg/sas_robot_driver_franka_hand_1.yaml @@ -1,7 +1,3 @@ "robot_ip_address": "172.16.0.2" -"robot_mode": "PositionControl" -"robot_speed": 20.0 -"thread_sampling_time_nsec": 40000000 -"q_min": [0.00] -"q_max": [0.04] +"thread_sampeling_time_ns": 100000000 diff --git a/include/Grasp.h b/include/Grasp.h new file mode 100644 index 0000000..138a0ef --- /dev/null +++ b/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 + + +#include +#include + + +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 diff --git a/include/GraspRequest.h b/include/GraspRequest.h new file mode 100644 index 0000000..4980bf1 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include + + +namespace sas_robot_driver_franka +{ +template +struct GraspRequest_ +{ + typedef GraspRequest_ 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_ > Ptr; + typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspRequest_ const> ConstPtr; + +}; // struct GraspRequest_ + +typedef ::sas_robot_driver_franka::GraspRequest_ > 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 +std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::GraspRequest_ & v) +{ +ros::message_operations::Printer< ::sas_robot_driver_franka::GraspRequest_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sas_robot_driver_franka::GraspRequest_ & lhs, const ::sas_robot_driver_franka::GraspRequest_ & 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 +bool operator!=(const ::sas_robot_driver_franka::GraspRequest_ & lhs, const ::sas_robot_driver_franka::GraspRequest_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sas_robot_driver_franka + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sas_robot_driver_franka::GraspRequest_ > + : TrueType + { }; + +template +struct IsMessage< ::sas_robot_driver_franka::GraspRequest_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sas_robot_driver_franka::GraspRequest_ > + : TrueType + { }; + +template +struct IsFixedSize< ::sas_robot_driver_franka::GraspRequest_ const> + : TrueType + { }; + +template +struct HasHeader< ::sas_robot_driver_franka::GraspRequest_ > + : FalseType + { }; + +template +struct HasHeader< ::sas_robot_driver_franka::GraspRequest_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sas_robot_driver_franka::GraspRequest_ > +{ + static const char* value() + { + return "337f46ba15e58c568d47e27881cf893c"; + } + + static const char* value(const ::sas_robot_driver_franka::GraspRequest_&) { return value(); } + static const uint64_t static_value1 = 0x337f46ba15e58c56ULL; + static const uint64_t static_value2 = 0x8d47e27881cf893cULL; +}; + +template +struct DataType< ::sas_robot_driver_franka::GraspRequest_ > +{ + static const char* value() + { + return "sas_robot_driver_franka/GraspRequest"; + } + + static const char* value(const ::sas_robot_driver_franka::GraspRequest_&) { return value(); } +}; + +template +struct Definition< ::sas_robot_driver_franka::GraspRequest_ > +{ + 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_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sas_robot_driver_franka::GraspRequest_ > + { + template 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 +struct Printer< ::sas_robot_driver_franka::GraspRequest_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::GraspRequest_& v) + { + s << indent << "width: "; + Printer::stream(s, indent + " ", v.width); + s << indent << "speed: "; + Printer::stream(s, indent + " ", v.speed); + s << indent << "force: "; + Printer::stream(s, indent + " ", v.force); + s << indent << "epsilon_inner: "; + Printer::stream(s, indent + " ", v.epsilon_inner); + s << indent << "epsilon_outer: "; + Printer::stream(s, indent + " ", v.epsilon_outer); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPREQUEST_H diff --git a/include/GraspResponse.h b/include/GraspResponse.h new file mode 100644 index 0000000..14732f7 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include + + +namespace sas_robot_driver_franka +{ +template +struct GraspResponse_ +{ + typedef GraspResponse_ 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_ > Ptr; + typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspResponse_ const> ConstPtr; + +}; // struct GraspResponse_ + +typedef ::sas_robot_driver_franka::GraspResponse_ > 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 +std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::GraspResponse_ & v) +{ +ros::message_operations::Printer< ::sas_robot_driver_franka::GraspResponse_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sas_robot_driver_franka::GraspResponse_ & lhs, const ::sas_robot_driver_franka::GraspResponse_ & rhs) +{ + return lhs.success == rhs.success; +} + +template +bool operator!=(const ::sas_robot_driver_franka::GraspResponse_ & lhs, const ::sas_robot_driver_franka::GraspResponse_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sas_robot_driver_franka + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sas_robot_driver_franka::GraspResponse_ > + : TrueType + { }; + +template +struct IsMessage< ::sas_robot_driver_franka::GraspResponse_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sas_robot_driver_franka::GraspResponse_ > + : TrueType + { }; + +template +struct IsFixedSize< ::sas_robot_driver_franka::GraspResponse_ const> + : TrueType + { }; + +template +struct HasHeader< ::sas_robot_driver_franka::GraspResponse_ > + : FalseType + { }; + +template +struct HasHeader< ::sas_robot_driver_franka::GraspResponse_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sas_robot_driver_franka::GraspResponse_ > +{ + static const char* value() + { + return "358e233cde0c8a8bcfea4ce193f8fc15"; + } + + static const char* value(const ::sas_robot_driver_franka::GraspResponse_&) { return value(); } + static const uint64_t static_value1 = 0x358e233cde0c8a8bULL; + static const uint64_t static_value2 = 0xcfea4ce193f8fc15ULL; +}; + +template +struct DataType< ::sas_robot_driver_franka::GraspResponse_ > +{ + static const char* value() + { + return "sas_robot_driver_franka/GraspResponse"; + } + + static const char* value(const ::sas_robot_driver_franka::GraspResponse_&) { return value(); } +}; + +template +struct Definition< ::sas_robot_driver_franka::GraspResponse_ > +{ + static const char* value() + { + return "bool success\n" +; + } + + static const char* value(const ::sas_robot_driver_franka::GraspResponse_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sas_robot_driver_franka::GraspResponse_ > + { + template 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 +struct Printer< ::sas_robot_driver_franka::GraspResponse_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::GraspResponse_& v) + { + s << indent << "success: "; + Printer::stream(s, indent + " ", v.success); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPRESPONSE_H diff --git a/include/GripperState.h b/include/GripperState.h new file mode 100644 index 0000000..79fa636 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include + + +namespace sas_robot_driver_franka +{ +template +struct GripperState_ +{ + typedef GripperState_ 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_ > Ptr; + typedef boost::shared_ptr< ::sas_robot_driver_franka::GripperState_ const> ConstPtr; + +}; // struct GripperState_ + +typedef ::sas_robot_driver_franka::GripperState_ > 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 +std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::GripperState_ & v) +{ +ros::message_operations::Printer< ::sas_robot_driver_franka::GripperState_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sas_robot_driver_franka::GripperState_ & lhs, const ::sas_robot_driver_franka::GripperState_ & 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 +bool operator!=(const ::sas_robot_driver_franka::GripperState_ & lhs, const ::sas_robot_driver_franka::GripperState_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sas_robot_driver_franka + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sas_robot_driver_franka::GripperState_ > + : TrueType + { }; + +template +struct IsMessage< ::sas_robot_driver_franka::GripperState_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sas_robot_driver_franka::GripperState_ > + : TrueType + { }; + +template +struct IsFixedSize< ::sas_robot_driver_franka::GripperState_ const> + : TrueType + { }; + +template +struct HasHeader< ::sas_robot_driver_franka::GripperState_ > + : FalseType + { }; + +template +struct HasHeader< ::sas_robot_driver_franka::GripperState_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sas_robot_driver_franka::GripperState_ > +{ + static const char* value() + { + return "53f8669159aaded70b1783087f07679d"; + } + + static const char* value(const ::sas_robot_driver_franka::GripperState_&) { return value(); } + static const uint64_t static_value1 = 0x53f8669159aaded7ULL; + static const uint64_t static_value2 = 0x0b1783087f07679dULL; +}; + +template +struct DataType< ::sas_robot_driver_franka::GripperState_ > +{ + static const char* value() + { + return "sas_robot_driver_franka/GripperState"; + } + + static const char* value(const ::sas_robot_driver_franka::GripperState_&) { return value(); } +}; + +template +struct Definition< ::sas_robot_driver_franka::GripperState_ > +{ + 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_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sas_robot_driver_franka::GripperState_ > + { + template 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 +struct Printer< ::sas_robot_driver_franka::GripperState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::GripperState_& v) + { + s << indent << "width: "; + Printer::stream(s, indent + " ", v.width); + s << indent << "max_width: "; + Printer::stream(s, indent + " ", v.max_width); + s << indent << "is_grasped: "; + Printer::stream(s, indent + " ", v.is_grasped); + s << indent << "temperature: "; + Printer::stream(s, indent + " ", v.temperature); + s << indent << "duration_ms: "; + Printer::stream(s, indent + " ", v.duration_ms); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRIPPERSTATE_H diff --git a/include/Move.h b/include/Move.h new file mode 100644 index 0000000..5a2a06d --- /dev/null +++ b/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 + + +#include +#include + + +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 diff --git a/include/MoveRequest.h b/include/MoveRequest.h new file mode 100644 index 0000000..6a30a44 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include + + +namespace sas_robot_driver_franka +{ +template +struct MoveRequest_ +{ + typedef MoveRequest_ 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_ > Ptr; + typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveRequest_ const> ConstPtr; + +}; // struct MoveRequest_ + +typedef ::sas_robot_driver_franka::MoveRequest_ > 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 +std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::MoveRequest_ & v) +{ +ros::message_operations::Printer< ::sas_robot_driver_franka::MoveRequest_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sas_robot_driver_franka::MoveRequest_ & lhs, const ::sas_robot_driver_franka::MoveRequest_ & rhs) +{ + return lhs.width == rhs.width && + lhs.speed == rhs.speed; +} + +template +bool operator!=(const ::sas_robot_driver_franka::MoveRequest_ & lhs, const ::sas_robot_driver_franka::MoveRequest_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sas_robot_driver_franka + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sas_robot_driver_franka::MoveRequest_ > + : TrueType + { }; + +template +struct IsMessage< ::sas_robot_driver_franka::MoveRequest_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sas_robot_driver_franka::MoveRequest_ > + : TrueType + { }; + +template +struct IsFixedSize< ::sas_robot_driver_franka::MoveRequest_ const> + : TrueType + { }; + +template +struct HasHeader< ::sas_robot_driver_franka::MoveRequest_ > + : FalseType + { }; + +template +struct HasHeader< ::sas_robot_driver_franka::MoveRequest_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sas_robot_driver_franka::MoveRequest_ > +{ + static const char* value() + { + return "b2d4f46fe020a06d64128c90310c767d"; + } + + static const char* value(const ::sas_robot_driver_franka::MoveRequest_&) { return value(); } + static const uint64_t static_value1 = 0xb2d4f46fe020a06dULL; + static const uint64_t static_value2 = 0x64128c90310c767dULL; +}; + +template +struct DataType< ::sas_robot_driver_franka::MoveRequest_ > +{ + static const char* value() + { + return "sas_robot_driver_franka/MoveRequest"; + } + + static const char* value(const ::sas_robot_driver_franka::MoveRequest_&) { return value(); } +}; + +template +struct Definition< ::sas_robot_driver_franka::MoveRequest_ > +{ + static const char* value() + { + return "float32 width\n" +"float32 speed\n" +; + } + + static const char* value(const ::sas_robot_driver_franka::MoveRequest_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sas_robot_driver_franka::MoveRequest_ > + { + template 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 +struct Printer< ::sas_robot_driver_franka::MoveRequest_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::MoveRequest_& v) + { + s << indent << "width: "; + Printer::stream(s, indent + " ", v.width); + s << indent << "speed: "; + Printer::stream(s, indent + " ", v.speed); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVEREQUEST_H diff --git a/include/MoveResponse.h b/include/MoveResponse.h new file mode 100644 index 0000000..a1f427d --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include + + +namespace sas_robot_driver_franka +{ +template +struct MoveResponse_ +{ + typedef MoveResponse_ 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_ > Ptr; + typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveResponse_ const> ConstPtr; + +}; // struct MoveResponse_ + +typedef ::sas_robot_driver_franka::MoveResponse_ > 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 +std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::MoveResponse_ & v) +{ +ros::message_operations::Printer< ::sas_robot_driver_franka::MoveResponse_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sas_robot_driver_franka::MoveResponse_ & lhs, const ::sas_robot_driver_franka::MoveResponse_ & rhs) +{ + return lhs.success == rhs.success; +} + +template +bool operator!=(const ::sas_robot_driver_franka::MoveResponse_ & lhs, const ::sas_robot_driver_franka::MoveResponse_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sas_robot_driver_franka + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sas_robot_driver_franka::MoveResponse_ > + : TrueType + { }; + +template +struct IsMessage< ::sas_robot_driver_franka::MoveResponse_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sas_robot_driver_franka::MoveResponse_ > + : TrueType + { }; + +template +struct IsFixedSize< ::sas_robot_driver_franka::MoveResponse_ const> + : TrueType + { }; + +template +struct HasHeader< ::sas_robot_driver_franka::MoveResponse_ > + : FalseType + { }; + +template +struct HasHeader< ::sas_robot_driver_franka::MoveResponse_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sas_robot_driver_franka::MoveResponse_ > +{ + static const char* value() + { + return "358e233cde0c8a8bcfea4ce193f8fc15"; + } + + static const char* value(const ::sas_robot_driver_franka::MoveResponse_&) { return value(); } + static const uint64_t static_value1 = 0x358e233cde0c8a8bULL; + static const uint64_t static_value2 = 0xcfea4ce193f8fc15ULL; +}; + +template +struct DataType< ::sas_robot_driver_franka::MoveResponse_ > +{ + static const char* value() + { + return "sas_robot_driver_franka/MoveResponse"; + } + + static const char* value(const ::sas_robot_driver_franka::MoveResponse_&) { return value(); } +}; + +template +struct Definition< ::sas_robot_driver_franka::MoveResponse_ > +{ + static const char* value() + { + return "bool success\n" +; + } + + static const char* value(const ::sas_robot_driver_franka::MoveResponse_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sas_robot_driver_franka::MoveResponse_ > + { + template 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 +struct Printer< ::sas_robot_driver_franka::MoveResponse_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::MoveResponse_& v) + { + s << indent << "success: "; + Printer::stream(s, indent + " ", v.success); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVERESPONSE_H diff --git a/launch/sas_robot_driver_franka_hand_1.launch b/launch/sas_robot_driver_franka_hand_1.launch index 0b2f33c..c2b065f 100644 --- a/launch/sas_robot_driver_franka_hand_1.launch +++ b/launch/sas_robot_driver_franka_hand_1.launch @@ -6,12 +6,5 @@ - - - - - - - diff --git a/msg/GripperState.msg b/msg/GripperState.msg new file mode 100644 index 0000000..dd916fa --- /dev/null +++ b/msg/GripperState.msg @@ -0,0 +1,5 @@ +float32 width +float32 max_width +bool is_grasped +uint16 temperature +uint64 duration_ms \ No newline at end of file diff --git a/package.xml b/package.xml index 0b00c45..0808ae8 100644 --- a/package.xml +++ b/package.xml @@ -60,6 +60,7 @@ sas_common sas_patient_side_manager pybind11_catkin + message_generation roscpp rospy @@ -82,6 +83,7 @@ sas_robot_driver sas_common sas_patient_side_manager + message_runtime diff --git a/src/hand/qros_effector_driver_franka_hand.cpp b/src/hand/qros_effector_driver_franka_hand.cpp new file mode 100644 index 0000000..ff275dd --- /dev/null +++ b/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 . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Quenitin Lin +# +# ################################################################ +*/ +#include "hand/qros_effector_driver_franka_hand.h" + +#include + + +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(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(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(gripper_state.width); + msg.max_width = static_cast(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 \ No newline at end of file diff --git a/src/hand/qros_effector_driver_franka_hand.h b/src/hand/qros_effector_driver_franka_hand.h new file mode 100644 index 0000000..3a7cd29 --- /dev/null +++ b/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 . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Quenitin Lin +# +# ################################################################ +*/ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// #define IN_TESTING + +// #include +#include +#include +#include +#include +#include +#include +#ifdef IN_TESTING +#include // dummy include +#include // dummy include +#include // 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 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 + diff --git a/src/hand/sas_robot_driver_franka_hand.cpp b/src/hand/sas_robot_driver_franka_hand.cpp deleted file mode 100644 index 5492425..0000000 --- a/src/hand/sas_robot_driver_franka_hand.cpp +++ /dev/null @@ -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 . -# -# ################################################################ -# -# 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(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 \ No newline at end of file diff --git a/src/hand/sas_robot_driver_franka_hand.h b/src/hand/sas_robot_driver_franka_hand.h deleted file mode 100644 index 6c8c9cd..0000000 --- a/src/hand/sas_robot_driver_franka_hand.h +++ /dev/null @@ -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 . -# -# ################################################################ -# -# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com -# -# ################################################################ -# -# Contributors: -# 1. Quenitin Lin -# -# ################################################################ -*/ -#include -#include -#include -#include -#include -#include -#include -// #include - -#include - -#include -#include -#include -#include - -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 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 - diff --git a/src/sas_robot_driver_franka_hand_node.cpp b/src/sas_robot_driver_franka_hand_node.cpp index 232f9f8..b2fcd05 100644 --- a/src/sas_robot_driver_franka_hand_node.cpp +++ b/src/sas_robot_driver_franka_hand_node.cpp @@ -36,7 +36,7 @@ #include #include #include -#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 +void get_optional_parameter(const std::string &node_prefix, ros::NodeHandle &nh, const std::string ¶m_name, T ¶m) +{ + 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; - - 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( + qros::EffectorDriverFrankaHand franka_hand_driver( + effector_driver_provider_prefix, robot_driver_franka_hand_configuration, - robot_driver_ros_configuration, + nh, &kill_this_process ); try diff --git a/srv/Grasp.srv b/srv/Grasp.srv new file mode 100644 index 0000000..7e63640 --- /dev/null +++ b/srv/Grasp.srv @@ -0,0 +1,7 @@ +float32 width +float32 speed +float32 force +float32 epsilon_inner +float32 epsilon_outer +--- +bool success \ No newline at end of file diff --git a/srv/Move.srv b/srv/Move.srv new file mode 100644 index 0000000..bb592b1 --- /dev/null +++ b/srv/Move.srv @@ -0,0 +1,4 @@ +float32 width +float32 speed +--- +bool success \ No newline at end of file