added ur_controllers package

ur_controllers/.clang-format

@@ -0,0 +1,66 @@
+---
+BasedOnStyle:  Google
+AccessModifierOffset: -2
+ConstructorInitializerIndentWidth: 2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: false
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+BinPackParameters: true
+ColumnLimit:    120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+DerivePointerBinding: false
+PointerBindsToType: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakString: 1
+PenaltyBreakFirstLessLess: 1000
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: false
+Standard:        Auto
+IndentWidth:     2
+TabWidth:        2
+UseTab:          Never
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles:  false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+SortIncludes: false
+SpaceAfterCStyleCast: false
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping: {
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+}
+...

ur_controllers/.clang-tidy

@@ -0,0 +1,10 @@
+Checks: '-*,readability-identifier-naming'
+CheckOptions:
+  - { key: readability-identifier-naming.NamespaceCase,        value: lower_case }
+  - { key: readability-identifier-naming.ClassCase,            value: CamelCase  }
+  - { key: readability-identifier-naming.PrivateMemberSuffix,  value: _         }
+  - { key: readability-identifier-naming.StructCase,           value: CamelCase  }
+  - { key: readability-identifier-naming.FunctionCase,         value: camelBack }
+  - { key: readability-identifier-naming.VariableCase,         value: lower_case }
+  - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_ }
+  - { key: readability-identifier-naming.GlobalConstantCase,   value: UPPER_CASE }

ur_controllers/CMakeLists.txt

