Merge pull request #89 in SWDEV/franka_ros from RACE-1778-rate-limiting to develop

* commit '4ac35eef':
  Fix changelog
  Update changelog
  lowpass -> low-pass
  Fixes for clang-tidy
  Add -fix for clang-tidy
  Dockerfile: Clean up APT cache
  Fix tests
  Use same version of Clang tools as libfranka
  FrankaHW: Use callbacks for parameter retrieval
  Fix controller switching bug when using same control interface
  Update changelog
  Allow setting rate limiting and cutoff frequency at runtime
  franka_control.launch: Launch franka_gripper_node when passing load_gripper:=true

.ci/Dockerfile

 FROM osrf/ros:kinetic-desktop-xenial
 RUN apt-get update && apt-get install -y \
-    clang \
-    clang-format \
-    clang-tidy \
-    ros-kinetic-ros-control
+    clang-5.0 \
+    clang-format-5.0 \
+    clang-tidy-5.0 \
+    ros-kinetic-ros-control \
+    && rm -rf /var/lib/apt/lists/*

CHANGELOG.md

@@ -4,15 +4,18 @@
 
 Requires `libfranka` >= 0.5.0
 
+  * **BREAKING** Fixes for MoveIt, improving robot performance:
+    * Fixed joint velocity and acceleration limits in `joint_limits.yaml`
+    * Use desired joint state for move group
   * **BREAKING** Updated joint limits in URDF
   * **BREAKING** Fixed velocity, acceleration and jerk limits in `franka_hw`
+  * **BREAKING** Start `franka_gripper_node` when giving `load_gripper:=true` to `franka_control.launch`
   * Allow to configure rate limiting, filtering and internal controller in `franka_control_node`
-  * **BREAKING** Enabled limiting and low-pass filtering by default (`franka_control_node.yaml`)
+  * **BREAKING** `FrankaHW::FrankaHW` takes additional parameters.
+  * **BREAKING** Enabled rate limiting and low-pass filtering by default (`franka_control_node.yaml`)
   * Publish desired joint state in `/joint_state_desired`
   * Removed `effort_joint_trajectory_controller` from `default_controllers.yaml`
-  * **BREAKING** Fixes for MoveIt:
-    * Fixed joint velocity and acceleration limits in `joint_limits.yaml`
-    * Use desired joint state for move group
+  * Fixed a bug when switching between controllers using the same `libfranka` interface
 
 ## 0.5.0 - 2018-06-28
 

cmake/ClangTools.cmake

 include(CMakeParseArguments)
 
-find_program(CLANG_FORMAT_PROG clang-format DOC "'clang-format' executable")
+find_program(CLANG_FORMAT_PROG clang-format-5.0 DOC "'clang-format' executable")
 if(CLANG_FORMAT_PROG AND NOT TARGET format)
   add_custom_target(format)
   add_custom_target(check-format)
 endif()
-find_program(CLANG_TIDY_PROG clang-tidy DOC "'clang-tidy' executable")
+find_program(CLANG_TIDY_PROG clang-tidy-5.0 DOC "'clang-tidy' executable")
 if(CLANG_TIDY_PROG AND NOT TARGET tidy)
   if(NOT CMAKE_EXPORT_COMPILE_COMMANDS)
     message(WARNING "Invoke Catkin/CMake with '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON'
@@ -46,7 +46,7 @@ function(add_tidy_target _target)
   cmake_parse_arguments(ARG "" "" "FILES;DEPENDS" ${ARGN})
 
   add_custom_target(tidy-${_target}
-    COMMAND ${CLANG_TIDY_PROG} -p=${CMAKE_BINARY_DIR} ${ARG_FILES}
+    COMMAND ${CLANG_TIDY_PROG} -fix -p=${CMAKE_BINARY_DIR} ${ARG_FILES}
     WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
     DEPENDS ${ARG_DEPENDS}
     COMMENT "Running clang-tidy for ${_target}"

franka_control/config/franka_control_node.yaml

@@ -7,11 +7,9 @@ joint_names:
   - panda_joint6
   - panda_joint7
 arm_id: panda
-# Rate limiter activation flag for each interface [true|false]
-# [Joint position, Joint velocity, Cartesian Pose, Cartesian Velocity, Torque]
-rate_limiting: [true, true, true, true, true]
-# Cutoff frequency of the lowpass filter for each interface. Set to >= 1000 to deactivate.
-# [Joint position, Joint velocity, Cartesian Pose, Cartesian Velocity, Torque]
-cutoff_freq: [100, 100, 100, 100, 100]
+# Activate rate limiter? [true|false]
+rate_limiting: true
+# Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
+cutoff_frequency: 100
 # Internal controller for motion generators [joint_impedance|cartesian_impedance]
 internal_controller: joint_impedance

franka_control/include/franka_control/franka_state_controller.h

@@ -26,7 +26,7 @@ class FrankaStateController
   /**
    * Creates an instance of a FrankaStateController.
    */
