From 369f3594af37dcfbb7c9e118cc824213ecd6f643 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Thu, 18 Jul 2019 15:12:45 +0200
Subject: [PATCH] publish digital IO bits

---
 .../ur_rtde_driver/ros/hardware_interface.h   | 10 ++++
 ur_rtde_driver/resources/rtde_recipe.txt      |  2 +
 ur_rtde_driver/src/ros/hardware_interface.cpp | 46 +++++++++++++++++++
 ur_rtde_driver/src/ur/ur_driver.cpp           |  1 -
 4 files changed, 58 insertions(+), 1 deletion(-)

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 fe4d778..e0a76c6 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
@@ -38,6 +38,8 @@
 #include "tf2_msgs/TFMessage.h"
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
+#include <ur_msgs/IOStates.h>
+
 #include <ur_controllers/speed_scaling_interface.h>
 #include <ur_controllers/scaled_joint_command_interface.h>
 
@@ -95,10 +97,15 @@ protected:
    */
   void publishPose();
 
+  void publishIOData();
+
   bool stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
 
   template <typename T>
   void readData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg, const std::string& var_name, T& data);
+  template <typename T, size_t N>
+  void readBitsetData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg, const std::string& var_name,
+                      std::bitset<N>& data);
 
   std::unique_ptr<UrDriver> ur_driver_;
 
@@ -118,6 +125,8 @@ protected:
   vector6d_t joint_efforts_;
   vector6d_t fts_measurements_;
   vector6d_t tcp_pose_;
+  std::bitset<18> actual_dig_out_bits_;
+  std::bitset<18> actual_dig_in_bits_;
   tf2::Vector3 tcp_force_;
   tf2::Vector3 tcp_torque_;
   geometry_msgs::TransformStamped tcp_transform_;
@@ -127,6 +136,7 @@ protected:
   std::vector<std::string> joint_names_;
 
   std::unique_ptr<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> tcp_pose_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::IOStates>> io_pub_;
 
   uint32_t runtime_state_;
   bool position_controller_running_;
diff --git a/ur_rtde_driver/resources/rtde_recipe.txt b/ur_rtde_driver/resources/rtde_recipe.txt
index a27d836..70d0b97 100644
--- a/ur_rtde_driver/resources/rtde_recipe.txt
+++ b/ur_rtde_driver/resources/rtde_recipe.txt
@@ -6,3 +6,5 @@ target_speed_fraction
 runtime_state
 actual_TCP_force
 actual_TCP_pose
+actual_digital_input_bits
+actual_digital_output_bits
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index 30bcb21..5e87fe6 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -185,6 +185,17 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   registerInterface(&fts_interface_);
 
   tcp_pose_pub_.reset(new realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>(root_nh, "/tf", 100));
+  io_pub_.reset(new realtime_tools::RealtimePublisher<ur_msgs::IOStates>(robot_hw_nh, "io_states", 1));
+  io_pub_->msg_.digital_in_states.resize(actual_dig_in_bits_.size());
+  io_pub_->msg_.digital_out_states.resize(actual_dig_out_bits_.size());
+  for (size_t i = 0; i < actual_dig_in_bits_.size(); ++i)
+  {
+    io_pub_->msg_.digital_in_states[i].pin = i;
+  }
+  for (size_t i = 0; i < actual_dig_out_bits_.size(); ++i)
+  {
+    io_pub_->msg_.digital_out_states[i].pin = i;
+  }
 
   deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this);
 
@@ -205,6 +216,18 @@ void HardwareInterface::readData(const std::unique_ptr<rtde_interface::DataPacka
   }
 }
 
+template <typename T, size_t N>
+void HardwareInterface::readBitsetData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg,
+                                       const std::string& var_name, std::bitset<N>& data)
+{
+  if (!data_pkg->getData<T, N>(var_name, data))
+  {
+    // This throwing should never happen unless misconfigured
+    std::string error_msg = "Did not find '" + var_name + "' in data sent from robot. This should not happen!";
+    throw std::runtime_error(error_msg);
+  }
+}
+
 void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period)
 {
   std::unique_ptr<rtde_interface::DataPackage> data_pkg = ur_driver_->getDataPackage();
@@ -217,6 +240,10 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
     readData(data_pkg, "runtime_state", runtime_state_);
     readData(data_pkg, "actual_TCP_force", fts_measurements_);
     readData(data_pkg, "actual_TCP_pose", tcp_pose_);
+    readBitsetData<uint64_t>(data_pkg, "actual_digital_input_bits", actual_dig_in_bits_);
+    readBitsetData<uint64_t>(data_pkg, "actual_digital_output_bits", actual_dig_out_bits_);
+
+    publishIOData();
 
     // Transform fts measurements to tool frame
     extractToolPose(time);
@@ -412,6 +439,25 @@ void HardwareInterface ::publishPose()
   }
 }
 
+void HardwareInterface::publishIOData()
+{
+  if (io_pub_)
+  {
+    if (io_pub_->trylock())
+    {
+      for (size_t i = 0; i < actual_dig_in_bits_.size(); ++i)
+      {
+        io_pub_->msg_.digital_in_states[i].state = actual_dig_in_bits_[i];
+      }
+      for (size_t i = 0; i < actual_dig_out_bits_.size(); ++i)
+      {
+        io_pub_->msg_.digital_out_states[i].state = actual_dig_out_bits_[i];
+      }
+      io_pub_->unlockAndPublish();
+    }
+  }
+}
+
 bool HardwareInterface::stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
 {
   if (isRobotProgramRunning())
diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp
index 3df1fcb..56379e9 100644
--- a/ur_rtde_driver/src/ur/ur_driver.cpp
+++ b/ur_rtde_driver/src/ur/ur_driver.cpp
@@ -73,7 +73,6 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
 
   std::string local_ip = rtde_client_->getIP();
 
-
   uint32_t reverse_port = 50001;        // TODO: Make this a parameter
   uint32_t script_sender_port = 50002;  // TODO: Make this a parameter