added first version of speed scaling readings

CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(catkin REQUIRED
     std_srvs
     tf
     trajectory_msgs
+    ur_controllers
     ur_msgs
 )
 find_package(Boost REQUIRED)
@@ -40,6 +41,7 @@ catkin_package(
     roscpp
     sensor_msgs
     trajectory_msgs
+    ur_controllers
     ur_msgs
   DEPENDS
     Boost

config/ur10_controllers.yaml

@@ -22,6 +22,11 @@ force_torque_sensor_controller:
    type:         force_torque_sensor_controller/ForceTorqueSensorController
    publish_rate: 125
 
+# Publish speed_scaling factor
+speed_scaling_state_controller:
+   type:         ur_controllers/SpeedScalingStateController
+   publish_rate: 125
+
 # Joint Trajectory Controller - position based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
 pos_based_pos_traj_controller:

include/ur_rtde_driver/ros/hardware_interface.h

@@ -33,6 +33,8 @@
 #include <hardware_interface/joint_state_interface.h>
 #include <algorithm>
 
+#include <ur_controllers/speed_scaling_interface.h>
+
 #include "ur_rtde_driver/ur/ur_driver.h"
 
 namespace ur_driver
@@ -57,6 +59,7 @@ protected:
 
   hardware_interface::JointStateInterface js_interface_;
   hardware_interface::PositionJointInterface pj_interface_;
+  ur_controllers::SpeedScalingInterface speedsc_interface_;
   // hardware_interface::VelocityJointInterface vj_interface_;
 
   vector6d_t joint_position_command_;
@@ -64,6 +67,7 @@ protected:
   vector6d_t joint_positions_;
   vector6d_t joint_velocities_;
   vector6d_t joint_efforts_;
+  double speed_scaling_value_;
   std::vector<std::string> joint_names_;
 
   bool position_controller_running_;

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"/>
+  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller"/>
   <arg name="stopped_controllers" default=""/>
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">

package.xml

@@ -29,6 +29,7 @@
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
   <depend>trajectory_msgs</depend>
+  <depend>ur_controllers</depend>
   <depend>ur_msgs</depend>
   <depend>tf</depend>
   <depend>std_srvs</depend>

src/ros/hardware_interface.cpp

@@ -69,9 +69,13 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
         hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
   }
 
+  speedsc_interface_.registerHandle(
+      ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_));
+
   // Register interfaces
   registerInterface(&js_interface_);
   registerInterface(&pj_interface_);
+  registerInterface(&speedsc_interface_);
 
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
 
@@ -93,6 +97,11 @@ 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_))
+    {
+      // 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!");
+    }
   }
   else
   {