-  FrankaStateController();
+  FrankaStateController() = default;
 
   /**
    * Initializes the controller with interfaces and publishers.
@@ -55,8 +55,8 @@ class FrankaStateController
 
   std::string arm_id_;
 
-  franka_hw::FrankaStateInterface* franka_state_interface_;
-  std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_;
+  franka_hw::FrankaStateInterface* franka_state_interface_{};
+  std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_{};
 
   realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> publisher_transforms_;
   realtime_tools::RealtimePublisher<franka_msgs::FrankaState> publisher_franka_states_;

franka_control/launch/franka_control.launch

@@ -6,6 +6,10 @@
   <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" />
   <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" />
 
+  <include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
+    <arg name="robot_ip" value="$(arg robot_ip)" />
+  </include>
+
   <node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
     <rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" />
     <param name="robot_ip" value="$(arg robot_ip)" />

franka_control/package.xml

@@ -31,6 +31,7 @@
   <depend>tf</depend>
 
   <exec_depend>franka_description</exec_depend>
+  <exec_depend>franka_gripper</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>message_runtime</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>

franka_control/src/franka_control_node.cpp

@@ -40,38 +40,30 @@ int main(int argc, char** argv) {
     return 1;
   }
 
-  std::vector<bool> limit_rate_vector;
-  if (!node_handle.getParam("rate_limiting", limit_rate_vector) || limit_rate_vector.size() != 5) {
-    ROS_ERROR("Invalid or no rate_limiting parameters provided");
+  std::array<std::string, 7> joint_names;
+  std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin());
+
+  bool rate_limiting;
+  if (!node_handle.getParamCached("rate_limiting", rate_limiting)) {
+    ROS_ERROR("Invalid or no rate_limiting parameter provided");
     return 1;
   }
 
-  std::vector<double> cutoff_freq_vector;
-  if (!node_handle.getParam("cutoff_freq", cutoff_freq_vector) || cutoff_freq_vector.size() != 5) {
-    ROS_ERROR("Invalid or no cutoff_freq parameters provided");
+  double cutoff_frequency;
+  if (!node_handle.getParamCached("cutoff_frequency", cutoff_frequency)) {
+    ROS_ERROR("Invalid or no cutoff_frequency parameter provided");
     return 1;
   }
 
-  franka::ControllerMode internal_controller;
-  std::string internal_controller_name;
-  if (!node_handle.getParam("internal_controller", internal_controller_name)) {
+  std::string internal_controller;
+  if (!node_handle.getParam("internal_controller", internal_controller)) {
     ROS_ERROR("No internal_controller parameter provided");
     return 1;
   }
 
-  std::array<std::string, 7> joint_names;
-  std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin());
-  std::array<bool, 5> rate_limiting;
-  std::copy(limit_rate_vector.cbegin(), limit_rate_vector.cend(), rate_limiting.begin());
-  std::array<double, 5> cutoff_freq;
-  std::copy(cutoff_freq_vector.cbegin(), cutoff_freq_vector.cend(), cutoff_freq.begin());
-
-  if (internal_controller_name == "joint_impedance") {
-    internal_controller = franka::ControllerMode::kJointImpedance;
-  } else if (internal_controller_name == "cartesian_impedance") {
-    internal_controller = franka::ControllerMode::kCartesianImpedance;
-  } else {
-    ROS_ERROR("Invalid internal_controller parameter provided");
+  urdf::Model urdf_model;
+  if (!urdf_model.initParamWithNodeHandle("robot_description", public_node_handle)) {
+    ROS_ERROR("Could not initialize URDF model from robot_description");
     return 1;
   }
 
@@ -97,29 +89,37 @@ int main(int argc, char** argv) {
 
   std::atomic_bool has_error(false);
 
-  using std::placeholders::_1;
-  using std::placeholders::_2;
   ServiceContainer services;
   services
       .advertiseService<franka_control::SetJointImpedance>(
           node_handle, "set_joint_impedance",
-          std::bind(franka_control::setJointImpedance, std::ref(robot), _1, _2))
+          [&robot](auto&& req, auto&& res) {
+            return franka_control::setJointImpedance(robot, req, res);
+          })
       .advertiseService<franka_control::SetCartesianImpedance>(
           node_handle, "set_cartesian_impedance",
-          std::bind(franka_control::setCartesianImpedance, std::ref(robot), _1, _2))
+          [&robot](auto&& req, auto&& res) {
+            return franka_control::setCartesianImpedance(robot, req, res);
+          })
       .advertiseService<franka_control::SetEEFrame>(
           node_handle, "set_EE_frame",
-          std::bind(franka_control::setEEFrame, std::ref(robot), _1, _2))
+          [&robot](auto&& req, auto&& res) { return franka_control::setEEFrame(robot, req, res); })
       .advertiseService<franka_control::SetKFrame>(
-          node_handle, "set_K_frame", std::bind(franka_control::setKFrame, std::ref(robot), _1, _2))
+          node_handle, "set_K_frame",
+          [&robot](auto&& req, auto&& res) { return franka_control::setKFrame(robot, req, res); })
       .advertiseService<franka_control::SetForceTorqueCollisionBehavior>(
           node_handle, "set_force_torque_collision_behavior",
-          std::bind(franka_control::setForceTorqueCollisionBehavior, std::ref(robot), _1, _2))
+          [&robot](auto&& req, auto&& res) {
+            return franka_control::setForceTorqueCollisionBehavior(robot, req, res);
+          })
       .advertiseService<franka_control::SetFullCollisionBehavior>(
           node_handle, "set_full_collision_behavior",
-          std::bind(franka_control::setFullCollisionBehavior, std::ref(robot), _1, _2))
+          [&robot](auto&& req, auto&& res) {
+            return franka_control::setFullCollisionBehavior(robot, req, res);
+          })
       .advertiseService<franka_control::SetLoad>(
-          node_handle, "set_load", std::bind(franka_control::setLoad, std::ref(robot), _1, _2));
+          node_handle, "set_load",
+          [&robot](auto&& req, auto&& res) { return franka_control::setLoad(robot, req, res); });
 
   actionlib::SimpleActionServer<franka_control::ErrorRecoveryAction> recovery_action_server(
       node_handle, "error_recovery",
@@ -136,8 +136,31 @@ int main(int argc, char** argv) {
       false);
 
   franka::Model model = robot.loadModel();
-  franka_hw::FrankaHW franka_control(joint_names, arm_id, internal_controller, rate_limiting,
-                                     cutoff_freq, public_node_handle, model);
+  auto get_rate_limiting = [&]() {
+    node_handle.getParamCached("rate_limiting", rate_limiting);
+    return rate_limiting;
+  };
+  auto get_internal_controller = [&]() {
+    node_handle.getParamCached("internal_controller", internal_controller);
+
+    franka::ControllerMode controller_mode;
+    if (internal_controller == "joint_impedance") {
+      controller_mode = franka::ControllerMode::kJointImpedance;
+    } else if (internal_controller == "cartesian_impedance") {
+      controller_mode = franka::ControllerMode::kCartesianImpedance;
+    } else {
+      ROS_WARN("Invalid internal_controller parameter provided, falling back to joint impedance");
+      controller_mode = franka::ControllerMode::kJointImpedance;
+    }
+
+    return controller_mode;
+  };
+  auto get_cutoff_frequency = [&]() {
+    node_handle.getParamCached("cutoff_frequency", cutoff_frequency);
+    return cutoff_frequency;
+  };
+  franka_hw::FrankaHW franka_control(joint_names, arm_id, urdf_model, model, get_rate_limiting,
+                                     get_cutoff_frequency, get_internal_controller);
 
   // Initialize robot state before loading any controller
   franka_control.update(robot.readOnce());

franka_control/src/franka_state_controller.cpp

@@ -3,6 +3,7 @@
 #include <franka_control/franka_state_controller.h>
 
 #include <cmath>
+#include <memory>
 #include <mutex>
 #include <string>
 
@@ -134,16 +135,6 @@ franka_msgs::Errors errorsToMessage(const franka::Errors& error) {
 
 namespace franka_control {
 
-FrankaStateController::FrankaStateController()
-    : franka_state_interface_(nullptr),
-      franka_state_handle_(nullptr),
-      publisher_transforms_(),
-      publisher_franka_states_(),
-      publisher_joint_states_(),
-      publisher_joint_states_desired_(),
-      publisher_external_wrench_(),
-      trigger_publish_(30.0) {}
-
 bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
                                  ros::NodeHandle& root_node_handle,
                                  ros::NodeHandle& controller_node_handle) {
@@ -157,12 +148,11 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
     return false;
   }
   double publish_rate(30.0);
-  if (controller_node_handle.getParam("publish_rate", publish_rate)) {
-    trigger_publish_ = franka_hw::TriggerRate(publish_rate);
-  } else {
+  if (!controller_node_handle.getParam("publish_rate", publish_rate)) {
     ROS_INFO_STREAM("FrankaStateController: Did not find publish_rate. Using default "
                     << publish_rate << " [Hz].");
   }
+  trigger_publish_ = franka_hw::TriggerRate(publish_rate);
 
   if (!controller_node_handle.getParam("joint_names", joint_names_) ||
       joint_names_.size() != robot_state_.q.size()) {
@@ -173,8 +163,8 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
   }
 
   try {
-    franka_state_handle_.reset(
-        new franka_hw::FrankaStateHandle(franka_state_interface_->getHandle(arm_id_ + "_robot")));
+    franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+        franka_state_interface_->getHandle(arm_id_ + "_robot"));
   } catch (const hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM("FrankaStateController: Exception getting franka state handle: " << ex.what());
     return false;

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

@@ -3,6 +3,7 @@
 #include <franka_example_controllers/cartesian_impedance_example_controller.h>
 
 #include <cmath>
+#include <memory>
 
 #include <controller_interface/controller_base.h>
 #include <franka/robot_state.h>
@@ -35,16 +36,15 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
     return false;
   }
 
-  franka_hw::FrankaModelInterface* model_interface =
-      robot_hw->get<franka_hw::FrankaModelInterface>();
+  auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
   if (model_interface == nullptr) {
     ROS_ERROR_STREAM(
         "CartesianImpedanceExampleController: Error getting model interface from hardware");
     return false;
   }
   try {
-    model_handle_.reset(
-        new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model")));
+    model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
+        model_interface->getHandle(arm_id + "_model"));
   } catch (hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM(
         "CartesianImpedanceExampleController: Exception getting model handle from interface: "
@@ -52,16 +52,15 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
     return false;
   }
 
-  franka_hw::FrankaStateInterface* state_interface =
-      robot_hw->get<franka_hw::FrankaStateInterface>();
+  auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
   if (state_interface == nullptr) {
     ROS_ERROR_STREAM(
         "CartesianImpedanceExampleController: Error getting state interface from hardware");
     return false;
   }
   try {
-    state_handle_.reset(
-        new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id + "_robot")));
+    state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+        state_interface->getHandle(arm_id + "_robot"));
   } catch (hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM(
         "CartesianImpedanceExampleController: Exception getting state handle from interface: "
@@ -69,8 +68,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
     return false;
   }
 
-  hardware_interface::EffortJointInterface* effort_joint_interface =
-      robot_hw->get<hardware_interface::EffortJointInterface>();
+  auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
   if (effort_joint_interface == nullptr) {
     ROS_ERROR_STREAM(
         "CartesianImpedanceExampleController: Error getting effort joint interface from hardware");
@@ -89,9 +87,10 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
   dynamic_reconfigure_compliance_param_node_ =
       ros::NodeHandle("dynamic_reconfigure_compliance_param_node");
 
-  dynamic_server_compliance_param_.reset(
-      new dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>(
-          dynamic_reconfigure_compliance_param_node_));
+  dynamic_server_compliance_param_ = std::make_unique<
+      dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>(
+
+      dynamic_reconfigure_compliance_param_node_);
   dynamic_server_compliance_param_->setCallback(
       boost::bind(&CartesianImpedanceExampleController::complianceParamCallback, this, _1, _2));
 

franka_example_controllers/src/cartesian_pose_example_controller.cpp

@@ -3,6 +3,7 @@
 #include <franka_example_controllers/cartesian_pose_example_controller.h>
 
 #include <cmath>
+#include <memory>
 #include <stdexcept>
 #include <string>
 
@@ -31,8 +32,8 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
   }
 
   try {
-    cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle(
-        cartesian_pose_interface_->getHandle(arm_id + "_robot")));
+    cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+        cartesian_pose_interface_->getHandle(arm_id + "_robot"));
   } catch (const hardware_interface::HardwareInterfaceException& e) {
     ROS_ERROR_STREAM(
         "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());

franka_example_controllers/src/cartesian_velocity_example_controller.cpp

@@ -4,6 +4,7 @@
 
 #include <array>
 #include <cmath>
+#include <memory>
 #include <string>
 
 #include <controller_interface/controller_base.h>
@@ -31,8 +32,8 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot
     return false;
   }
   try {
-    velocity_cartesian_handle_.reset(new franka_hw::FrankaCartesianVelocityHandle(
-        velocity_cartesian_interface_->getHandle(arm_id + "_robot")));
+    velocity_cartesian_handle_ = std::make_unique<franka_hw::FrankaCartesianVelocityHandle>(
+        velocity_cartesian_interface_->getHandle(arm_id + "_robot"));
   } catch (const hardware_interface::HardwareInterfaceException& e) {
     ROS_ERROR_STREAM(
         "CartesianVelocityExampleController: Exception getting Cartesian handle: " << e.what());

franka_example_controllers/src/elbow_example_controller.cpp

@@ -3,6 +3,7 @@
 #include <franka_example_controllers/elbow_example_controller.h>
 
 #include <cmath>
+#include <memory>
 #include <stdexcept>
 #include <string>
 
@@ -29,8 +30,8 @@ bool ElbowExampleController::init(hardware_interface::RobotHW* robot_hardware,
   }
 
   try {
-    cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle(
-        cartesian_pose_interface_->getHandle(arm_id + "_robot")));
+    cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+        cartesian_pose_interface_->getHandle(arm_id + "_robot"));
   } catch (const hardware_interface::HardwareInterfaceException& e) {
     ROS_ERROR_STREAM("ElbowExampleController: Exception getting Cartesian handle: " << e.what());
     return false;

franka_example_controllers/src/force_example_controller.cpp

@@ -3,6 +3,7 @@
 #include <franka_example_controllers/force_example_controller.h>
 
 #include <cmath>
+#include <memory>
 
 #include <controller_interface/controller_base.h>
 #include <pluginlib/class_list_macros.h>
@@ -30,38 +31,35 @@ bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
     return false;
   }
 
-  franka_hw::FrankaModelInterface* model_interface =
-      robot_hw->get<franka_hw::FrankaModelInterface>();
+  auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
   if (model_interface == nullptr) {
     ROS_ERROR_STREAM("ForceExampleController: Error getting model interface from hardware");
     return false;
   }
   try {
-    model_handle_.reset(
-        new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model")));
+    model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
+        model_interface->getHandle(arm_id + "_model"));
   } catch (hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM(
         "ForceExampleController: Exception getting model handle from interface: " << ex.what());
     return false;
   }
 
-  franka_hw::FrankaStateInterface* state_interface =
-      robot_hw->get<franka_hw::FrankaStateInterface>();
+  auto* state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
   if (state_interface == nullptr) {
     ROS_ERROR_STREAM("ForceExampleController: Error getting state interface from hardware");
     return false;
   }
   try {
-    state_handle_.reset(
-        new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id + "_robot")));
+    state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+        state_interface->getHandle(arm_id + "_robot"));
   } catch (hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM(
         "ForceExampleController: Exception getting state handle from interface: " << ex.what());
     return false;
   }
 
-  hardware_interface::EffortJointInterface* effort_joint_interface =
-      robot_hw->get<hardware_interface::EffortJointInterface>();
+  auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
   if (effort_joint_interface == nullptr) {
     ROS_ERROR_STREAM("ForceExampleController: Error getting effort joint interface from hardware");
     return false;
@@ -77,9 +75,10 @@ bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
 
   dynamic_reconfigure_desired_mass_param_node_ =
       ros::NodeHandle("dynamic_reconfigure_desired_mass_param_node");
-  dynamic_server_desired_mass_param_.reset(
-      new dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>(
-          dynamic_reconfigure_desired_mass_param_node_));
+  dynamic_server_desired_mass_param_ = std::make_unique<
+      dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>(
+
+      dynamic_reconfigure_desired_mass_param_node_);
   dynamic_server_desired_mass_param_->setCallback(
       boost::bind(&ForceExampleController::desiredMassParamCallback, this, _1, _2));
 

franka_example_controllers/src/joint_impedance_example_controller.cpp

@@ -3,6 +3,7 @@
 #include <franka_example_controllers/joint_impedance_example_controller.h>
 
 #include <cmath>
+#include <memory>
 
 #include <controller_interface/controller_base.h>
 #include <pluginlib/class_list_macros.h>
@@ -72,16 +73,15 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
                     << coriolis_factor_);
   }
 
-  franka_hw::FrankaModelInterface* model_interface =
-      robot_hw->get<franka_hw::FrankaModelInterface>();
+  auto* model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
   if (model_interface == nullptr) {
     ROS_ERROR_STREAM(
         "JointImpedanceExampleController: Error getting model interface from hardware");
     return false;
   }
   try {
-    model_handle_.reset(
-        new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model")));
+    model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
+        model_interface->getHandle(arm_id + "_model"));
   } catch (hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM(
         "JointImpedanceExampleController: Exception getting model handle from interface: "
@@ -89,16 +89,15 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
     return false;
   }
 
-  franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface =
-      robot_hw->get<franka_hw::FrankaPoseCartesianInterface>();
+  auto* cartesian_pose_interface = robot_hw->get<franka_hw::FrankaPoseCartesianInterface>();
   if (cartesian_pose_interface == nullptr) {
     ROS_ERROR_STREAM(
         "JointImpedanceExampleController: Error getting cartesian pose interface from hardware");
     return false;
   }
   try {
-    cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle(
-        cartesian_pose_interface->getHandle(arm_id + "_robot")));
+    cartesian_pose_handle_ = std::make_unique<franka_hw::FrankaCartesianPoseHandle>(
+        cartesian_pose_interface->getHandle(arm_id + "_robot"));
   } catch (hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM(
         "JointImpedanceExampleController: Exception getting cartesian pose handle from interface: "
@@ -106,8 +105,7 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
     return false;
   }
 
-  hardware_interface::EffortJointInterface* effort_joint_interface =
-      robot_hw->get<hardware_interface::EffortJointInterface>();
+  auto* effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
   if (effort_joint_interface == nullptr) {
     ROS_ERROR_STREAM(
         "JointImpedanceExampleController: Error getting effort joint interface from hardware");

franka_example_controllers/src/model_example_controller.cpp

@@ -7,6 +7,7 @@
 #include <array>
 #include <cstring>
 #include <iterator>
+#include <memory>
 
 #include <controller_interface/controller_base.h>
 #include <hardware_interface/hardware_interface.h>
@@ -48,8 +49,8 @@ bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw,
   }
 
   try {
-    franka_state_handle_.reset(
-        new franka_hw::FrankaStateHandle(franka_state_interface_->getHandle(arm_id + "_robot")));
+    franka_state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
+        franka_state_interface_->getHandle(arm_id + "_robot"));
   } catch (const hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM(
         "ModelExampleController: Exception getting franka state handle: " << ex.what());
@@ -57,8 +58,8 @@ bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw,
   }
 
   try {
-    model_handle_.reset(
-        new franka_hw::FrankaModelHandle(model_interface_->getHandle(arm_id + "_model")));
+    model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
+        model_interface_->getHandle(arm_id + "_model"));
   } catch (hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM(
         "ModelExampleController: Exception getting model handle from interface: " << ex.what());

franka_gripper/src/franka_gripper_node.cpp

@@ -84,46 +84,56 @@ int main(int argc, char** argv) {
 
   franka::Gripper gripper(robot_ip);
 
-  std::function<bool(const HomingGoalConstPtr&)> homing_handler =
-      std::bind(homing, std::cref(gripper), std::placeholders::_1);
-  std::function<bool(const StopGoalConstPtr&)> stop_handler =
-      std::bind(stop, std::cref(gripper), std::placeholders::_1);
-  std::function<bool(const GraspGoalConstPtr&)> grasp_handler =
-      std::bind(grasp, std::cref(gripper), std::placeholders::_1);
-  std::function<bool(const MoveGoalConstPtr&)> move_handler =
-      std::bind(move, std::cref(gripper), std::placeholders::_1);
-
-  SimpleActionServer<HomingAction> homing_action_server_(
+  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",
-      std::bind(handleErrors<HomingAction, HomingGoalConstPtr, HomingResult>,
-                &homing_action_server_, homing_handler, std::placeholders::_1),
+      [=, &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", std::bind(handleErrors<StopAction, StopGoalConstPtr, StopResult>,
-                                     &stop_action_server_, stop_handler, std::placeholders::_1),
+  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", std::bind(handleErrors<MoveAction, MoveGoalConstPtr, MoveResult>,
-                                     &move_action_server_, move_handler, std::placeholders::_1),
+  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", std::bind(handleErrors<GraspAction, GraspGoalConstPtr, GraspResult>,
-                                      &grasp_action_server_, grasp_handler, std::placeholders::_1),
+  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",
-      std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_grasp_epsilon,
-                default_speed, &gripper_command_action_server, std::placeholders::_1),
+      [=, &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();
+  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);

franka_hw/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
   hardware_interface
   joint_limits_interface
   roscpp
+  urdf
 )
 
 find_package(Franka 0.5.0 REQUIRED)
@@ -16,7 +17,7 @@ find_package(Franka 0.5.0 REQUIRED)
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES franka_hw
-  CATKIN_DEPENDS controller_interface hardware_interface joint_limits_interface roscpp
+  CATKIN_DEPENDS controller_interface hardware_interface joint_limits_interface roscpp urdf
   DEPENDS Franka
 )
 

franka_hw/include/franka_hw/franka_hw.h

@@ -11,17 +11,16 @@
 #include <franka/model.h>
 #include <franka/robot.h>
 #include <franka/robot_state.h>
+#include <franka_hw/control_mode.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
 #include <hardware_interface/joint_command_interface.h>
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
 #include <joint_limits_interface/joint_limits_interface.h>
-#include <ros/node_handle.h>
 #include <ros/time.h>
-
-#include <franka_hw/control_mode.h>
-#include <franka_hw/franka_cartesian_command_interface.h>
-#include <franka_hw/franka_model_interface.h>
-#include <franka_hw/franka_state_interface.h>
+#include <urdf/model.h>
 
 namespace franka_hw {
 
@@ -34,42 +33,46 @@ class FrankaHW : public hardware_interface::RobotHW {
    *
    * @param[in] joint_names An array of joint names being controlled.
    * @param[in] arm_id Unique identifier for the robot being controlled.
-   * @param[in] limit_rate An array of booleans indicating if the rate limiter should be used or not
-   * for each interface
-   * @param[in] internal_controller Internal controller to be used for control loops using only
-   * motion generation
-   * @param[in] cutoff_freq An array of doubles indicating the cutoff frequency for the lowpass
-   * applied in each interface
-   * @param[in] node_handle A node handle to get parameters from.
+   * @param[in] urdf_model A URDF model to initialize joint limits from.
+   * @param[in] get_limit_rate Getter that should return true if the rate limiter
+   * should be used, false otherwise. Defaults to true.
+   * @param[in] get_cutoff_frequency Getter for cutoff frequency for the low-pass filter.
+   * Defaults to franka::kDefaultCutoffFrequency.
+   * @param[in] get_internal_controller Getter for an internal controller to
+   * be used for control loops using only motion generation. Defaults to joint impedance.
    */
   FrankaHW(const std::array<std::string, 7>& joint_names,
            const std::string& arm_id,
-           franka::ControllerMode internal_controller,
-           const std::array<bool, 5>& limit_rate,
-           const std::array<double, 5>& cutoff_freq,
-           const ros::NodeHandle& node_handle);
+           const urdf::Model& urdf_model,
+           std::function<bool()> get_limit_rate = []() { return true; },
+           std::function<double()> get_cutoff_frequency =
+               []() { return franka::kDefaultCutoffFrequency; },
+           std::function<franka::ControllerMode()> get_internal_controller =
+               []() { return franka::ControllerMode::kJointImpedance; });
 
   /**
    * Creates an instance of FrankaHW that provides a model interface.
    *
    * @param[in] joint_names An array of joint names being controlled.
    * @param[in] arm_id Unique identifier for the robot being controlled.
-   * @param[in] node_handle A node handle to get parameters from.
-   * @param[in] limit_rate An array of booleans indicating if the rate limiter should be used or not
-   * for each interface
-   * @param[in] internal_controller Internal controller to be used for control loops using only
-   * motion generation
-   * @param[in] cutoff_freq An array of doubles indicating the cutoff frequency for the lowpass
-   * applied in each interface
+   * @param[in] urdf_model A URDF model to initialize joint limits from.
    * @param[in] model Robot model.
+   * @param[in] get_limit_rate Getter that should return true if the rate limiter
+   * should be used, false otherwise. Defaults to true.
+   * @param[in] get_cutoff_frequency Getter for cutoff frequency for the low-pass filter.
+   * Defaults to franka::kDefaultCutoffFrequency.
+   * @param[in] get_internal_controller Getter for an internal controller to
+   * be used for control loops using only motion generation. Defaults to joint impedance.
    */
   FrankaHW(const std::array<std::string, 7>& joint_names,
            const std::string& arm_id,
-           franka::ControllerMode internal_controller,
-           const std::array<bool, 5>& limit_rate,
-           const std::array<double, 5>& cutoff_freq,
-           const ros::NodeHandle& node_handle,
-           franka::Model& model);
+           const urdf::Model& urdf_model,
+           franka::Model& model,
+           std::function<bool()> get_limit_rate = []() { return true; },
+           std::function<double()> get_cutoff_frequency =
+               []() { return franka::kDefaultCutoffFrequency; },
+           std::function<franka::ControllerMode()> get_internal_controller =
+               []() { return franka::ControllerMode::kJointImpedance; });
 
   ~FrankaHW() override = default;
 
