franka_gripper_node.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Franka Emika GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #include <functional>
4 #include <mutex>
5 #include <string>
6 #include <thread>
7 #include <vector>
8 
9 #include <ros/init.h>
10 #include <ros/node_handle.h>
11 #include <ros/rate.h>
12 #include <ros/spinner.h>
13 #include <sensor_msgs/JointState.h>
14 
15 #include <franka/gripper_state.h>
17 
18 namespace {
19 
20 template <typename T_action, typename T_goal, typename T_result>
21 void handleErrors(actionlib::SimpleActionServer<T_action>* server,
22  std::function<bool(const T_goal&)> handler,
23  const T_goal& goal) {
24  T_result result;
25  try {
26  result.success = handler(goal);
27  server->setSucceeded(result);
28  } catch (const franka::Exception& ex) {
29  ROS_ERROR_STREAM("" << ex.what());
30  result.success = false;
31  result.error = ex.what();
32  server->setAborted(result);
33  }
34 }
35 
36 } // anonymous namespace
37 
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;
59 
60 int main(int argc, char** argv) {
61  ros::init(argc, argv, "franka_gripper_node");
62  ros::NodeHandle node_handle("~");
63  std::string robot_ip;
64  if (!node_handle.getParam("robot_ip", robot_ip)) {
65  ROS_ERROR("franka_gripper_node: Could not parse robot_ip parameter");
66  return -1;
67  }
68 
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);
72  }
73 
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)) {
79  ROS_INFO_STREAM("franka_gripper_node: Found default_grasp_epsilon "
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"];
83  }
84 
85  franka::Gripper gripper(robot_ip);
86 
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); };
91 
92  SimpleActionServer<HomingAction> homing_action_server(
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,
97  goal);
98  },
99  false);
100 
101  SimpleActionServer<StopAction> stop_action_server(
102  node_handle, "stop",
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);
106  },
107  false);
108 
109  SimpleActionServer<MoveAction> move_action_server(
110  node_handle, "move",
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);
114  },
115  false);
116 
117  SimpleActionServer<GraspAction> grasp_action_server(
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);
122  },
123  false);
124 
125  SimpleActionServer<GripperCommandAction> gripper_command_action_server(
126  node_handle, "gripper_action",
127  [=, &gripper, &gripper_command_action_server](auto&& goal) {
128  return gripperCommandExecuteCallback(gripper, default_grasp_epsilon, default_speed,
129  &gripper_command_action_server, goal);
130  },
131  false);
132 
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();
138 
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 "
142  << publish_rate);
143  }
144 
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!");
148  return -1;
149  }
150  if (joint_names.size() != 2) {
151  ROS_ERROR("franka_gripper_node: Got wrong number of joint_names!");
152  return -1;
153  }
154 
155  franka::GripperState gripper_state;
156  std::mutex gripper_state_mutex;
157  std::thread read_thread([&gripper_state, &gripper, &gripper_state_mutex]() {
158  ros::Rate read_rate(10);
159  while (ros::ok()) {
160  {
161  std::lock_guard<std::mutex> _(gripper_state_mutex);
162  updateGripperState(gripper, &gripper_state);
163  }
164  read_rate.sleep();
165  }
166  });
167 
168  ros::Publisher gripper_state_publisher =
169  node_handle.advertise<sensor_msgs::JointState>("joint_states", 1);
171  spinner.start();
172  ros::Rate rate(publish_rate);
173  while (ros::ok()) {
174  if (gripper_state_mutex.try_lock()) {
175  sensor_msgs::JointState joint_states;
176  joint_states.header.stamp = ros::Time::now();
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();
187  }
188  rate.sleep();
189  }
190  read_thread.join();
191  return 0;
192 }
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(""))
void spinner()
int main(int argc, char **argv)
ROSCPP_DECL bool ok()
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.
bool sleep()
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
static Time now()
#define ROS_ERROR_STREAM(args)
bool grasp(const franka::Gripper &gripper, const GraspGoalConstPtr &goal)
Calls the libfranka grasp service of the gripper.
#define ROS_ERROR(...)


franka_gripper
Author(s): Franka Emika GmbH
autogenerated on Fri Oct 23 2020 03:47:02