qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added gripper driver for service control

Browse Source
This commit is contained in:
qlin1806
2024-07-22 05:25:28 -07:00
parent 25e88f56ed
commit 101c45c228
19 changed files with 1805 additions and 305 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
CMakeLists.txt
View File

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

6
cfg/sas_robot_driver_franka_hand_1.yaml
View File

@@ -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

123
include/Grasp.h Normal file
View File

@@ -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

235
include/GraspRequest.h Normal file
View File

@@ -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

195
include/GraspResponse.h Normal file
View File

@@ -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

235
include/GripperState.h Normal file
View File

@@ -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

123
include/Move.h Normal file
View File

@@ -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

205
include/MoveRequest.h Normal file
View File

@@ -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

195
include/MoveResponse.h Normal file
View File

@@ -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

7
launch/sas_robot_driver_franka_hand_1.launch
View File

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

5
msg/GripperState.msg Normal file
View File

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

2
package.xml
View File

@@ -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 -->

292
src/hand/qros_effector_driver_franka_hand.cpp Normal file
View File

@@ -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

125
src/hand/qros_effector_driver_franka_hand.h Normal file
View File

@@ -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

152
src/hand/sas_robot_driver_franka_hand.cpp
View File

@@ -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

117
src/hand/sas_robot_driver_franka_hand.h
View File

@@ -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

46
src/sas_robot_driver_franka_hand_node.cpp
View File

@@ -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;
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

7
srv/Grasp.srv Normal file
View File

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

4
srv/Move.srv Normal file
View File

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