diff --git a/ur_rtde_driver/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt index c522c8b..482ef89 100644 --- a/ur_rtde_driver/CMakeLists.txt +++ b/ur_rtde_driver/CMakeLists.txt @@ -113,6 +113,12 @@ add_executable(ur_rtde_driver_node target_link_libraries(ur_rtde_driver_node ${catkin_LIBRARIES} ur_rtde_driver) add_dependencies(ur_rtde_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +if(CATKIN_ENABLE_TESTING) + find_package(catkin COMPONENTS rostest) + + add_rostest(test/driver.test) +endif() + install(TARGETS ur_rtde_driver ur_rtde_driver_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/ur_rtde_driver/package.xml b/ur_rtde_driver/package.xml index 7cf7a0d..0706ab2 100644 --- a/ur_rtde_driver/package.xml +++ b/ur_rtde_driver/package.xml @@ -21,6 +21,7 @@ catkin boost + rostest actionlib control_msgs diff --git a/ur_rtde_driver/scripts/io_test.py b/ur_rtde_driver/scripts/io_test.py deleted file mode 100755 index 0d6d861..0000000 --- a/ur_rtde_driver/scripts/io_test.py +++ /dev/null @@ -1,40 +0,0 @@ -#!/usr/bin/env python - -import rospy -from ur_msgs.srv import SetIO -from ur_msgs.msg import IOStates - -def main(): - rospy.init_node('io_testing_client') - service_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetIO) - service_client.wait_for_service() - - maximum_messages = 5 - pin = 0 - - service_client(1, pin, 0) - messages = 0 - pin_state = True - - while(pin_state): - if messages >= 5: - return False - io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates) - pin_state = io_state.digital_out_states[pin].state - messages += 1 - - service_client(1, pin, 1) - messages = 0 - pin_state = False - - while(not pin_state): - if messages >= 5: - return False - io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates) - pin_state = io_state.digital_out_states[pin].state - messages += 1 - - return True - -if __name__ == '__main__': - print(main()) diff --git a/ur_rtde_driver/scripts/trajectory_test.py b/ur_rtde_driver/scripts/trajectory_test.py deleted file mode 100755 index e8e805d..0000000 --- a/ur_rtde_driver/scripts/trajectory_test.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python - -import rospy -import actionlib -from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectoryPoint - -def main(position_list, duration_list): - rospy.init_node('trajectory_testing_client') - client = actionlib.SimpleActionClient('/scaled_pos_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction) - client.wait_for_server() - goal = FollowJointTrajectoryGoal() - - goal.trajectory.joint_names = ["elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", "wrist_1_joint", - "wrist_2_joint", "wrist_3_joint"] - - for i in range(len(position_list)): - point = JointTrajectoryPoint() - point.positions = position_list[i] - point.time_from_start = rospy.Duration(duration_list[i]) - goal.trajectory.points.append(point) - - client.send_goal(goal) - client.wait_for_result() - - return client.get_result().error_code == 0 - - - - -if __name__ == '__main__': - position_list = [[0.0 for i in range(6)]] - position_list.append([-0.5 for i in range(6)]) - position_list.append([-1.0 for i in range(6)]) - duration_list = [6.0, 9.0, 12.0] - - print(main(position_list, duration_list)) diff --git a/ur_rtde_driver/test/driver.test b/ur_rtde_driver/test/driver.test new file mode 100644 index 0000000..b351822 --- /dev/null +++ b/ur_rtde_driver/test/driver.test @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/ur_rtde_driver/test/io_test.py b/ur_rtde_driver/test/io_test.py new file mode 100755 index 0000000..94c999c --- /dev/null +++ b/ur_rtde_driver/test/io_test.py @@ -0,0 +1,68 @@ +#!/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) diff --git a/ur_rtde_driver/test/trajectory_test.py b/ur_rtde_driver/test/trajectory_test.py new file mode 100755 index 0000000..3423451 --- /dev/null +++ b/ur_rtde_driver/test/trajectory_test.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python +import sys +import time +import unittest + +import rospy +import actionlib +from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal +from trajectory_msgs.msg import JointTrajectoryPoint + +PKG = 'ur_rtde_driver' +NAME = 'trajectory_test' + + +class TrajectoryTest(unittest.TestCase): + def __init__(self, *args): + super(TrajectoryTest, self).__init__(*args) + rospy.init_node('trajectory_testing_client') + self.client = actionlib.SimpleActionClient( + '/scaled_pos_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction) + timeout = rospy.Duration(10) + try: + self.client.wait_for_server(timeout) + except rospy.exceptions.ROSException as err: + self.fail( + "Could not reach controller action. Make sure that the driver is actually running." + " Msg: {}".format(err)) + + rospy.sleep(5) + + def test_trajectory(self): + """Test robot movement""" + goal = FollowJointTrajectoryGoal() + + goal.trajectory.joint_names = ["elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", + "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] + position_list = [[0.0 for i in range(6)]] + position_list.append([-0.5 for i in range(6)]) + position_list.append([-1.0 for i in range(6)]) + duration_list = [6.0, 9.0, 12.0] + + for i, position in enumerate(position_list): + point = JointTrajectoryPoint() + point.positions = position + point.time_from_start = rospy.Duration(duration_list[i]) + goal.trajectory.points.append(point) + + self.client.send_goal(goal) + self.client.wait_for_result() + + self.assertEqual(self.client.get_result().error_code, 0) + + +if __name__ == '__main__': + import rostest + rostest.run(PKG, NAME, TrajectoryTest, sys.argv)