Wait for controller action server in test_move. (UniversalRobots#535)

Author: adamheins

ur_robot_driver/scripts/test_move

@@ -107,6 +107,12 @@ class TrajectoryClient:
             FollowJointTrajectoryAction,
         )
 
+        # Wait for action server to be ready
+        timeout = rospy.Duration(5)
+        if not trajectory_client.wait_for_server(timeout):
+            rospy.logerr("Could not reach controller action server.")
+            sys.exit(-1)
+
         # Create and fill trajectory goal
         goal = FollowJointTrajectoryGoal()
         goal.trajectory.joint_names = JOINT_NAMES
@@ -143,6 +149,12 @@ class TrajectoryClient:
             FollowCartesianTrajectoryAction,
         )
 
+        # Wait for action server to be ready
+        timeout = rospy.Duration(5)
+        if not trajectory_client.wait_for_server(timeout):
+            rospy.logerr("Could not reach controller action server.")
+            sys.exit(-1)
+
         # The following list are arbitrary positions
         # Change to your own needs if desired
         pose_list = [