From 699bc08ac6ae1c4ed47687760c7da2789250f21a Mon Sep 17 00:00:00 2001
From: Thomas Timm Andersen <thomas.timm.dk@gmail.com>
Date: Wed, 16 Sep 2015 11:42:55 +0200
Subject: [PATCH] Added ROS publisher of IOStates

---
 src/robot_state.cpp    | 22 ++++++++++++++++++-
 src/ur_ros_wrapper.cpp | 48 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 69 insertions(+), 1 deletion(-)

diff --git a/src/robot_state.cpp b/src/robot_state.cpp
index c01868f..07104c0 100644
--- a/src/robot_state.cpp
+++ b/src/robot_state.cpp
@@ -209,7 +209,7 @@ double RobotState::getVersion() {
 	double ver;
 	val_lock_.lock();
 	ver = version_msg_.major_version + 0.1 * version_msg_.minor_version
-	+ .0000001 * version_msg_.svn_revision;
+			+ .0000001 * version_msg_.svn_revision;
 	val_lock_.unlock();
 	return ver;
 
@@ -218,3 +218,23 @@ double RobotState::getVersion() {
 void RobotState::finishedReading() {
 	new_data_available_ = false;
 }
+
+int RobotState::getDigitalInputBits() {
+	return mb_data_.digitalInputBits;
+}
+int RobotState::getDigitalOutputBits() {
+	return mb_data_.digitalOutputBits;
+}
+double RobotState::getAnalogInput0() {
+	return mb_data_.analogInput0;
+}
+double RobotState::getAnalogInput1() {
+	return mb_data_.analogInput1;
+}
+double RobotState::getAnalogOutput0() {
+	return mb_data_.analogOutput0;
+
+}
+double RobotState::getAnalogOutput1() {
+	return mb_data_.analogOutput1;
+}
diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index 70ffe36..6c37e6e 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -31,6 +31,9 @@
 #include "ur_msgs/SetPayloadResponse.h"
 #include "ur_msgs/SetIORequest.h"
 #include "ur_msgs/SetIOResponse.h"
+#include "ur_msgs/IOStates.h"
+#include "ur_msgs/Digital.h"
+#include "ur_msgs/Analog.h"
 #include "std_msgs/String.h"
 
 class RosWrapper {
@@ -366,6 +369,51 @@ private:
 		}
 	}
 
+	void publishMsg() {
+		ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("/io_states",
+				1);
+
+		while (ros::ok()) {
+			ur_msgs::IOStates io_msg;
+			std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
+			std::unique_lock<std::mutex> locker(msg_lock);
+			while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) {
+				msg_cond_.wait(locker);
+			}
+
+			for (unsigned int i = 0; i < 10; i++) {
+				ur_msgs::Digital digi;
+				digi.pin = i;
+				digi.state =
+						((robot_.sec_interface_->robot_state_->getDigitalInputBits()
+								& (1 << i)) >> i);
+				io_msg.digital_in_states.push_back(digi);
+				digi.state =
+						((robot_.sec_interface_->robot_state_->getDigitalOutputBits()
+								& (1 << i)) >> i);
+				io_msg.digital_out_states.push_back(digi);
+			}
+			ur_msgs::Analog ana;
+			ana.pin = 0;
+			ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0();
+			io_msg.analog_in_states.push_back(ana);
+			ana.pin = 1;
+			ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1();
+			io_msg.analog_in_states.push_back(ana);
+
+			ana.pin = 0;
+			ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0();
+			io_msg.analog_out_states.push_back(ana);
+			ana.pin = 1;
+			ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1();
+			io_msg.analog_out_states.push_back(ana);
+			io_pub.publish(io_msg);
+
+			robot_.sec_interface_->robot_state_->finishedReading();
+
+		}
+	}
+
 };
 
 int main(int argc, char **argv) {