original version

CMakeLists.txt

+cmake_minimum_required(VERSION 3.4)
+project(franka_gripper)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  message_generation
+  control_msgs
+  actionlib
+  sensor_msgs
+  xmlrpcpp
+  actionlib_msgs
+)
+
+find_package(Franka 0.7.0 REQUIRED)
+
+add_action_files(
+  DIRECTORY action
+  FILES Grasp.action
+        Homing.action
+        Stop.action
+        Move.action
+)
+
+add_message_files(
+  DIRECTORY msg
+  FILES GraspEpsilon.msg
+)
+
+generate_messages(DEPENDENCIES actionlib_msgs)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES franka_gripper
+  CATKIN_DEPENDS roscpp
+                 message_runtime
+                 control_msgs
+                 actionlib
+                 sensor_msgs
+                 xmlrpcpp
+                 actionlib_msgs
+  DEPENDS Franka
+)
+
+add_library(franka_gripper
+  src/franka_gripper.cpp
+)
+
+add_dependencies(franka_gripper
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  ${PROJECT_NAME}_generate_messages_cpp
+)
+
+target_link_libraries(franka_gripper
+  ${Franka_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+target_include_directories(franka_gripper SYSTEM PUBLIC
+  ${Franka_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+target_include_directories(franka_gripper PUBLIC
+  include
+)
+
+add_executable(franka_gripper_node
+  src/franka_gripper_node.cpp
+)
+
+add_dependencies(franka_gripper_node
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  franka_gripper
+)
+
+target_link_libraries(franka_gripper_node
+   ${catkin_LIBRARIES}
+   franka_gripper
+)
+
+target_include_directories(franka_gripper_node SYSTEM PUBLIC
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Installation
+install(TARGETS franka_gripper
+                franka_gripper_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+install(DIRECTORY config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+## Tools
+include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
+  RESULT_VARIABLE CLANG_TOOLS
+)
+if(CLANG_TOOLS)
+  file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
+  file(GLOB_RECURSE HEADERS
+    ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
+  )
+  add_format_target(franka_gripper FILES ${SOURCES} ${HEADERS})
+  add_tidy_target(franka_gripper
+    FILES ${SOURCES}
+    DEPENDS franka_gripper franka_gripper_node
+  )
+endif()

action/Grasp.action

+float64 width # [m]
+GraspEpsilon epsilon
+float64 speed # [m/s]
+float64 force # [N]
+---
+bool success
+string error
+---

action/Homing.action

+---
+bool success
+string error
+---

action/Move.action

+float64 width  # [m]
+float64 speed  # [m/s]
+---
+bool success
+string error
+---

action/Stop.action

+---
+bool success
+string error
+---

config/franka_gripper_node.yaml

+publish_rate: 30  # [Hz]
+default_speed: 0.1  # [m/s]
+default_grasp_epsilon:
+  inner: 0.005 # [m]
+  outer: 0.005 # [m]

include/franka_gripper/franka_gripper.h

+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <cmath>
+#include <string>
+
+#include <actionlib/server/simple_action_server.h>
+#include <control_msgs/GripperCommandAction.h>
+#include <ros/node_handle.h>
+
+#include <franka/exception.h>
+#include <franka/gripper.h>
+#include <franka/gripper_state.h>
+#include <franka_gripper/GraspAction.h>
+#include <franka_gripper/HomingAction.h>
+#include <franka_gripper/MoveAction.h>
+#include <franka_gripper/StopAction.h>
+
+namespace franka_gripper {
+
+/**
+ * Reads a gripper state if possible
+ *
+ * @param[in] state A gripper state to update
+ * @param[in] gripper A pointer to a franka gripper
+ *
+ * @return True if update was successful, false otherwise.
+ */
+bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* state);
+
+/**
+ * Wraps the execution of a gripper command action to catch exceptions and
+ * report results
+ *
+ * @note
+ * For compatibility with current MoveIt! behavior, the given goal's command position is
+ * multiplied by a factor of 2 before being commanded to the gripper!
+ *
+ * @param[in] gripper A pointer to a franka gripper
+ * @param[in] default_speed The default speed for a gripper action
+ * @param[in] grasp_epsilon The epsilon window of the grasp.
+ * @param[in] action_server A pointer to a gripper action server
+ * @param[in] goal A gripper action goal
+ */
+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);
+
+/**
+ * Calls the libfranka move service of the gripper
+ *
+ * @param[in] gripper A gripper instance to execute the command
+ * @param[in] goal A move goal with target width and velocity
+ *
+ * @return True if command was successful, false otherwise.
+ */
+bool move(const franka::Gripper& gripper, const MoveGoalConstPtr& goal);
+
+/**
+ * Calls the libfranka homing service of the gripper
+ *
+ * @param[in] gripper A gripper instance to execute the command
+ *
+ * @return True if command was successful, false otherwise.
+ */
+bool homing(const franka::Gripper& gripper, const HomingGoalConstPtr& /*goal*/);
+
+/**
+ * Calls the libfranka stop service of the gripper to stop applying force
+ *
+ * @param[in] gripper A gripper instance to execute the command
+ *
+ * @return True if command was successful, false otherwise.
+ */
+bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/);
+
+/**
+ * Calls the libfranka grasp service of the gripper
+ *
+ * An object is considered grasped if the distance \f$d\f$ between the gripper fingers satisfies
+ * \f$(\text{width} - \text{epsilon_inner}) < d < (\text{width} + \text{epsilon_outer})\f$.
+ *
+ * @param[in] gripper A gripper instance to execute the command
+ * @param[in] goal A grasp goal with target width, epsilon_inner, epsilon_outer, velocity and effort
+ * @return True if an object has been grasped, false otherwise.
+ */
+bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal);
+
+}  // namespace franka_gripper

launch/franka_gripper.launch

+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_ip" />
+  <arg name="arm_id"      default="panda" />
+  <arg name="joint_names" default="[$(arg arm_id)_finger_joint1, $(arg arm_id)_finger_joint2]" />
+
+  <node name="franka_gripper" pkg="franka_gripper" type="franka_gripper_node" output="screen">
+    <param name="robot_ip" value="$(arg robot_ip)"/>
+    <rosparam command="load" file="$(find franka_gripper)/config/franka_gripper_node.yaml" />
+    <rosparam param="joint_names" subst_value="true">$(arg joint_names)</rosparam>
+  </node>
+
+</launch>

mainpage.dox

+/**
+ * @mainpage
+ * @htmlinclude "manifest.html"
+ *
+ * Overview page for Franka Emika research robots: https://frankaemika.github.io
+ */

msg/GraspEpsilon.msg

+float64 inner # [m]
+float64 outer # [m]

package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>franka_gripper</name>
+  <version>0.7.0</version>
+  <description>This package implements the franka gripper of type Franka Hand for the use in ros</description>
+  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
+  <license>Apache 2.0</license>
+
+  <url type="website">http://wiki.ros.org/franka_gripper</url>
+  <url type="repository">https://github.com/frankaemika/franka_ros</url>
+  <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
+  <author>Franka Emika GmbH</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>message_generation</depend>
+  <depend>libfranka</depend>
+  <depend>control_msgs</depend>
+  <depend>actionlib</depend>
+  <depend>sensor_msgs</depend>
+  <depend>xmlrpcpp</depend>
+  <depend>actionlib_msgs</depend>
+
+  <exec_depend>message_runtime</exec_depend>
+
+  <build_export_depend>message_runtime</build_export_depend>
+</package>

rosdoc.yaml

+- builder: doxygen
+  javadoc_autobrief: YES

src/franka_gripper.cpp

+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_gripper/franka_gripper.h>
+
+#include <cmath>
+#include <functional>
+#include <thread>
+
+#include <ros/node_handle.h>
+
+#include <franka/exception.h>
+#include <franka/gripper_state.h>
+#include <franka_gripper/GraspAction.h>
+#include <franka_gripper/HomingAction.h>
+#include <franka_gripper/MoveAction.h>
+#include <franka_gripper/StopAction.h>
+
+namespace franka_gripper {
+
+bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* state) {
+  try {
+    *state = gripper.readOnce();
+  } catch (const franka::Exception& ex) {
+    ROS_ERROR_STREAM("GripperServer: Exception reading gripper state: " << ex.what());
+    return false;
+  }
+  return true;
+}
+
+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) {
+  auto gripper_command_handler = [goal, grasp_epsilon, default_speed, &gripper]() {
+    // HACK: As one gripper finger is <mimic>, MoveIt!'s trajectory execution manager
+    // only sends us the width of one finger. Multiply by 2 to get the intended width.
+    double target_width = 2 * goal->command.position;
+
+    franka::GripperState state = gripper.readOnce();
+    if (target_width > state.max_width || target_width < 0.0) {
+      ROS_ERROR_STREAM("GripperServer: Commanding out of range width! max_width = "
+                       << state.max_width << " command = " << target_width);
+      return false;
+    }
+    constexpr double kSamePositionThreshold = 1e-4;
+    if (std::abs(target_width - state.width) < kSamePositionThreshold) {
+      return true;
+    }
+    if (target_width >= state.width) {
+      return gripper.move(target_width, default_speed);
+    }
+    return gripper.grasp(target_width, default_speed, goal->command.max_effort, grasp_epsilon.inner,
+                         grasp_epsilon.outer);
+  };
+
+  try {
+    if (gripper_command_handler()) {
+      franka::GripperState state;
+      if (updateGripperState(gripper, &state)) {
+        control_msgs::GripperCommandResult result;
+        result.effort = 0.0;
+        result.position = state.width;
+        result.reached_goal = static_cast<decltype(result.reached_goal)>(true);
+        result.stalled = static_cast<decltype(result.stalled)>(false);
+        action_server->setSucceeded(result);
+        return;
+      }
+    }
+  } catch (const franka::Exception& ex) {
+    ROS_ERROR_STREAM("" << ex.what());
+  }
+  action_server->setAborted();
+}
+
+bool move(const franka::Gripper& gripper, const MoveGoalConstPtr& goal) {
+  return gripper.move(goal->width, goal->speed);
+}
+
+bool homing(const franka::Gripper& gripper, const HomingGoalConstPtr& /*goal*/) {
+  return gripper.homing();
+}
+
+bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/) {
+  return gripper.stop();
+}
+
+bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal) {
+  return gripper.grasp(goal->width, goal->speed, goal->force, goal->epsilon.inner,
+                       goal->epsilon.outer);
+}
+
+}  // namespace franka_gripper