@@ -0,0 +1,210 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ur_controllers)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+    controller_interface
+        hardware_interface
+        joint_trajectory_controller
+        pluginlib
+        realtime_tools
+        std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ur_controllers
+    CATKIN_DEPENDS
+    controller_interface
+        hardware_interface
+        joint_trajectory_controller
+        pluginlib
+        realtime_tools
+        std_msgs
+    # DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+  src/scaled_joint_trajectory_controller.cpp
+  src/speed_scaling_state_controller.cpp
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/ur_controllers_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(TARGETS ${PROJECT_NAME}
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+        FILES_MATCHING PATTERN "*.h" PATTERN ".svn"
+        EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES controller_plugins.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_ur_controllers.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

ur_controllers/controller_plugins.xml

@@ -0,0 +1,12 @@
+<library path="lib/libur_controllers">
+  <class name="ur_controllers/SpeedScalingStateController" type="ur_controllers::SpeedScalingStateController" base_class_type="controller_interface::ControllerBase">
+  <description>
+    This controller publishes the readings of all available speed scaling factors.
+  </description>
+  </class>
+  <class name="position_controllers/ScaledJointTrajectoryController" type="position_controllers::ScaledJointTrajectoryController" base_class_type="controller_interface::ControllerBase">
+  <description>
+    Scaled joint trajectory controller
+  </description>
+  </class>
+</library>

ur_controllers/include/ur_controllers/hardware_interface_adapter.h

@@ -0,0 +1,84 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-18
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
+#define UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
+
+
+#include <joint_trajectory_controller/hardware_interface_adapter.h>
+#include "ur_controllers/scaled_joint_command_interface.h"
+
+/**
+ * \brief Adapter for a position-controlled hardware interface. Forwards desired positions as commands.
+ *
+ * The following is an example configuration of a controller that uses this adapter.
+ * \code
+ * head_controller:
+ *   type: "position_controllers/ScaledJointTrajectoryController"
+ *   joints:
+ *     - head_1_joint
+ *     - head_2_joint
+ *
+ *   constraints:
+ *     goal_time: 0.6
+ *     stopped_velocity_tolerance: 0.02
+ *     head_1_joint: {trajectory: 0.05, goal: 0.02}
+ *     head_2_joint: {trajectory: 0.05, goal: 0.02}
+ *   stop_trajectory_duration: 0.5
+ *   state_publish_rate:  25
+ * \endcode
+ */
+template <class State>
+class HardwareInterfaceAdapter<ur_controllers::ScaledPositionJointInterface, State>
+{
+public:
+  HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
+
+  bool init(std::vector<ur_controllers::ScaledJointHandle>& joint_handles, ros::NodeHandle& /*controller_nh*/)
+  {
+    // Store pointer to joint handles
+    joint_handles_ptr_ = &joint_handles;
+
+
+    return true;
+  }
+
+  void starting(const ros::Time& /*time*/)
+  {
+    if (!joint_handles_ptr_) {return;}
+
+    // Semantic zero for commands
+    for (auto& jh : *joint_handles_ptr_)
+    {
+      jh.setCommand(jh.getPosition());
+    }
+  }
+
+  void stopping(const ros::Time& /*time*/) {}
+
+  void updateCommand(const ros::Time&     /*time*/,
+                     const ros::Duration& /*period*/,
+                     const State&         desired_state,
+                     const State&         /*state_error*/)
+  {
+    // Forward desired position to command
+    const unsigned int n_joints = joint_handles_ptr_->size();
+    for (unsigned int i = 0; i < n_joints; ++i) {(*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);}
+  }
+
+private:
+  std::vector<ur_controllers::ScaledJointHandle>* joint_handles_ptr_;
+};
+
+#endif // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED

ur_controllers/include/ur_controllers/scaled_joint_command_interface.h

@@ -0,0 +1,139 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright (C) 2012, hiDOF INC.
+// Copyright (C) 2019, FZI Forschungszentrum Informatik (Scaling extension)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of hiDOF, Inc. nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+/// \author Wim Meeussen
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-18
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS_SCALED_JOINT_COMMAND_INTERFACE_H_INCLUDED
+#define UR_CONTROLLERS_SCALED_JOINT_COMMAND_INTERFACE_H_INCLUDED
+
+#include <hardware_interface/internal/hardware_resource_manager.h>
+#include <hardware_interface/joint_state_interface.h>
+
+namespace ur_controllers
+{
+class ScaledJointHandle : public hardware_interface::JointStateHandle
+{
+public:
+  ScaledJointHandle() : cmd_(0), scaling_factor_(0)
+  {
+  }
+
+  /**
+   * \param js This joint's state handle
+   * \param cmd A pointer to the storage for this joint's output command
+   */
+  ScaledJointHandle(const hardware_interface::JointStateHandle& js, double* cmd, double* scaling_factor)
+    : hardware_interface::JointStateHandle(js), cmd_(cmd), scaling_factor_(scaling_factor)
+  {
+    if (cmd_ == nullptr)
+    {
+      throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
+                                                           "'. Command data pointer is null.");
+    }
+    if (scaling_factor_ == nullptr)
+    {
+      throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
+                                                           "'. Scaling factor pointer is null.");
+    }
+  }
+
+  virtual ~ScaledJointHandle() = default;
+
+  void setCommand(double command)
+  {
+    assert(cmd_);
+    *cmd_ = command;
+  }
+  double getCommand() const
+  {
+    assert(cmd_);
+    return *cmd_;
+  }
+  void setScalingFactor(double scaling_factor)
+  {
+    assert(scaling_factor_);
+    *scaling_factor_ = scaling_factor;
+  }
+  double getScalingFactor() const
+  {
+    assert(scaling_factor_);
+    return *scaling_factor_;
+  }
+
+private:
+  double* cmd_;
+  double* scaling_factor_;
+};
+
+/** \brief Hardware interface to support commanding an array of joints.
+ *
+ * This \ref HardwareInterface supports commanding the output of an array of
+ * named joints. Note that these commands can have any semantic meaning as long
+ * as they each can be represented by a single double, they are not necessarily
+ * effort commands. To specify a meaning to this command, see the derived
+ * classes like \ref EffortJointInterface etc.
+ *
+ * These interfaces provide an additional scaling factor implemented, e.g. by a robot's speed
+ * slider.
+ *
+ *
+ * \note Getting a joint handle through the getHandle() method \e will claim that resource.
+ *
+ */
+class ScaledJointCommandInterface
+  : public hardware_interface::HardwareResourceManager<ScaledJointHandle, hardware_interface::ClaimResources>
+{
+};
+
+/// \ref ScaledJointCommandInterface for commanding effort-based joints.
+class ScaledEffortJointInterface : public ScaledJointCommandInterface
+{
+};
+
+/// \ref ScaledJointCommandInterface for commanding velocity-based joints.
+class ScaledVelocityJointInterface : public ScaledJointCommandInterface
+{
+};
+
+/// \ref ScaledJointCommandInterface for commanding position-based joints.
+class ScaledPositionJointInterface : public ScaledJointCommandInterface
+{
+};
+}  // namespace ur_controllers
+#endif  // ifndef UR_CONTROLLERS_SCALED_JOINT_COMMAND_INTERFACE_H_INCLUDED

ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.h

