Merge remote-tracking branch 'origin/master' into feature/SWDEV-348-finished-and-cancelled-motions

.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

.ci/debug.sh

+#!/bin/sh
+
+set -e
+set -x
+
+. /opt/ros/kinetic/setup.sh
+
+rm -f src/CMakeLists.txt
+catkin_init_workspace src
+
+rm -rf build-debug
+mkdir build-debug
+cd build-debug
+
+cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_EXPORT_COMPILE_COMMANDS=ON ../src
+. devel/setup.sh
+
+cmake --build .
+cmake --build . --target tests
+ctest -V
+
+cmake --build . --target check-format
+cmake --build . --target check-tidy

Jenkinsfile

@@ -3,50 +3,48 @@
 node {
   step([$class: 'StashNotifier'])
 
-  def gitCommit
   try {
-    stage('Checkout') {
-      dir('catkin_ws/src') {
+    dir('src/franka_ros') {
       checkout scm
-        gitCommit = sh(script: 'git rev-parse HEAD', returnStdout: true).trim()
     }
 
-      dir('libfranka') {
-        // TODO(walc_fl): Remove hard-coded repository URL
-        checkout resolveScm(source: [$class: 'GitSCMSource', remote: 'https://github.com/frankaemika/libfranka.git',
-                                     credentialsId: '6a639baf-566a-4e66-b089-74cd9ecb38a8', includes: '*', excludes: '',
-                                     extensions: [[$class: 'SubmoduleOption', parentCredentials: true, recursiveSubmodules: true]]],
-                            targets: [BRANCH_NAME, 'master'])
-      }
+    sh 'rm -rf dist'
+    dir('dist') {
+      try {
+        step([$class: 'CopyArtifact',
+              filter: 'libfranka-*-amd64.tar.gz',
+              fingerprintArtifacts: true,
+              projectName: "SWDEV/libfranka/${BRANCH_NAME}",
+              selector: [$class: 'StatusBuildSelector', stable: false]])
+      } catch (e) {
+        // Fall back to master branch.
+        step([$class: 'CopyArtifact',
+              filter: 'libfranka-*-amd64.tar.gz',
+              fingerprintArtifacts: true,
+              projectName: "SWDEV/libfranka/master",
+              selector: [$class: 'StatusBuildSelector', stable: false]])
+      }
+      sh '''
+        tar xfz libfranka-*-amd64.tar.gz
+        ln -sf libfranka-*-amd64 libfranka
+      '''
     }
 
-    stage('Build libfranka') {
-      dir('libfranka/build') {
-        sh 'cmake -DCMAKE_BUILD_TYPE=Debug -DBUILD_COVERAGE=OFF -DBUILD_TESTS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_DOCUMENTATION=OFF ..'
-        sh 'cmake --build .'
+    docker.build('franka_ros-ci-worker', 'src/franka_ros/.ci').inside {
+      withEnv(["CMAKE_PREFIX_PATH+=${env.WORKSPACE}/dist/libfranka/lib/cmake/Franka",
+               "ROS_HOME=${env.WORKSPACE}/ros-home"]) {
+        stage('Build & Lint (Debug)') {
+          sh 'src/franka_ros/.ci/debug.sh'
+          junit 'build-debug/test_results/**/*.xml'
         }
       }
-
-    dir('catkin_ws') {
-      stage('Build in debug mode') {
-        env.FRANKA_DIR = "${pwd()}/../libfranka/build"
-        sh 'src/scripts/ci/debug-build.sh'
     }
 
-      stage('Archive results') {
-        junit 'build/test_results/**/*.xml'
-      }
-    }
     currentBuild.result = 'SUCCESS'
   } catch (e) {
     currentBuild.result = 'FAILED'
     throw e;
   } finally {
-    // Explicitly specify commit hash, otherwise Stash will get notified about the `libfranka` commit as well.
-    if (gitCommit) {
-      step([$class: 'StashNotifier', commitSha1: gitCommit])
-    } else {
     step([$class: 'StashNotifier'])
   }
 }
-}

franka_gripper/action/Grasp.action

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

franka_gripper/config/gripper_node_config.yaml

@@ -4,4 +4,3 @@ joint_names:
   - franka_emika_finger_joint2
 width_tolerance: 0.01  # [m]
 default_speed: 0.1  # [m/s]
\ No newline at end of file
-newton_to_m_ampere_factor: 14.9  # [mA/N]

franka_gripper/include/franka_gripper/franka_gripper.h

@@ -31,14 +31,12 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st
   *
   * @param[in] gripper A pointer to a franka gripper
   * @param[in] default_speed The default speed for a gripper action
-  * @param[in] newton_to_m_ampere_factor The mapping factor from Newton to milliampere
   * @param[in] action_server A pointer to a gripper action server
   * @param[in] goal A gripper action goal
   */
 void gripperCommandExecuteCallback(
     const franka::Gripper& gripper,
     double default_speed,
-    double newton_to_m_ampere_factor,
     actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
     const control_msgs::GripperCommandGoalConstPtr& goal);
 

franka_gripper/src/franka_gripper.cpp

@@ -28,7 +28,6 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st
 void gripperCommandExecuteCallback(
     const franka::Gripper& gripper,
     double default_speed,
-    double newton_to_m_ampere_factor,
     actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
     const control_msgs::GripperCommandGoalConstPtr& goal) {
   std::function<bool()> gripper_command_handler = [=, &gripper]() {
@@ -41,8 +40,7 @@ void gripperCommandExecuteCallback(
     if (goal->command.position >= state.width) {
       return gripper.move(goal->command.position, default_speed);
     }
-    return gripper.grasp(goal->command.position, default_speed,
-                         goal->command.max_effort * newton_to_m_ampere_factor);
+    return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort);
   };
 
   try {
@@ -77,7 +75,7 @@ bool stop(const franka::Gripper& gripper, const StopGoalConstPtr& /*goal*/) {
 }
 
 bool grasp(const franka::Gripper& gripper, const GraspGoalConstPtr& goal) {
-  return gripper.grasp(goal->width, goal->speed, goal->max_current);
+  return gripper.grasp(goal->width, goal->speed, goal->force);
 }
 
 }  // namespace franka_gripper

franka_gripper/src/franka_gripper_node.cpp

@@ -73,12 +73,6 @@ int main(int argc, char** argv) {
     ROS_INFO_STREAM("franka_gripper_node: Found default_speed" << default_speed);
   }
 
-  double newton_to_m_ampere_factor(14.9);
-  if (node_handle.getParam("newton_to_m_ampere_factor", newton_to_m_ampere_factor)) {
-    ROS_INFO_STREAM("franka_gripper_node: Found newton_to_m_ampere_factor"
-                    << newton_to_m_ampere_factor);
-  }
-
   franka::Gripper gripper(robot_ip);
 
   std::function<bool(const HomingGoalConstPtr&)> homing_handler =
@@ -114,7 +108,7 @@ int main(int argc, char** argv) {
   SimpleActionServer<GripperCommandAction> gripper_command_action_server(
       node_handle, "gripper_action",
       std::bind(&gripperCommandExecuteCallback, std::cref(gripper), default_speed,
-                newton_to_m_ampere_factor, &gripper_command_action_server, std::placeholders::_1),
+                &gripper_command_action_server, std::placeholders::_1),
       false);
 
   homing_action_server_.start();

scripts/ci/debug-build.sh

-#!/bin/sh
-
-set -e
-
-. /opt/ros/kinetic/setup.sh
-
-rm -rf build devel
-catkin_make --cmake-args -DFranka_DIR:PATH=$FRANKA_DIR -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
-. devel/setup.sh
-catkin_make -j1 check-format
-catkin_make -j1 check-tidy
-catkin_make run_tests