merged develop, adds rt_config parameter to franka_hw

CHANGELOG.md

@@ -30,6 +30,7 @@ Requires `libfranka` >= 0.7.0
   * **BREAKING** Moved `panda_moveit_config` to [`ros-planning`](https://github.com/ros-planning/panda_moveit_config).
   * Added support for ROS Melodic Morenia.
   * Raised minimum CMake version to 3.4 to match `libfranka`.
+  * Add rosparam to choose value of `franka::RealtimeConfig`.
 
 ## 0.6.0 - 2018-08-08
 

Jenkinsfile

@@ -35,7 +35,7 @@ def getStages(rosDistribution, ubuntuVersion) {
         }
 
         docker.build("franka_ros-ci-worker:${rosDistribution}",
-                     "-f src/franka_ros/.ci/Dockerfile.${rosDistribution} src/franka_ros/.ci").inside {
+                     "-f src/franka_ros/.ci/Dockerfile.${rosDistribution} src/franka_ros/.ci").inside('-e MAKEFLAGS') {
           withEnv(["CMAKE_PREFIX_PATH+=${env.WORKSPACE}/dist/libfranka/lib/cmake/Franka",
                    "ROS_HOME=${env.WORKSPACE}/ros-home"]) {
             stage("${rosDistribution}: Build & Lint (Debug)") {

franka_control/config/franka_combined_control_node.yaml

@@ -21,6 +21,8 @@ panda_1:
   cutoff_frequency: 100
   # Internal controller for motion generators [joint_impedance|cartesian_impedance]
   internal_controller: joint_impedance
+  # Used to decide whether to enforce realtime mode [enforce|ignore]
+  realtime_config: enforce
   # Configure the initial defaults for the collision behavior reflexes.
   collision_config:
     lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
@@ -51,6 +53,8 @@ panda_2:
   cutoff_frequency: 100
   # Internal controller for motion generators [joint_impedance|cartesian_impedance]
   internal_controller: joint_impedance
+  # Used to decide whether to enforce realtime mode [enforce|ignore]
+  realtime_config: enforce
   # Configure the initial defaults for the collision behavior reflexes.
   collision_config:
     lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]

franka_control/config/franka_control_node.yaml

@@ -15,6 +15,8 @@ rate_limiting: true
 cutoff_frequency: 100
 # Internal controller for motion generators [joint_impedance|cartesian_impedance]
 internal_controller: joint_impedance
+# Used to decide whether to enforce realtime mode [enforce|ignore]
+realtime_config: enforce
 # Configure the initial defaults for the collision behavior reflexes.
 collision_config:
   lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]

franka_control/src/franka_control_node.cpp

@@ -31,6 +31,17 @@ int main(int argc, char** argv) {
 
   franka::Robot& robot = franka_control.robot();
 
+  std::string realtime_config_param = node_handle.param("realtime_config", std::string("enforce"));
+  franka::RealtimeConfig realtime_config;
+  if (realtime_config_param == "enforce") {
+    realtime_config = franka::RealtimeConfig::kEnforce;
+  } else if (realtime_config_param == "ignore") {
+    realtime_config = franka::RealtimeConfig::kIgnore;
+  } else {
+    ROS_ERROR("Invalid realtime_config parameter provided. Valid values are 'enforce', 'ignore'.");
+    return 1;
+  }
+
   std::atomic_bool has_error(false);
 
   ServiceContainer services;

franka_hw/include/franka_hw/franka_hw.h

@@ -9,6 +9,7 @@
 #include <list>
 #include <string>
 
+#include <franka/control_types.h>
 #include <franka/duration.h>
 #include <franka/model.h>
 #include <franka/rate_limiting.h>
@@ -500,6 +501,7 @@ class FrankaHW : public hardware_interface::RobotHW {
   std::string robot_ip_;
   urdf::Model urdf_model_;
   double joint_limit_warning_threshold_{0.1};
+  franka::RealtimeConfig realtime_config_;
 
   bool initialized_{false};
   std::atomic_bool controller_active_{false};

franka_hw/src/franka_hw.cpp

@@ -127,6 +127,17 @@ bool FrankaHW::initParameters(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
         joint_limit_warning_threshold_);
   }
 
+  std::string realtime_config_param = robot_hw_nh.param("realtime_config", std::string("enforce"));
+  franka::RealtimeConfig realtime_config;
+  if (realtime_config_param == "enforce") {
+    realtime_config_ = franka::RealtimeConfig::kEnforce;
+  } else if (realtime_config_param == "ignore") {
+    realtime_config_ = franka::RealtimeConfig::kIgnore;
+  } else {
+    ROS_ERROR("Invalid realtime_config parameter provided. Valid values are 'enforce', 'ignore'.");
+    return false;
+  }
+
   // Get full collision behavior config from the parameter server.
   std::vector<double> thresholds =
       getCollisionThresholds("lower_torque_thresholds_acceleration", robot_hw_nh,