diff --git a/ur_driver/CMakeLists.txt b/ur_driver/CMakeLists.txt
index 1c389a306fe920a6a27e5e260355fd307039226f..7a7e1057572b52fd5c79623c92a0027ceb0c377c 100644
--- a/ur_driver/CMakeLists.txt
+++ b/ur_driver/CMakeLists.txt
@@ -6,85 +6,19 @@ project(ur_driver)
## is used, also find other catkin packages
find_package(catkin REQUIRED)
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
catkin_python_setup()
-#######################################
-## Declare ROS messages and services ##
-#######################################
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-# FILES
-# Service1.srv
-# Service2.srv
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# control_msgs# sensor_msgs
-# )
###################################
## catkin specific configuration ##
###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package()
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-# include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
-
-## Declare a cpp library
-# add_library(ur_driver
-# src/${PROJECT_NAME}/ur_driver.cpp
-# )
-
-## Declare a cpp executable
-# add_executable(ur_driver_node src/ur_driver_node.cpp)
-
-## Add cmake target dependencies of the executable/library
-## as an example, message headers may need to be generated before nodes
-# add_dependencies(ur_driver_node ur_driver_generate_messages_cpp)
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(ur_driver_node
-# ${catkin_LIBRARIES}
-# )
-
#############
## Install ##
#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-install(PROGRAMS test_move.py
+install(PROGRAMS test_move.py test_io.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
@@ -92,21 +26,6 @@ install(PROGRAMS src/ur_driver/driver.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
-## Mark executables and/or libraries for installation
-# install(TARGETS ur_driver ur_driver_node
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-# FILES_MATCHING PATTERN "*.h"
-# PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES prog prog_reset
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
@@ -114,12 +33,3 @@ install(FILES prog prog_reset
#############
## Testing ##
#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_ur_driver.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
diff --git a/ur_driver/package.xml b/ur_driver/package.xml
index e935770a53136af89b4f57fc4e625d70f30cb0a6..2f64d4fdb89fdcb91e645e3db59274753c7dba9a 100644
--- a/ur_driver/package.xml
+++ b/ur_driver/package.xml
@@ -6,58 +6,24 @@
Driver for the UR5/10 arm based on the Polyscope control scheme.
</description>
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="aub@ipa.fhg.de">Alexander Bubeck</maintainer>
-
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
-
-
- <!-- Url tags are optional, but mutiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
<url type="website">http://ros.org/wiki/ur_driver</url>
-
-
- <!-- Author tags are optional, mutiple are allowed, one per tag -->
- <!-- Authors do not have to be maintianers, but could be -->
- <!-- Example: -->
<author>Stuart Glaser</author>
<author email="sedwards@swri.org">Shaun Edwards</author>
- <!-- The *_depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use run_depend for packages you need at runtime: -->
- <!-- <run_depend>message_runtime</run_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
+ <run_depend>rospy</run_depend>
<run_depend>actionlib</run_depend>
<run_depend>control_msgs</run_depend>
- <run_depend>rospy</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
+ <run_depend>ur_msgs</run_depend>
<run_depend>python-beautifulsoup</run_depend>
- <!-- The export tag contains other, unspecified, tags -->
<export>
- <!-- You can specify that this package is a metapackage here: -->
- <!-- <metapackage/> -->
-
- <!-- Other tools can request additional information be placed here -->
-
</export>
</package>
diff --git a/ur_driver/prog b/ur_driver/prog
index 66ebf4fe6d2659f049ab6ad2a8e6734bb3660262..6f118e1a5880171dd56f9cb3e14f73ec34aaaae0 100644
--- a/ur_driver/prog
+++ b/ur_driver/prog
@@ -9,6 +9,11 @@ def driverProg():
MSG_STOPJ = 6
MSG_SERVOJ = 7
MSG_WRENCH = 9
+ MSG_SET_DIGITAL_OUT = 10
+ MSG_GET_IO = 11
+ MSG_SET_FLAG = 12
+ MSG_SET_TOOL_VOLTAGE = 13
+ MSG_SET_ANALOG_OUT = 14
MULT_wrench = 10000.0
MULT_jointstate = 10000.0
MULT_time = 1000000.0
@@ -63,6 +68,16 @@ def driverProg():
socket_send_int(floor(MULT_wrench * wrench[3]))
socket_send_int(floor(MULT_wrench * wrench[4]))
socket_send_int(floor(MULT_wrench * wrench[5]))
+ socket_send_int(MSG_GET_IO)
+ socket_send_int(1*get_digital_in(0) + 2*get_digital_in(1) + 4 * get_digital_in(2) + 8 * get_digital_in(3) + 16 * get_digital_in(4) + 32 * get_digital_in(5) + 64 * get_digital_in(6) + 128 * get_digital_in(7) + 256 * get_digital_in(8) + 512 * get_digital_in(9))
+ socket_send_int(1*get_digital_out(0) + 2*get_digital_out(1) + 4 * get_digital_out(2) + 8 * get_digital_out(3) + 16 * get_digital_out(4) + 32 * get_digital_out(5) + 64 * get_digital_out(6) + 128 * get_digital_out(7) + 256 * get_digital_out(8) + 512 * get_digital_out(9))
+ socket_send_int(pow(2,0)*get_flag(0) + pow(2,1)*get_flag(1) + pow(2,2)*get_flag(2) + pow(2,3)*get_flag(3) + pow(2,4)*get_flag(4) + pow(2,5)*get_flag(5) + pow(2,6)*get_flag(6) + pow(2,7)*get_flag(7) + pow(2,8)*get_flag(8) + pow(2,9)*get_flag(9) + pow(2,10)*get_flag(10) + pow(2,11)*get_flag(11) + pow(2,12)*get_flag(12) + pow(2,13)*get_flag(13) + pow(2,14)*get_flag(14) + pow(2,15)*get_flag(15) + pow(2,16)*get_flag(16) + pow(2,17)*get_flag(17) + pow(2,18)*get_flag(18) + pow(2,19)*get_flag(19) + pow(2,20)*get_flag(20) + pow(2,21)*get_flag(21) + pow(2,22)*get_flag(22) + pow(2,23)*get_flag(23) + pow(2,24)*get_flag(24) + pow(2,25)*get_flag(25) + pow(2,26)*get_flag(26) + pow(2,27)*get_flag(27) + pow(2,28)*get_flag(28) + pow(2,29)*get_flag(29) + pow(2,30)*get_flag(30) + pow(2,31)*get_flag(31) + pow(2,32)*get_flag(32))
+ socket_send_int(get_analog_out(0)*1000000)
+ socket_send_int(get_analog_out(1)*1000000)
+ socket_send_int(get_analog_in(0)*1000000)
+ socket_send_int(get_analog_in(1)*1000000)
+ socket_send_int(get_analog_in(2)*1000000)
+ socket_send_int(get_analog_in(3)*1000000)
#socket_send_int(7895160) # Recognizable ".xxx" or 00787878
exit_critical
end
@@ -205,6 +220,46 @@ def driverProg():
elif mtype == MSG_STOPJ:
send_out("Received stopj")
stopj(1.0)
+ elif mtype == MSG_SET_DIGITAL_OUT:
+ #send_out("Received Digital Out Signal")
+ # Reads the parameters
+ params_mult = socket_read_binary_integer(1+6+1)
+ if params_mult[0] == 0:
+ send_out("Received no parameters for set_out message")
+ end
+ if params_mult[2] > 0:
+ set_digital_out(params_mult[1], True)
+ elif params_mult[2] == 0:
+ set_digital_out(params_mult[1], False)
+ end
+ elif mtype == MSG_SET_FLAG:
+ #send_out("Received Set Flag Signal")
+ # Reads the parameters
+ params_mult = socket_read_binary_integer(1+6+1)
+ if params_mult[0] == 0:
+ send_out("Received no parameters for set_out message")
+ end
+ if params_mult[2] != 0:
+ set_flag(params_mult[1], True)
+ elif params_mult[2] == 0:
+ set_flag(params_mult[1], False)
+ end
+ elif mtype == MSG_SET_ANALOG_OUT:
+ #send_out("Received Analog Out Signal")
+ # Reads the parameters
+ params_mult = socket_read_binary_integer(1+6+1)
+ if params_mult[0] == 0:
+ send_out("Received no parameters for set_out message")
+ end
+ set_analog_out(params_mult[1], (params_mult[2] / 1000000))
+ elif mtype == MSG_SET_TOOL_VOLTAGE:
+ #send_out("Received Tool Voltage Signal")
+ # Reads the parameters
+ params_mult = socket_read_binary_integer(1+6+1)
+ if params_mult[0] == 0:
+ send_out("Received no parameters for set_out message")
+ end
+ set_tool_voltage(params_mult[1])
else:
send_out("Received unknown message type")
end
diff --git a/ur_driver/src/ur_driver/driver.py b/ur_driver/src/ur_driver/driver.py
index 1a63b5566a72d6d4a64db880d46890f33d5ccc84..85e6785377ad99ec168987d6aa4fea64f178e929 100755
--- a/ur_driver/src/ur_driver/driver.py
+++ b/ur_driver/src/ur_driver/driver.py
@@ -19,6 +19,14 @@ from geometry_msgs.msg import WrenchStamped
from ur_driver.deserialize import RobotState, RobotMode
+from ur_msgs.srv import SetIO
+from ur_msgs.msg import *
+
+# renaming classes
+DigitalIn = Digital
+DigitalOut = Digital
+Flag = Digital
+
prevent_programming = False
# Joint offsets, pulled from calibration information stored in the URDF
@@ -39,11 +47,23 @@ MSG_WAYPOINT_FINISHED = 5
MSG_STOPJ = 6
MSG_SERVOJ = 7
MSG_WRENCH = 9
+MSG_SET_DIGITAL_OUT = 10
+MSG_GET_IO = 11
+MSG_SET_FLAG = 12
+MSG_SET_TOOL_VOLTAGE = 13
+MSG_SET_ANALOG_OUT = 14
MULT_wrench = 10000.0
MULT_jointstate = 10000.0
MULT_time = 1000000.0
MULT_blend = 1000.0
+FUN_SET_DIGITAL_OUT = 1
+FUN_SET_FLAG = 2
+FUN_SET_ANALOG_OUT = 3
+FUN_SET_TOOL_VOLTAGE = 4
+
+IO_SLEEP_TIME = 0.05
+
JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
@@ -57,6 +77,7 @@ connected_robot_lock = threading.Lock()
connected_robot_cond = threading.Condition(connected_robot_lock)
pub_joint_states = rospy.Publisher('joint_states', JointState)
pub_wrench = rospy.Publisher('wrench', WrenchStamped)
+pub_io_states = rospy.Publisher('io_states', IOStates)
#dump_state = open('dump_state', 'wb')
class EOF(Exception): pass
@@ -321,6 +342,75 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
wrench_msg.wrench.torque.z = state[5]
pub_wrench.publish(wrench_msg)
+ #gets all IO States and publishes them into a message
+ elif mtype == MSG_GET_IO:
+
+ #gets digital in states
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ buf = buf[4:]
+ msg = IOStates()
+ for i in range(0, 10):
+ msg.digital_in_states.append(DigitalIn(i, (inp & (1<<i))>>i))
+ #gets digital out states
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ buf = buf[4:]
+ for i in range(0, 10):
+ msg.digital_out_states.append(DigitalOut(i, (inp & (1<<i))>>i))
+ #gets flag states
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ buf = buf[4:]
+ for i in range(0, 32):
+ msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
+ #gets analog_out[0] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_out_states.append(Analog(0, inp))
+ #gets analog_out[1] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_out_states.append(Analog(1, inp))
+ #gets analog_in[0] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_in_states.append(Analog(0, inp))
+ #gets analog_in[1] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_in_states.append(Analog(1, inp))
+ #gets analog_in[2] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_in_states.append(Analog(2, inp))
+ #gets analog_in[3] state
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ inp = struct.unpack_from("!i", buf, 0)[0]
+ inp /= 1000000.0
+ buf = buf[4:]
+ msg.analog_in_states.append(Analog(3, inp))
+ pub_io_states.publish(msg)
+
elif mtype == MSG_QUIT:
print "Quitting"
raise EOF("Received quit")
@@ -354,7 +444,48 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
buf = struct.pack("!%ii" % len(params), *params)
with self.socket_lock:
self.request.send(buf)
-
+
+ #Experimental set_digital_output implementation
+ def set_digital_out(self, pinnum, value):
+ params = [MSG_SET_DIGITAL_OUT] + \
+ [pinnum] + \
+ [value]
+ buf = struct.pack("!%ii" % len(params), *params)
+ #print params
+ with self.socket_lock:
+ self.request.send(buf)
+ time.sleep(IO_SLEEP_TIME)
+
+ def set_analog_out(self, pinnum, value):
+ params = [MSG_SET_ANALOG_OUT] + \
+ [pinnum] + \
+ [value * 1000000]
+ buf = struct.pack("!%ii" % len(params), *params)
+ #print params
+ with self.socket_lock:
+ self.request.send(buf)
+ time.sleep(IO_SLEEP_TIME)
+
+ def set_tool_voltage(self, value):
+ params = [MSG_SET_TOOL_VOLTAGE] + \
+ [value] + \
+ [0]
+ buf = struct.pack("!%ii" % len(params), *params)
+ #print params
+ with self.socket_lock:
+ self.request.send(buf)
+ time.sleep(IO_SLEEP_TIME+.5)
+
+ def set_flag(self, pin, val):
+ params = [MSG_SET_FLAG] + \
+ [pin] + \
+ [val]
+ buf = struct.pack("!%ii" % len(params), *params)
+ #print params
+ with self.socket_lock:
+ self.request.send(buf)
+ #set_flag will fail if called too closely together--added delay
+ time.sleep(IO_SLEEP_TIME)
def send_stopj(self):
with self.socket_lock:
@@ -673,6 +804,27 @@ def get_my_ip(robot_ip, port):
s.close()
return tmp
+def handle_set_io(req):
+ r = getConnectedRobot(wait=False)
+ if r:
+ if req.fun == FUN_SET_DIGITAL_OUT:
+ r.set_digital_out(req.pin, req.state)
+ return True
+ elif req.fun == FUN_SET_FLAG:
+ r.set_flag(req.pin, req.state)
+ return True
+ elif req.fun == FUN_SET_ANALOG_OUT:
+ r.set_analog_out(req.pin, req.state)
+ return True
+ elif req.fun == FUN_SET_TOOL_VOLTAGE:
+ r.set_tool_voltage(req.pin)
+ return True
+ else:
+ raise ROSServiceException("Robot not connected")
+
+def set_io_server():
+ s= rospy.Service('set_io', SetIO, handle_set_io)
+
def main():
rospy.init_node('ur_driver', disable_signals=True)
if rospy.get_param("use_sim_time", False):
@@ -721,6 +873,8 @@ def main():
connection.connect()
connection.send_reset_program()
+ set_io_server()
+
action_server = None
try:
while not rospy.is_shutdown():
diff --git a/ur_driver/src/ur_driver/io_interface.py b/ur_driver/src/ur_driver/io_interface.py
new file mode 100644
index 0000000000000000000000000000000000000000..a7fa6d7ba65bdb9ac2509fdf4937a9076afbf0de
--- /dev/null
+++ b/ur_driver/src/ur_driver/io_interface.py
@@ -0,0 +1,72 @@
+#!/usr/bin/env python
+
+import sys
+import rospy
+from ur_msgs.srv import *
+from ur_msgs.msg import *
+
+FUN_SET_DIGITAL_OUT = 1
+FUN_SET_FLAG = 2
+FUN_SET_ANALOG_OUT = 3
+FUN_SET_TOOL_VOLTAGE = 4
+
+Flag_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+Digital_Out_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+Digital_In_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+Analog_Out_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+Analog_In_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+
+ANALOG_TOLERANCE_VALUE = 0.01
+DELAY_TIME = 0.3
+i = 0
+set_io = rospy.ServiceProxy('set_io', SetIO)
+
+def set_io_val(fun, pin, val):
+ rospy.wait_for_service('set_io')
+ try:
+ set_io(fun, pin, val)
+ except rospy.ServiceException, e:
+ print "Service call failed: %s"%e
+
+def set_tool_voltage(volts):
+ set_io_val(FUN_SET_TOOL_VOLTAGE, volts, 0)
+
+def set_digital_out(pin, val):
+ set_io_val(FUN_SET_DIGITAL_OUT, pin, val)
+ if Digital_Out_States[pin] != val:
+ rospy.logwarn("DIGITAL OUT IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
+ set_digital_out(pin, val)
+
+def set_analog_out(pin, val):
+ set_io_val(FUN_SET_ANALOG_OUT, pin, val)
+ if abs(Analog_Out_States[pin] - val) > ANALOG_TOLERANCE_VALUE:
+ rospy.logwarn("ANALOG OUT IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
+ set_digital_out(pin, val)
+
+def set_flag(pin, val):
+ set_io_val(FUN_SET_FLAG, pin, val)
+ if Flag_States[pin] != val:
+ rospy.logwarn("SETTING A FLAG IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
+ set_digital_out(pin, val)
+
+def callback(data):
+ for i in range(0,32):
+ del Flag_States[i]
+ Flag_States.insert(i, data.flag_states[i].state)
+ for i in range(0,10):
+ del Digital_Out_States[i]
+ Digital_Out_States.insert(i, data.digital_out_states[i].state)
+ for i in range(0,10):
+ del Digital_In_States[i]
+ Digital_In_States.insert(i, data.digital_in_states[i].state)
+ for i in range(0,2):
+ del Analog_Out_States[i]
+ Analog_Out_States.insert(i, data.analog_out_states[i].state)
+ for i in range(0,4):
+ del Analog_In_States[i]
+ Analog_In_States.insert(i, data.analog_in_states[i].state)
+
+def get_states():
+ rospy.init_node('UR_State_Getter')
+ rospy.Subscriber("io_states", IOStates, callback)
+
diff --git a/ur_driver/test_io.py b/ur_driver/test_io.py
new file mode 100755
index 0000000000000000000000000000000000000000..1d62d1703f8026e7d31ae11c20cda9352a45c358
--- /dev/null
+++ b/ur_driver/test_io.py
@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+import time
+from ur_driver.io_interface import *
+
+if __name__ == "__main__":
+ print "toggling things"
+ get_states()
+ print "listener has been activated"
+ while(True):
+ set_tool_voltage(12)
+ set_digital_out(0, True)
+ set_analog_out(0, 0.75)
+ set_flag(0, True)
+ print(Flag_States[0])
+ print(Analog_Out_States[0])
+ print(Digital_Out_States[0])
+ time.sleep(1)
+ set_tool_voltage(24)
+ set_digital_out(0, False)
+ set_analog_out(0, 0.25)
+ set_flag(0, False)
+ print(Flag_States[0])
+ print(Analog_Out_States[0])
+ print(Digital_Out_States[0])
+ time.sleep(1)
+
+
diff --git a/ur_msgs/CMakeLists.txt b/ur_msgs/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f9bd5a2a98906411f9901c8660a0d159f095633d
--- /dev/null
+++ b/ur_msgs/CMakeLists.txt
@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ur_msgs)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
+
+
+## Generate messages in the 'msg' folder
+ add_message_files(
+ FILES
+ Analog.msg
+ Digital.msg
+ IOStates.msg
+ )
+
+## Generate services in the 'srv' folder
+ add_service_files(
+ FILES
+ SetIO.srv
+ )
+
+
+## Generate added messages and services with any dependencies listed here
+ generate_messages(
+ DEPENDENCIES
+ std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES ur_msgs
+ CATKIN_DEPENDS message_runtime std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+#############
+## Install ##
+#############
+
+#############
+## Testing ##
+#############
diff --git a/ur_msgs/msg/Analog.msg b/ur_msgs/msg/Analog.msg
new file mode 100644
index 0000000000000000000000000000000000000000..9015f6c49eadcaec438c39ca1c3343073963a80d
--- /dev/null
+++ b/ur_msgs/msg/Analog.msg
@@ -0,0 +1,2 @@
+uint8 pin
+float32 state
diff --git a/ur_msgs/msg/Digital.msg b/ur_msgs/msg/Digital.msg
new file mode 100644
index 0000000000000000000000000000000000000000..6183945954e683178d10d2fde80da6ed1d314de9
--- /dev/null
+++ b/ur_msgs/msg/Digital.msg
@@ -0,0 +1,2 @@
+uint8 pin
+bool state
diff --git a/ur_msgs/msg/IOStates.msg b/ur_msgs/msg/IOStates.msg
new file mode 100644
index 0000000000000000000000000000000000000000..e7195735371cd94d40d749828776b6afd3b5b9b5
--- /dev/null
+++ b/ur_msgs/msg/IOStates.msg
@@ -0,0 +1,5 @@
+Digital[] digital_in_states
+Digital[] digital_out_states
+Digital[] flag_states
+Analog[] analog_in_states
+Analog[] analog_out_states
diff --git a/ur_msgs/package.xml b/ur_msgs/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..dacd053bf603ea52f7ff4fa188d9c6734e6694f5
--- /dev/null
+++ b/ur_msgs/package.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package>
+ <name>ur_msgs</name>
+ <version>0.0.0</version>
+ <description>The ur_msgs package</description>
+ <maintainer email="glusia@rpi.edu">Andrew Glusiec</maintainer>
+ <license>BSD</license>
+ <author email="glusia@rpi.edu">Andrew Glusiec</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>message_generation</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <run_depend>message_runtime</run_depend>
+ <run_depend>std_msgs</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ </export>
+</package>
diff --git a/ur_msgs/srv/SetIO.srv b/ur_msgs/srv/SetIO.srv
new file mode 100644
index 0000000000000000000000000000000000000000..13fea4fc90e95086879f9f5cac8ceace578d098d
--- /dev/null
+++ b/ur_msgs/srv/SetIO.srv
@@ -0,0 +1,5 @@
+int8 fun
+int8 pin
+float32 state
+---
+bool success