mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 10:00:48 +02:00
69 lines
1.9 KiB
Python
Executable File
69 lines
1.9 KiB
Python
Executable File
#!/usr/bin/env python
|
|
import sys
|
|
import time
|
|
import unittest
|
|
|
|
|
|
import rospy
|
|
import rostopic
|
|
|
|
|
|
PKG = 'ur_rtde_driver'
|
|
NAME = 'io_test'
|
|
|
|
|
|
from ur_msgs.srv import SetIO
|
|
from ur_msgs.msg import IOStates
|
|
|
|
|
|
class IOTest(unittest.TestCase):
|
|
def __init__(self, *args):
|
|
super(IOTest, self).__init__(*args)
|
|
rospy.init_node('io_test')
|
|
|
|
timeout = 10
|
|
|
|
self.service_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetIO)
|
|
try:
|
|
self.service_client.wait_for_service(timeout)
|
|
except rospy.exceptions.ROSException as err:
|
|
self.fail(
|
|
"Could not reach SetIO service. Make sure that the driver is actually running."
|
|
" Msg: {}".format(err))
|
|
|
|
def test_set_io(self):
|
|
"""Test to set an IO and check whether it has been set."""
|
|
|
|
maximum_messages = 5
|
|
pin = 0
|
|
self.assertEqual(maximum_messages, 5)
|
|
|
|
self.service_client(1, pin, 0)
|
|
messages = 0
|
|
pin_state = True
|
|
|
|
while(pin_state):
|
|
if messages >= maximum_messages:
|
|
self.fail("Could not read desired state after {} messages.".format(maximum_messages))
|
|
io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates)
|
|
pin_state = io_state.digital_out_states[pin].state
|
|
messages += 1
|
|
self.assertEqual(pin_state, 0)
|
|
|
|
self.service_client(1, pin, 1)
|
|
messages = 0
|
|
pin_state = False
|
|
|
|
while(not pin_state):
|
|
if messages >= maximum_messages:
|
|
self.fail("Could not read desired state after {} messages.".format(maximum_messages))
|
|
io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates)
|
|
pin_state = io_state.digital_out_states[pin].state
|
|
messages += 1
|
|
self.assertEqual(pin_state, 1)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
import rostest
|
|
rostest.run(PKG, NAME, IOTest, sys.argv)
|