@@ -0,0 +1,193 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-18
+ *
+ */
+//----------------------------------------------------------------------
+#ifndef UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED
+#define UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED
+
+#include "ur_controllers/hardware_interface_adapter.h"
+#include <joint_trajectory_controller/joint_trajectory_controller.h>
+
+namespace ur_controllers
+{
+template <class SegmentImpl, class HardwareInterface>
+class ScaledJointTrajectoryController
+  : public joint_trajectory_controller::JointTrajectoryController<SegmentImpl, HardwareInterface>
+{
+public:
+  ScaledJointTrajectoryController() = default;
+  virtual ~ScaledJointTrajectoryController() = default;
+
+  void update(const ros::Time& time, const ros::Duration& period)
+  {
+    this->scaling_factor_ = this->joints_[0].getScalingFactor();
+    // Get currently followed trajectory
+    typename Base::TrajectoryPtr curr_traj_ptr;
+    this->curr_trajectory_box_.get(curr_traj_ptr);
+    typename Base::Trajectory& curr_traj = *curr_traj_ptr;
+
+    // Update time data
+    typename Base::TimeData time_data;
+    time_data.time = time;                                                        // Cache current time
+    time_data.period = ros::Duration(this->scaling_factor_ * period.toSec());     // Cache current control period
+    time_data.uptime = this->time_data_.readFromRT()->uptime + time_data.period;  // Update controller uptime
+    ros::Time traj_time = this->time_data_.readFromRT()->uptime + period;
+    this->time_data_.writeFromNonRT(time_data);  // TODO: Grrr, we need a lock-free data structure here!
+
+    // NOTE: It is very important to execute the two above code blocks in the specified sequence: first get current
+    // trajectory, then update time data. Hopefully the following paragraph sheds a bit of light on the rationale.
+    // The non-rt thread responsible for processing new commands enqueues trajectories that can start at the _next_
+    // control cycle (eg. zero start time) or later (eg. when we explicitly request a start time in the future).
+    // If we reverse the order of the two blocks above, and update the time data first; it's possible that by the time
+    // we fetch the currently followed trajectory, it has been updated by the non-rt thread with something that starts
+    // in the next control cycle, leaving the current cycle without a valid trajectory.
+
+    // Update current state and state error
+    for (unsigned int i = 0; i < this->joints_.size(); ++i)
+    {
+      this->current_state_.position[i] = this->joints_[i].getPosition();
+      this->current_state_.velocity[i] = this->joints_[i].getVelocity();
+      // There's no acceleration data available in a joint handle
+
+      typename Base::TrajectoryPerJoint::const_iterator segment_it =
+          sample(curr_traj[i], traj_time.toSec(), this->desired_joint_state_);
+      if (curr_traj[i].end() == segment_it)
+      {
+        // Non-realtime safe, but should never happen under normal operation
+        ROS_ERROR_NAMED(this->name_, "Unexpected error: No trajectory defined at current time. Please contact the "
+                                     "package "
+                                     "maintainer.");
+        return;
+      }
+      this->desired_state_.position[i] = this->desired_joint_state_.position[0];
+      this->desired_state_.velocity[i] = this->desired_joint_state_.velocity[0];
+      this->desired_state_.acceleration[i] = this->desired_joint_state_.acceleration[0];
+      ;
+
+      this->state_joint_error_.position[0] =
+          angles::shortest_angular_distance(this->current_state_.position[i], this->desired_joint_state_.position[0]);
+      this->state_joint_error_.velocity[0] = this->desired_joint_state_.velocity[0] - this->current_state_.velocity[i];
+      this->state_joint_error_.acceleration[0] = 0.0;
+
+      this->state_error_.position[i] =
+          angles::shortest_angular_distance(this->current_state_.position[i], this->desired_joint_state_.position[0]);
+      this->state_error_.velocity[i] = this->desired_joint_state_.velocity[0] - this->current_state_.velocity[i];
+      this->state_error_.acceleration[i] = 0.0;
+
+      // Check tolerances
+      const typename Base::RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle();
+      if (rt_segment_goal && rt_segment_goal == this->rt_active_goal_)
+      {
+        // Check tolerances
+        if (time_data.uptime.toSec() < segment_it->endTime())
+        {
+          // Currently executing a segment: check path tolerances
+          const joint_trajectory_controller::SegmentTolerancesPerJoint<typename Base::Scalar>& joint_tolerances =
+              segment_it->getTolerances();
+          if (!checkStateTolerancePerJoint(this->state_joint_error_, joint_tolerances.state_tolerance))
+          {
+            if (this->verbose_)
+            {
+              ROS_ERROR_STREAM_NAMED(this->name_, "Path tolerances failed for joint: " << this->joint_names_[i]);
+              checkStateTolerancePerJoint(this->state_joint_error_, joint_tolerances.state_tolerance, true);
+            }
+            rt_segment_goal->preallocated_result_->error_code =
+                control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
+            rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
+            this->rt_active_goal_.reset();
+            this->successful_joint_traj_.reset();
+          }
+        }
+        else if (segment_it == --curr_traj[i].end())
+        {
+          if (this->verbose_)
+            ROS_DEBUG_STREAM_THROTTLE_NAMED(1, this->name_,
+                                            "Finished executing last segment, checking goal tolerances");
+
+          // Controller uptime
+          const ros::Time uptime = this->time_data_.readFromRT()->uptime;
+
+          // Checks that we have ended inside the goal tolerances
+          const joint_trajectory_controller::SegmentTolerancesPerJoint<typename Base::Scalar>& tolerances =
+              segment_it->getTolerances();
+          const bool inside_goal_tolerances =
+              checkStateTolerancePerJoint(this->state_joint_error_, tolerances.goal_state_tolerance);
+
+          if (inside_goal_tolerances)
+          {
+            this->successful_joint_traj_[i] = 1;
+          }
+          else if (uptime.toSec() < segment_it->endTime() + tolerances.goal_time_tolerance)
+          {
+            // Still have some time left to meet the goal state tolerances
+          }
+          else
+          {
+            if (this->verbose_)
+            {
+              ROS_ERROR_STREAM_NAMED(this->name_, "Goal tolerances failed for joint: " << this->joint_names_[i]);
+              // Check the tolerances one more time to output the errors that occurs
+              checkStateTolerancePerJoint(this->state_joint_error_, tolerances.goal_state_tolerance, true);
+            }
+
+            rt_segment_goal->preallocated_result_->error_code =
+                control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
+            rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
+            this->rt_active_goal_.reset();
+            this->successful_joint_traj_.reset();
+          }
+        }
+      }
+    }
+
+    // If there is an active goal and all segments finished successfully then set goal as succeeded
+    typename Base::RealtimeGoalHandlePtr current_active_goal(this->rt_active_goal_);
+    if (current_active_goal && this->successful_joint_traj_.count() == this->joints_.size())
+    {
+      current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
+      current_active_goal->setSucceeded(current_active_goal->preallocated_result_);
+      current_active_goal.reset();  // do not publish feedback
+      this->rt_active_goal_.reset();
+      this->successful_joint_traj_.reset();
+    }
+
+    // Hardware interface adapter: Generate and send commands
+    this->hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period, this->desired_state_, this->state_error_);
+
+    // Set action feedback
+    if (current_active_goal)
+    {
+      current_active_goal->preallocated_feedback_->header.stamp = this->time_data_.readFromRT()->time;
+      current_active_goal->preallocated_feedback_->desired.positions = this->desired_state_.position;
+      current_active_goal->preallocated_feedback_->desired.velocities = this->desired_state_.velocity;
+      current_active_goal->preallocated_feedback_->desired.accelerations = this->desired_state_.acceleration;
+      current_active_goal->preallocated_feedback_->actual.positions = this->current_state_.position;
+      current_active_goal->preallocated_feedback_->actual.velocities = this->current_state_.velocity;
+      current_active_goal->preallocated_feedback_->error.positions = this->state_error_.position;
+      current_active_goal->preallocated_feedback_->error.velocities = this->state_error_.velocity;
+      current_active_goal->setFeedback(current_active_goal->preallocated_feedback_);
+    }
+
+    // Publish state
+    this->publishState(time_data.uptime);
+  }
+
+protected:
+  using Base = joint_trajectory_controller::JointTrajectoryController<SegmentImpl, HardwareInterface>;
+  double scaling_factor_;
+
+private:
+  /* data */
+};
+}  // namespace ur_controllers
+
+#endif  // ifndef UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED

