implemented setting of standard digital and analog outputs via SetIO service

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -41,6 +41,7 @@
 
 #include <ur_msgs/IOStates.h>
 #include <ur_msgs/ToolDataMsg.h>
+#include <ur_msgs/SetIO.h>
 
 #include <ur_controllers/speed_scaling_interface.h>
 #include <ur_controllers/scaled_joint_command_interface.h>
@@ -111,6 +112,7 @@ protected:
                       std::bitset<N>& data);
 
   void speedScalingCallback(const std_msgs::Float64::ConstPtr& msg);
+  bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res);
 
   std::unique_ptr<UrDriver> ur_driver_;
 
@@ -154,6 +156,7 @@ protected:
   std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::ToolDataMsg>> tool_data_pub_;
 
   ros::Subscriber speed_slider_sub_;
+  ros::ServiceServer set_io_srv_;
 
   uint32_t runtime_state_;
   bool position_controller_running_;

ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_writer.h

@@ -52,9 +52,10 @@ public:
   bool sendStandardDigitalOutput(uint8_t output_pin, bool value);
   bool sendConfigurableDigitalOutput(uint8_t output_pin, bool value);
   bool sendToolDigitalOutput(bool value);
-  bool sendStandardAnalogOuput(uint8_t output_pin, bool value);
+  bool sendStandardAnalogOuput(uint8_t output_pin, double value);
 
 private:
+  uint8_t pinToMask(uint8_t pin);
   comm::URStream<PackageHeader>* stream_;
   std::vector<std::string> recipe_;
   uint8_t recipe_id_;

ur_rtde_driver/resources/rtde_input_recipe.txt

@@ -1,2 +1,8 @@
 speed_slider_mask
 speed_slider_fraction
+standard_digital_output_mask
+standard_digital_output
+standard_analog_output_mask
+standard_analog_output_type
+standard_analog_output_0
+standard_analog_output_1

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -218,6 +218,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
 
   speed_slider_sub_ = robot_hw_nh.subscribe("set_speed_slider", 10, &HardwareInterface::speedScalingCallback, 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);
 
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
@@ -540,4 +542,22 @@ void HardwareInterface::speedScalingCallback(const std_msgs::Float64::ConstPtr&
 {
   ur_driver_->getRTDEWriter().sendSpeedSlider(msg->data);
 }
+
+bool HardwareInterface::setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res)
+{
+  if (req.fun == req.FUN_SET_DIGITAL_OUT)
+  {
+    res.success = ur_driver_->getRTDEWriter().sendStandardDigitalOutput(req.pin, req.state);
+  }
+  else if (req.fun == req.FUN_SET_ANALOG_OUT)
+  {
+    res.success = ur_driver_->getRTDEWriter().sendStandardAnalogOuput(req.pin, req.state);
+  }
+  else
+  {
+    res.success = false;
+  }
+
+  return true;
+}
 }  // namespace ur_driver

ur_rtde_driver/src/rtde/data_package.cpp

@@ -220,6 +220,8 @@ std::unordered_map<std::string, DataPackage::_rtde_type_variant> DataPackage::g_
   { "tool_digital_output", uint8_t() },
   { "standard_analog_output_mask", uint8_t() },
   { "standard_analog_output_type", uint8_t() },
+  { "standard_analog_output_0", double() },
+  { "standard_analog_output_1", double() },
 };
 
 void rtde_interface::DataPackage::initEmpty()

ur_rtde_driver/src/rtde/rtde_writer.cpp

@@ -63,19 +63,47 @@ bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction)
   package.reset(new DataPackage(recipe_));
   package->initEmpty();
   uint32_t mask = 1;
-  package->setData("speed_slider_mask", mask);
+  bool success = true;
-  package->setData("speed_slider_fraction", speed_slider_fraction);
+  success = package->setData("speed_slider_mask", mask);
+  success = success && package->setData("speed_slider_fraction", speed_slider_fraction);
 
+  if (success)
+  {
     if (!queue_.tryEnqueue(std::move(package)))
     {
       return false;
     }
-  return true;
+  }
+  return success;
 }
 
 bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value)
 {
+  std::unique_ptr<DataPackage> package;
+  package.reset(new DataPackage(recipe_));
+  package->initEmpty();
+  uint8_t mask = pinToMask(output_pin);
+  bool success = true;
+  uint8_t digital_output;
+  if (value)
+  {
+    digital_output = 255;
+  }
+  else
+  {
+    digital_output = 0;
+  }
+  success = package->setData("standard_digital_output_mask", mask);
+  success = success && package->setData("standard_digital_output", digital_output);
+
+  if (success)
+  {
+    if (!queue_.tryEnqueue(std::move(package)))
+    {
       return false;
+    }
+  }
+  return success;
 }
 
 bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value)
@@ -88,9 +116,38 @@ bool RTDEWriter::sendToolDigitalOutput(bool value)
   return false;
 }
 
-bool RTDEWriter::sendStandardAnalogOuput(uint8_t output_pin, bool value)
+bool RTDEWriter::sendStandardAnalogOuput(uint8_t output_pin, double value)
 {
+  std::unique_ptr<DataPackage> package;
+  package.reset(new DataPackage(recipe_));
+  package->initEmpty();
+  uint8_t mask = pinToMask(output_pin);
+  // default to current for now, as no functionality to choose included in set io service
+  uint8_t output_type = 0;
+  bool success = true;
+  success = package->setData("standard_analog_output_mask", mask);
+  success = success && package->setData("standard_analog_output_type", output_type);
+  success = success && package->setData("standard_analog_output_0", value);
+  success = success && package->setData("standard_analog_output_1", value);
+
+  if (success)
+  {
+    if (!queue_.tryEnqueue(std::move(package)))
+    {
       return false;
+    }
+  }
+  return success;
+}
+
+uint8_t RTDEWriter::pinToMask(uint8_t pin)
+{
+  if (pin > 7)
+  {
+    return 0;
+  }
+
+  return 1 << pin;
 }
 
 }  // namespace rtde_interface