@@ -90,7 +93,7 @@ class FrankaHW : public hardware_interface::RobotHW {
    * @throw franka::RealtimeException if realtime priority cannot be set for the current thread.
    */
   void control(franka::Robot& robot,
-               std::function<bool(const ros::Time&, const ros::Duration&)> ros_callback);
+               const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback);
 
   /**
    * Updates the controller interfaces from the given robot state.
@@ -178,7 +181,7 @@ class FrankaHW : public hardware_interface::RobotHW {
                     const franka::RobotState& robot_state,
                     franka::Duration time_step) {
     robot_state_ = robot_state;
-    if (!controller_active_ || ros_callback && !ros_callback(robot_state, time_step)) {
+    if (!controller_active_ || (ros_callback && !ros_callback(robot_state, time_step))) {
       return franka::MotionFinished(command);
     }
     return command;
@@ -201,9 +204,9 @@ class FrankaHW : public hardware_interface::RobotHW {
 
   std::array<std::string, 7> joint_names_;
   const std::string arm_id_;
-  franka::ControllerMode internal_controller_;
-  std::array<bool, 5> limit_rate_;
-  std::array<double, 5> cutoff_freq_;
+  std::function<franka::ControllerMode()> get_internal_controller_;
+  std::function<bool()> get_limit_rate_;
+  std::function<double()> get_cutoff_frequency_;
 
   franka::JointPositions position_joint_command_;
   franka::JointVelocities velocity_joint_command_;