ur_controllers/include/ur_controllers/speed_scaling_interface.h

@@ -0,0 +1,50 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-17
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS_SPEED_SCALING_INTERFACE_H_INCLUDED
+#define UR_CONTROLLERS_SPEED_SCALING_INTERFACE_H_INCLUDED
+
+#include <hardware_interface/internal/hardware_resource_manager.h>
+
+namespace ur_controllers
+{
+class SpeedScalingHandle
+{
+public:
+  SpeedScalingHandle() : name_(""), scaling_factor_(0){};
+  SpeedScalingHandle(const std::string& name, const double* scaling_factor)
+    : name_(name), scaling_factor_(scaling_factor){};
+  virtual ~SpeedScalingHandle() = default;
+
+  std::string getName() const
+  {
+    return name_;
+  }
+
+  const double* getScalingFactor() const
+  {
+    return scaling_factor_;
+  }
+
+private:
+  std::string name_;
+  const double* scaling_factor_;
+};
+/** \brief Hardware interface to support reading the speed scaling factor. */
+class SpeedScalingInterface : public hardware_interface::HardwareResourceManager<SpeedScalingHandle>
+{
+};
+}  // namespace ur_controllers
+
+#endif  // ifndef UR_CONTROLLERS_SPEED_SCALING_INTERFACE_H_INCLUDED

