Changed test_move.py to use wait_for_message instead of global vars

test_move.py

@@ -13,7 +13,6 @@ JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
 Q1 = [2.2,0,-1.57,0,0,0]
 Q2 = [1.5,0,-1.57,0,0,0]
 Q3 = [1.5,-0.2,-1.57,0,0,0]
-joints_pos = [0.0]*6
     
 client = None
 
@@ -22,20 +21,23 @@ def move1():
     g = FollowJointTrajectoryGoal()
     g.trajectory = JointTrajectory()
     g.trajectory.joint_names = JOINT_NAMES
+    try:
+        joint_states = rospy.wait_for_message("joint_states", JointState)
+        joints_pos = joint_states.position
         g.trajectory.points = [
             JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
             JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
             JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
             JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
         client.send_goal(g)
-    try:
         client.wait_for_result()
     except KeyboardInterrupt:
         client.cancel_goal()
         raise
+    except:
+        raise
 
 def move_disordered():
-    global joints_pos
     order = [4, 2, 3, 1, 5, 0]
     g = FollowJointTrajectoryGoal()
     g.trajectory = JointTrajectory()
@@ -43,6 +45,9 @@ def move_disordered():
     q1 = [Q1[i] for i in order]
     q2 = [Q2[i] for i in order]
     q3 = [Q3[i] for i in order]
+    try:
+        joint_states = rospy.wait_for_message("joint_states", JointState)
+        joints_pos = joint_states.position
         g.trajectory.points = [
             JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
             JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
@@ -50,13 +55,19 @@ def move_disordered():
             JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
         client.send_goal(g)
         client.wait_for_result()
+    except KeyboardInterrupt:
+        client.cancel_goal()
+        raise
+    except:
+        raise
     
 def move_repeated():
-    global joints_pos
     g = FollowJointTrajectoryGoal()
     g.trajectory = JointTrajectory()
     g.trajectory.joint_names = JOINT_NAMES
-    
+    try:
+        joint_states = rospy.wait_for_message("joint_states", JointState)
+        joints_pos = joint_states.position
         d = 2.0
         g.trajectory.points = [JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0))]
         for i in range(10):
@@ -70,17 +81,20 @@ def move_repeated():
                 JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d)))
             d += 2
         client.send_goal(g)
-    try:
         client.wait_for_result()
     except KeyboardInterrupt:
         client.cancel_goal()
         raise
+    except:
+        raise
 
 def move_interrupt():
-    global joints_pos
     g = FollowJointTrajectoryGoal()
     g.trajectory = JointTrajectory()
     g.trajectory.joint_names = JOINT_NAMES
+    try:
+        joint_states = rospy.wait_for_message("joint_states", JointState)
+        joints_pos = joint_states.position
         g.trajectory.points = [
             JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
             JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
@@ -91,16 +105,12 @@ def move_interrupt():
         time.sleep(2.0)
         print "Interrupting"
         client.send_goal(g)
-    try:
         client.wait_for_result()
     except KeyboardInterrupt:
         client.cancel_goal()
         raise
-        
+    except:
-
+        raise
-def joint_subscriber(msg):
-    global joints_pos
-    joints_pos = msg.position
    
 def main():
     global client
@@ -110,7 +120,6 @@ def main():
         print "Waiting for server..."
         client.wait_for_server()
         print "Connected to server"
-        rospy.Subscriber("joint_states", JointState, joint_subscriber)
         parameters = rospy.get_param(None)
         index = str(parameters).find('prefix')
         if (index > 0):