13 #include <sensor_msgs/JointState.h> 20 template <
typename T_action,
typename T_goal,
typename T_result>
22 std::function<
bool(
const T_goal&)> handler,
26 result.success = handler(goal);
30 result.success =
false;
31 result.error = ex.what();
39 using control_msgs::GripperCommandAction;
41 using franka_gripper::GraspAction;
42 using franka_gripper::GraspEpsilon;
43 using franka_gripper::GraspGoalConstPtr;
44 using franka_gripper::GraspResult;
47 using franka_gripper::HomingAction;
48 using franka_gripper::HomingGoalConstPtr;
49 using franka_gripper::HomingResult;
51 using franka_gripper::MoveAction;
52 using franka_gripper::MoveGoalConstPtr;
53 using franka_gripper::MoveResult;
55 using franka_gripper::StopAction;
56 using franka_gripper::StopGoalConstPtr;
57 using franka_gripper::StopResult;
60 int main(
int argc,
char** argv) {
61 ros::init(argc, argv,
"franka_gripper_node");
64 if (!node_handle.
getParam(
"robot_ip", robot_ip)) {
65 ROS_ERROR(
"franka_gripper_node: Could not parse robot_ip parameter");
69 double default_speed(0.1);
70 if (node_handle.
getParam(
"default_speed", default_speed)) {
71 ROS_INFO_STREAM(
"franka_gripper_node: Found default_speed " << default_speed);
74 GraspEpsilon default_grasp_epsilon;
75 default_grasp_epsilon.inner = 0.005;
76 default_grasp_epsilon.outer = 0.005;
77 std::map<std::string, double> epsilon_map;
78 if (node_handle.
getParam(
"default_grasp_epsilon", epsilon_map)) {
80 <<
"inner: " << epsilon_map[
"inner"] <<
", outer: " << epsilon_map[
"outer"]);
81 default_grasp_epsilon.inner = epsilon_map[
"inner"];
82 default_grasp_epsilon.outer = epsilon_map[
"outer"];
87 auto homing_handler = [&gripper](
auto&& goal) {
return homing(gripper, goal); };
88 auto stop_handler = [&gripper](
auto&& goal) {
return stop(gripper, goal); };
89 auto grasp_handler = [&gripper](
auto&& goal) {
return grasp(gripper, goal); };
90 auto move_handler = [&gripper](
auto&& goal) {
return move(gripper, goal); };
93 node_handle,
"homing",
94 [=, &homing_action_server](
auto&& goal) {
95 return handleErrors<franka_gripper::HomingAction, franka_gripper::HomingGoalConstPtr,
96 franka_gripper::HomingResult>(&homing_action_server, homing_handler,
103 [=, &stop_action_server](
auto&& goal) {
104 return handleErrors<franka_gripper::StopAction, franka_gripper::StopGoalConstPtr,
105 franka_gripper::StopResult>(&stop_action_server, stop_handler, goal);
111 [=, &move_action_server](
auto&& goal) {
112 return handleErrors<franka_gripper::MoveAction, franka_gripper::MoveGoalConstPtr,
113 franka_gripper::MoveResult>(&move_action_server, move_handler, goal);
118 node_handle,
"grasp",
119 [=, &grasp_action_server](
auto&& goal) {
120 return handleErrors<franka_gripper::GraspAction, franka_gripper::GraspGoalConstPtr,
121 franka_gripper::GraspResult>(&grasp_action_server, grasp_handler, goal);
126 node_handle,
"gripper_action",
127 [=, &gripper, &gripper_command_action_server](
auto&& goal) {
129 &gripper_command_action_server, goal);
133 homing_action_server.
start();
134 stop_action_server.start();
135 move_action_server.start();
136 grasp_action_server.start();
137 gripper_command_action_server.start();
139 double publish_rate(30.0);
140 if (!node_handle.
getParam(
"publish_rate", publish_rate)) {
141 ROS_INFO_STREAM(
"franka_gripper_node: Could not find parameter publish_rate. Defaulting to " 145 std::vector<std::string> joint_names;
146 if (!node_handle.
getParam(
"joint_names", joint_names)) {
147 ROS_ERROR(
"franka_gripper_node: Could not parse joint_names!");
150 if (joint_names.size() != 2) {
151 ROS_ERROR(
"franka_gripper_node: Got wrong number of joint_names!");
156 std::mutex gripper_state_mutex;
157 std::thread read_thread([&gripper_state, &gripper, &gripper_state_mutex]() {
161 std::lock_guard<std::mutex> _(gripper_state_mutex);
169 node_handle.
advertise<sensor_msgs::JointState>(
"joint_states", 1);
174 if (gripper_state_mutex.try_lock()) {
175 sensor_msgs::JointState joint_states;
177 joint_states.name.push_back(joint_names[0]);
178 joint_states.name.push_back(joint_names[1]);
179 joint_states.position.push_back(gripper_state.
width * 0.5);
180 joint_states.position.push_back(gripper_state.
width * 0.5);
181 joint_states.velocity.push_back(0.0);
182 joint_states.velocity.push_back(0.0);
183 joint_states.effort.push_back(0.0);
184 joint_states.effort.push_back(0.0);
185 gripper_state_publisher.publish(joint_states);
186 gripper_state_mutex.unlock();
bool move(const franka::Gripper &gripper, const MoveGoalConstPtr &goal)
Calls the libfranka move service of the gripper.
bool stop(const franka::Gripper &gripper, const StopGoalConstPtr &)
Calls the libfranka stop service of the gripper to stop applying force.
bool homing(const franka::Gripper &gripper, const HomingGoalConstPtr &)
Calls the libfranka homing service of the gripper.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
int main(int argc, char **argv)
void gripperCommandExecuteCallback(const franka::Gripper &gripper, const GraspEpsilon &grasp_epsilon, double default_speed, actionlib::SimpleActionServer< control_msgs::GripperCommandAction > *action_server, const control_msgs::GripperCommandGoalConstPtr &goal)
Wraps the execution of a gripper command action to catch exceptions and report results.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool updateGripperState(const franka::Gripper &gripper, franka::GripperState *state)
Reads a gripper state if possible.
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM(args)
bool grasp(const franka::Gripper &gripper, const GraspGoalConstPtr &goal)
Calls the libfranka grasp service of the gripper.