ur_controllers/include/ur_controllers/speed_scaling_state_controller.h

@@ -0,0 +1,46 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-17
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED
+#define UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED
+
+#include <controller_interface/controller.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <std_msgs/Float64.h>
+
+#include "ur_controllers/speed_scaling_interface.h"
+
+namespace ur_controllers
+{
+class SpeedScalingStateController : public controller_interface::Controller<SpeedScalingInterface>
+{
+public:
+  SpeedScalingStateController() = default;
+  virtual ~SpeedScalingStateController() override = default;
+
+  virtual bool init(SpeedScalingInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
+  virtual void starting(const ros::Time& time) override;
+  virtual void update(const ros::Time& time, const ros::Duration& /*period*/) override;
+  virtual void stopping(const ros::Time& /*time*/) override;
+
+private:
+  std::vector<SpeedScalingHandle> sensors_;
+  //TODO: We should use a better datatype later on
+  typedef std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64> > RtPublisherPtr;
+  std::vector<RtPublisherPtr> realtime_pubs_;
+  std::vector<ros::Time> last_publish_times_;
+  double publish_rate_;
+};
+}  // namespace ur_controllers
+#endif  // ifndef UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED

ur_controllers/package.xml

@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ur_controllers</name>
+  <version>0.0.0</version>
+  <description>The ur_controllers package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="mauch@fzi.de">Felix Mauch</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>FZI</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ur_controllers</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>joint_trajectory_controller</depend>
+  <depend>pluginlib</depend>
+  <depend>realtime_tools</depend>
+  <depend>std_msgs</depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <controller_interface plugin="${prefix}/controller_plugins.xml"/>
+
+  </export>
+</package>

ur_controllers/src/scaled_joint_trajectory_controller.cpp

@@ -0,0 +1,29 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-18
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_controllers/scaled_joint_trajectory_controller.h"
+#include "ur_controllers/scaled_joint_command_interface.h"
+
+#include <pluginlib/class_list_macros.hpp>
+#include <trajectory_interface/quintic_spline_segment.h>
+
+
+namespace position_controllers
+{
+typedef ur_controllers::ScaledJointTrajectoryController<trajectory_interface::QuinticSplineSegment<double>,
+                                                        ur_controllers::ScaledPositionJointInterface>
+    ScaledJointTrajectoryController;
+}
+
+PLUGINLIB_EXPORT_CLASS(position_controllers::ScaledJointTrajectoryController, controller_interface::ControllerBase)

ur_controllers/src/speed_scaling_state_controller.cpp

@@ -0,0 +1,89 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-17
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_controllers/speed_scaling_state_controller.h"
+
+#include <pluginlib/class_list_macros.hpp>
+
+namespace ur_controllers
+{
+bool SpeedScalingStateController::init(SpeedScalingInterface* hw, ros::NodeHandle& root_nh,
+                                       ros::NodeHandle& controller_nh)
+{
+  // get all joint states from the hardware interface
+  const std::vector<std::string>& sensor_names = hw->getNames();
+  for (unsigned i = 0; i < sensor_names.size(); i++)
+    ROS_DEBUG("Got sensor %s", sensor_names[i].c_str());
+
+  // get publishing period
+  if (!controller_nh.getParam("publish_rate", publish_rate_))
+  {
+    ROS_ERROR("Parameter 'publish_rate' not set");
+    return false;
+  }
+
+  for (unsigned i = 0; i < sensor_names.size(); i++)
+  {
+    // sensor handle
+    sensors_.push_back(hw->getHandle(sensor_names[i]));
+
+    // realtime publisher
+    RtPublisherPtr rt_pub(new realtime_tools::RealtimePublisher<std_msgs::Float64>(root_nh, sensor_names[i], 4));
+    realtime_pubs_.push_back(rt_pub);
+  }
+
+  // Last published times
+  last_publish_times_.resize(sensor_names.size());
+  return true;
+}
+
+void SpeedScalingStateController ::starting(const ros::Time& time)
+{
+  // initialize time
+  for (unsigned i = 0; i < last_publish_times_.size(); i++)
+  {
+    last_publish_times_[i] = time;
+  }
+}
+
+void SpeedScalingStateController ::update(const ros::Time& time, const ros::Duration& /*period*/)
+{
+  // limit rate of publishing
+  for (unsigned i = 0; i < realtime_pubs_.size(); i++)
+  {
+    if (publish_rate_ > 0.0 && last_publish_times_[i] + ros::Duration(1.0 / publish_rate_) < time)
+    {
+      // try to publish
+      if (realtime_pubs_[i]->trylock())
+      {
+        // we're actually publishing, so increment time
+        last_publish_times_[i] = last_publish_times_[i] + ros::Duration(1.0 / publish_rate_);
+
+        // populate message
+        // realtime_pubs_[i]->msg_.header.stamp = time;
+        realtime_pubs_[i]->msg_.data = *sensors_[i].getScalingFactor();
+
+        realtime_pubs_[i]->unlockAndPublish();
+      }
+    }
+  }
+}
+
+void SpeedScalingStateController ::stopping(const ros::Time& /*time*/)
+{
+}
+}  // namespace ur_controllers
+
+PLUGINLIB_EXPORT_CLASS(ur_controllers::SpeedScalingStateController, controller_interface::ControllerBase)
+

ur_rtde_driver/config/ur10_controllers.yaml

@@ -30,7 +30,8 @@ speed_scaling_state_controller:
 # Joint Trajectory Controller - position based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
 pos_based_pos_traj_controller:
-   type: position_controllers/JointTrajectoryController
+   #type: position_controllers/JointTrajectoryController
+   type: position_controllers/ScaledJointTrajectoryController
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -34,6 +34,7 @@
 #include <algorithm>
 
 #include <ur_controllers/speed_scaling_interface.h>
+#include <ur_controllers/scaled_joint_command_interface.h>
 
 #include "ur_rtde_driver/ur/ur_driver.h"
 
@@ -58,6 +59,7 @@ protected:
   std::unique_ptr<UrDriver> ur_driver_;
 
   hardware_interface::JointStateInterface js_interface_;
+  ur_controllers::ScaledPositionJointInterface spj_interface_;
   hardware_interface::PositionJointInterface pj_interface_;
   ur_controllers::SpeedScalingInterface speedsc_interface_;
   // hardware_interface::VelocityJointInterface vj_interface_;
@@ -67,7 +69,9 @@ protected:
   vector6d_t joint_positions_;
   vector6d_t joint_velocities_;
   vector6d_t joint_efforts_;
-  double speed_scaling_value_;
+  double speed_scaling_;
+  double target_speed_fraction_;
+  double speed_scaling_combined_;
   std::vector<std::string> joint_names_;
 
   bool position_controller_running_;

ur_rtde_driver/launch/ur10_ros_control.launch

@@ -16,7 +16,7 @@
   <arg name="base_frame" default="$(arg prefix)base" />
   <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
   <arg name="shutdown_on_disconnect" default="true" />
-  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller"/>
+  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller"/>
   <arg name="stopped_controllers" default=""/>
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -60,22 +60,26 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   // Create ros_control interfaces
   for (std::size_t i = 0; i < joint_positions_.size(); ++i)
   {
+    ROS_INFO_STREAM("Registing handles for joint " << joint_names_[i]);
     // Create joint state interface for all joints
     js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_positions_[i],
                                                                       &joint_velocities_[i], &joint_efforts_[i]));
 
     // Create joint position control interface
