implemented setting of speed_slider via ROS topic

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -33,6 +33,7 @@
 #include <hardware_interface/joint_state_interface.h>
 #include <algorithm>
 #include <std_msgs/Bool.h>
+#include <std_msgs/Float64.h>
 #include <std_srvs/Trigger.h>
 #include <realtime_tools/realtime_publisher.h>
 #include "tf2_msgs/TFMessage.h"
@@ -109,6 +110,8 @@ 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);
+
   std::unique_ptr<UrDriver> ur_driver_;
 
   ros::ServiceServer deactivate_srv_;
@@ -150,6 +153,8 @@ 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_;
+
   uint32_t runtime_state_;
   bool position_controller_running_;
 

ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_writer.h

@@ -30,7 +30,10 @@
 
 #include "ur_rtde_driver/rtde/package_header.h"
 #include "ur_rtde_driver/rtde/rtde_package.h"
+#include "ur_rtde_driver/rtde/data_package.h"
 #include "ur_rtde_driver/comm/stream.h"
+#include "ur_rtde_driver/queue/readerwriterqueue.h"
+#include <thread>
 
 namespace ur_driver
 {
@@ -40,10 +43,10 @@ class RTDEWriter
 {
 public:
   RTDEWriter() = delete;
-  RTDEWriter(comm::URStream<PackageHeader>* stream, const std::string& recipe_file);
+  RTDEWriter(comm::URStream<PackageHeader>* stream, const std::vector<std::string>& recipe);
   ~RTDEWriter() = default;
-  bool init(uint8_t recipe_id);
+  void init(uint8_t recipe_id);
-  bool start();
+  void run();
 
   bool sendSpeedSlider(double speed_slider_fraction);
   bool sendStandardDigitalOutput(uint8_t output_pin, bool value);
@@ -55,6 +58,8 @@ private:
   comm::URStream<PackageHeader>* stream_;
   std::vector<std::string> recipe_;
   uint8_t recipe_id_;
+  moodycamel::BlockingReaderWriterQueue<std::unique_ptr<DataPackage>> queue_;
+  std::thread writer_thread_;
 };
 
 }  // namespace rtde_interface

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -216,6 +216,8 @@ 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);
+
   deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this);
 
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
@@ -533,4 +535,9 @@ bool HardwareInterface::stopControl(std_srvs::TriggerRequest& req, std_srvs::Tri
   }
   return true;
 }
+
+void HardwareInterface::speedScalingCallback(const std_msgs::Float64::ConstPtr& msg)
+{
+  ur_driver_->getRTDEWriter().sendSpeedSlider(msg->data);
+}
 }  // namespace ur_driver

ur_rtde_driver/src/rtde/rtde_client.cpp

@@ -40,7 +40,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st
   , parser_(recipe_)
   , prod_(stream_, parser_)
   , pipeline_(prod_, PIPELINE_NAME, notifier)
-  , writer_(&stream_, input_recipe_file)
+  , writer_(&stream_, input_recipe_)
   , max_frequency_(URE_MAX_FREQUENCY)
 {
 }

ur_rtde_driver/src/rtde/rtde_writer.cpp

@@ -31,35 +31,63 @@ namespace ur_driver
 {
 namespace rtde_interface
 {
-RTDEWriter::RTDEWriter(comm::URStream<PackageHeader>* stream, const std::string& recipe_file) : stream_(stream)
+RTDEWriter::RTDEWriter(comm::URStream<PackageHeader>* stream, const std::vector<std::string>& recipe)
+  : stream_(stream), recipe_(recipe), queue_{ 32 }
 {
 }
 
-bool RTDEWriter::init(uint8_t recipe_id)
+void RTDEWriter::init(uint8_t recipe_id)
 {
-  return false;
+  recipe_id_ = recipe_id;
+  writer_thread_ = std::thread(&RTDEWriter::run, this);
 }
-bool RTDEWriter::start()
+
+void RTDEWriter::run()
 {
-  return false;
+  uint8_t buffer[4096];
+  size_t size;
+  size_t written;
+  std::unique_ptr<DataPackage> package;
+  while (true)
+  {
+    queue_.waitDequeue(package);
+    package->setRecipeID(recipe_id_);
+    size = package->serializePackage(buffer);
+    stream_->write(buffer, size, written);
+  }
 }
 
 bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction)
+{
+  std::unique_ptr<DataPackage> package;
+  package.reset(new DataPackage(recipe_));
+  package->initEmpty();
+  uint32_t mask = 1;
+  package->setData("speed_slider_mask", mask);
+  package->setData("speed_slider_fraction", speed_slider_fraction);
+
+  if (!queue_.tryEnqueue(std::move(package)))
   {
     return false;
   }
+  return true;
+}
+
 bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value)
 {
   return false;
 }
+
 bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value)
 {
   return false;
 }
+
 bool RTDEWriter::sendToolDigitalOutput(bool value)
 {
   return false;
 }
+
 bool RTDEWriter::sendStandardAnalogOuput(uint8_t output_pin, bool value)
 {
   return false;