src/franka_gripper_node.cpp

+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <functional>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <ros/init.h>
+#include <ros/node_handle.h>
+#include <ros/rate.h>
+#include <ros/spinner.h>
+#include <sensor_msgs/JointState.h>
+
+#include <franka/gripper_state.h>
+#include <franka_gripper/franka_gripper.h>
+
+namespace {
+
+template <typename T_action, typename T_goal, typename T_result>
+void handleErrors(actionlib::SimpleActionServer<T_action>* server,
+                  std::function<bool(const T_goal&)> handler,
+                  const T_goal& goal) {
+  T_result result;
+  try {
+    result.success = handler(goal);
+    server->setSucceeded(result);
+  } catch (const franka::Exception& ex) {
+    ROS_ERROR_STREAM("" << ex.what());
+    result.success = false;
+    result.error = ex.what();
+    server->setAborted(result);
+  }
+}
+
+}  // anonymous namespace
+
+using actionlib::SimpleActionServer;
+using control_msgs::GripperCommandAction;
+using franka_gripper::grasp;
+using franka_gripper::GraspAction;
+using franka_gripper::GraspEpsilon;
+using franka_gripper::GraspGoalConstPtr;
+using franka_gripper::GraspResult;
+using franka_gripper::gripperCommandExecuteCallback;
+using franka_gripper::homing;
+using franka_gripper::HomingAction;
+using franka_gripper::HomingGoalConstPtr;
+using franka_gripper::HomingResult;
+using franka_gripper::move;
+using franka_gripper::MoveAction;
+using franka_gripper::MoveGoalConstPtr;
+using franka_gripper::MoveResult;
+using franka_gripper::stop;
+using franka_gripper::StopAction;
+using franka_gripper::StopGoalConstPtr;
+using franka_gripper::StopResult;
+using franka_gripper::updateGripperState;
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "franka_gripper_node");
+  ros::NodeHandle node_handle("~");
+  std::string robot_ip;
+  if (!node_handle.getParam("robot_ip", robot_ip)) {
+    ROS_ERROR("franka_gripper_node: Could not parse robot_ip parameter");
+    return -1;
+  }
+
+  double default_speed(0.1);
+  if (node_handle.getParam("default_speed", default_speed)) {
+    ROS_INFO_STREAM("franka_gripper_node: Found default_speed " << default_speed);
+  }
+
+  GraspEpsilon default_grasp_epsilon;
+  default_grasp_epsilon.inner = 0.005;
+  default_grasp_epsilon.outer = 0.005;
+  std::map<std::string, double> epsilon_map;
+  if (node_handle.getParam("default_grasp_epsilon", epsilon_map)) {
+    ROS_INFO_STREAM("franka_gripper_node: Found default_grasp_epsilon "
+                    << "inner: " << epsilon_map["inner"] << ", outer: " << epsilon_map["outer"]);
+    default_grasp_epsilon.inner = epsilon_map["inner"];
+    default_grasp_epsilon.outer = epsilon_map["outer"];
+  }
+
+  franka::Gripper gripper(robot_ip);
+
+  auto homing_handler = [&gripper](auto&& goal) { return homing(gripper, goal); };
+  auto stop_handler = [&gripper](auto&& goal) { return stop(gripper, goal); };
+  auto grasp_handler = [&gripper](auto&& goal) { return grasp(gripper, goal); };
+  auto move_handler = [&gripper](auto&& goal) { return move(gripper, goal); };
+
+  SimpleActionServer<HomingAction> homing_action_server(
+      node_handle, "homing",
+      [=, &homing_action_server](auto&& goal) {
+        return handleErrors<franka_gripper::HomingAction, franka_gripper::HomingGoalConstPtr,
+                            franka_gripper::HomingResult>(&homing_action_server, homing_handler,
+                                                          goal);
+      },
+      false);
+
+  SimpleActionServer<StopAction> stop_action_server(
+      node_handle, "stop",
+      [=, &stop_action_server](auto&& goal) {
+        return handleErrors<franka_gripper::StopAction, franka_gripper::StopGoalConstPtr,
+                            franka_gripper::StopResult>(&stop_action_server, stop_handler, goal);
+      },
+      false);
+
+  SimpleActionServer<MoveAction> move_action_server(
+      node_handle, "move",
+      [=, &move_action_server](auto&& goal) {
+        return handleErrors<franka_gripper::MoveAction, franka_gripper::MoveGoalConstPtr,
+                            franka_gripper::MoveResult>(&move_action_server, move_handler, goal);
+      },
+      false);
+
+  SimpleActionServer<GraspAction> grasp_action_server(
+      node_handle, "grasp",
+      [=, &grasp_action_server](auto&& goal) {
+        return handleErrors<franka_gripper::GraspAction, franka_gripper::GraspGoalConstPtr,
+                            franka_gripper::GraspResult>(&grasp_action_server, grasp_handler, goal);
+      },
+      false);
+
+  SimpleActionServer<GripperCommandAction> gripper_command_action_server(
+      node_handle, "gripper_action",
+      [=, &gripper, &gripper_command_action_server](auto&& goal) {
+        return gripperCommandExecuteCallback(gripper, default_grasp_epsilon, default_speed,
+                                             &gripper_command_action_server, goal);
+      },
+      false);
+
+  homing_action_server.start();
+  stop_action_server.start();
+  move_action_server.start();
+  grasp_action_server.start();
+  gripper_command_action_server.start();
+
+  double publish_rate(30.0);
+  if (!node_handle.getParam("publish_rate", publish_rate)) {
+    ROS_INFO_STREAM("franka_gripper_node: Could not find parameter publish_rate. Defaulting to "
+                    << publish_rate);
+  }
+
+  std::vector<std::string> joint_names;
+  if (!node_handle.getParam("joint_names", joint_names)) {
+    ROS_ERROR("franka_gripper_node: Could not parse joint_names!");
+    return -1;
+  }
+  if (joint_names.size() != 2) {
+    ROS_ERROR("franka_gripper_node: Got wrong number of joint_names!");
+    return -1;
+  }
+
+  franka::GripperState gripper_state;
+  std::mutex gripper_state_mutex;
+  std::thread read_thread([&gripper_state, &gripper, &gripper_state_mutex]() {
+    ros::Rate read_rate(10);
+    while (ros::ok()) {
+      {
+        std::lock_guard<std::mutex> _(gripper_state_mutex);
+        updateGripperState(gripper, &gripper_state);
+      }
+      read_rate.sleep();
+    }
+  });
+
+  ros::Publisher gripper_state_publisher =
+      node_handle.advertise<sensor_msgs::JointState>("joint_states", 1);
+  ros::AsyncSpinner spinner(2);
+  spinner.start();
+  ros::Rate rate(publish_rate);
+  while (ros::ok()) {
+    if (gripper_state_mutex.try_lock()) {
+      sensor_msgs::JointState joint_states;
+      joint_states.header.stamp = ros::Time::now();
+      joint_states.name.push_back(joint_names[0]);
+      joint_states.name.push_back(joint_names[1]);
+      joint_states.position.push_back(gripper_state.width * 0.5);
+      joint_states.position.push_back(gripper_state.width * 0.5);
+      joint_states.velocity.push_back(0.0);
+      joint_states.velocity.push_back(0.0);
+      joint_states.effort.push_back(0.0);
+      joint_states.effort.push_back(0.0);
+      gripper_state_publisher.publish(joint_states);
+      gripper_state_mutex.unlock();
+    }
+    rate.sleep();
+  }
+  read_thread.join();
+  return 0;
+}