-    pj_interface_.registerHandle(
-        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
+    // pj_interface_.registerHandle(
+    // hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
+    spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
+        js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
   }
 
-  speedsc_interface_.registerHandle(
-      ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_));
+  // speedsc_interface_.registerHandle(
+  // ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_));
 
   // Register interfaces
   registerInterface(&js_interface_);
-  registerInterface(&pj_interface_);
-  registerInterface(&speedsc_interface_);
+  registerInterface(&spj_interface_);
+  // registerInterface(&pj_interface_);
+  // registerInterface(&speedsc_interface_);
 
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
 
@@ -97,11 +101,18 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
       // This throwing should never happen unless misconfigured
       throw std::runtime_error("Did not find joint velocity in data sent from robot. This should not happen!");
     }
-    if (!data_pkg->getData("target_speed_fraction", speed_scaling_value_))
+    if (!data_pkg->getData("target_speed_fraction", target_speed_fraction_))
+    {
+      // This throwing should never happen unless misconfigured
+      throw std::runtime_error("Did not find target_speed_fraction_ in data sent from robot. This should not happen!");
+    }
+    if (!data_pkg->getData("speed_scaling", speed_scaling_))
     {
       // This throwing should never happen unless misconfigured
       throw std::runtime_error("Did not find speed_scaling in data sent from robot. This should not happen!");
     }
+
+    speed_scaling_combined_ = speed_scaling_ * target_speed_fraction_;
   }
   else
   {
@@ -113,6 +124,7 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio
 {
   if (position_controller_running_)
   {
+    // ROS_INFO_STREAM("Writing command: " << joint_position_command_);
     ur_driver_->writeJointCommand(joint_position_command_);
   }
 }
@@ -132,7 +144,7 @@ void HardwareInterface ::doSwitch(const std::list<hardware_interface::Controller
   {
     for (auto& resource_it : controller_it.claimed_resources)
     {
-      if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
+      if (resource_it.hardware_interface == "ur_controllers::ScaledPositionJointInterface")
       {
         position_controller_running_ = true;
       }