Moved everything to rostests

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}

ur_rtde_driver/package.xml

@@ -21,6 +21,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>boost</build_depend>
+  <build_depend>rostest</build_depend>
 
   <depend>actionlib</depend>
   <depend>control_msgs</depend>

ur_rtde_driver/scripts/io_test.py

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

ur_rtde_driver/scripts/trajectory_test.py

@@ -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))

ur_rtde_driver/test/driver.test

@@ -0,0 +1,8 @@
+<launch>
+  <include file="$(find ur_rtde_driver)/launch/ur10_bringup.launch">
+    <arg name="robot_ip" value="192.168.56.101"/>
+  </include>
+
+  <test test-name="io_test" pkg="ur_rtde_driver" type="io_test.py" name="io_test1"/>
+  <test test-name="trajectory_test" pkg="ur_rtde_driver" type="trajectory_test.py" name="traj_test1"/>
+</launch>

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)

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)