Merge pull request #92 from ipa-fxm/IO-Fixed

IO-Support (fixed and rebased)

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)

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>

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

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():

ur_driver/src/ur_driver/io_interface.py

+#!/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)
+

ur_driver/test_io.py

+#!/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)
+
+

ur_msgs/CMakeLists.txt

+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 ##
+#############

ur_msgs/msg/Analog.msg

+uint8 pin
+float32 state

ur_msgs/msg/Digital.msg

+uint8 pin
+bool state

ur_msgs/msg/IOStates.msg

+Digital[] digital_in_states
+Digital[] digital_out_states
+Digital[] flag_states
+Analog[] analog_in_states
+Analog[] analog_out_states

ur_msgs/package.xml

+<?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>

ur_msgs/srv/SetIO.srv

+int8 fun
+int8 pin
+float32 state
+---
+bool success