From 4ddfe664d9bada13e29ba2af6ca654b7d2e210a4 Mon Sep 17 00:00:00 2001
From: Tristan Schnell <schnell@fzi.de>
Date: Thu, 1 Aug 2019 13:39:04 +0200
Subject: [PATCH] changed ROS interface to use set speed slider service

---
 .../ur_rtde_driver/ros/hardware_interface.h   |  5 ++--
 ur_rtde_driver/src/ros/hardware_interface.cpp | 27 +++++++++++++------
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
index 96321cc..54d6893 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
@@ -47,6 +47,7 @@
 #include <ur_controllers/scaled_joint_command_interface.h>
 
 #include "ur_rtde_driver/ur/ur_driver.h"
+#include "ur_rtde_driver/SetSpeedSlider.h"
 
 namespace ur_driver
 {
@@ -111,7 +112,7 @@ protected:
   void readBitsetData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg, const std::string& var_name,
                       std::bitset<N>& data);
 
-  void speedScalingCallback(const std_msgs::Float64::ConstPtr& msg);
+  bool setSpeedSlider(ur_rtde_driver::SetSpeedSliderRequest& req, ur_rtde_driver::SetSpeedSliderResponse& res);
   bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res);
 
   std::unique_ptr<UrDriver> ur_driver_;
@@ -155,7 +156,7 @@ protected:
   std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::IOStates>> io_pub_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::ToolDataMsg>> tool_data_pub_;
 
-  ros::Subscriber speed_slider_sub_;
+  ros::ServiceServer set_speed_slider_srv_;
   ros::ServiceServer set_io_srv_;
 
   uint32_t runtime_state_;
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index b823db6..a9f2cfb 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -216,8 +216,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   }
   tool_data_pub_.reset(new realtime_tools::RealtimePublisher<ur_msgs::ToolDataMsg>(robot_hw_nh, "tool_data", 1));
 
-  speed_slider_sub_ = robot_hw_nh.subscribe("set_speed_slider", 10, &HardwareInterface::speedScalingCallback, this);
-
+  set_speed_slider_srv_ = robot_hw_nh.advertiseService("set_speed_slider", &HardwareInterface::setSpeedSlider, this);
   set_io_srv_ = robot_hw_nh.advertiseService("set_io", &HardwareInterface::setIO, this);
 
   deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this);
@@ -538,9 +537,18 @@ bool HardwareInterface::stopControl(std_srvs::TriggerRequest& req, std_srvs::Tri
   return true;
 }
 
-void HardwareInterface::speedScalingCallback(const std_msgs::Float64::ConstPtr& msg)
+bool HardwareInterface::setSpeedSlider(ur_rtde_driver::SetSpeedSliderRequest& req,
+                                       ur_rtde_driver::SetSpeedSliderResponse& res)
 {
-  ur_driver_->getRTDEWriter().sendSpeedSlider(msg->data);
+  if (req.data >= 0.01 && req.data <= 1.0)
+  {
+    res.success = ur_driver_->getRTDEWriter().sendSpeedSlider(req.data);
+  }
+  else
+  {
+    res.success = false;
+  }
+  return true;
 }
 
 bool HardwareInterface::setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res)
@@ -550,11 +558,14 @@ bool HardwareInterface::setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse
     if (req.pin <= 7)
     {
       res.success = ur_driver_->getRTDEWriter().sendStandardDigitalOutput(req.pin, req.state);
-    } else if (req.pin <= 15)
+    }
+    else if (req.pin <= 15)
     {
-      res.success = ur_driver_->getRTDEWriter().sendConfigurableDigitalOutput(req.pin-8, req.state);
-    } else {
-      res.success = ur_driver_->getRTDEWriter().sendToolDigitalOutput(req.pin-16, req.state);
+      res.success = ur_driver_->getRTDEWriter().sendConfigurableDigitalOutput(req.pin - 8, req.state);
+    }
+    else
+    {
+      res.success = ur_driver_->getRTDEWriter().sendToolDigitalOutput(req.pin - 16, req.state);
     }
   }
   else if (req.fun == req.FUN_SET_ANALOG_OUT)