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
-    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:
+        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)
         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,64 +45,72 @@ def move_disordered():
     q1 = [Q1[i] for i in order]
     q2 = [Q2[i] for i in order]
     q3 = [Q3[i] for i in order]
-    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)
-    client.wait_for_result()
-    
-def move_repeated():
-    global joints_pos
-    g = FollowJointTrajectoryGoal()
-    g.trajectory = JointTrajectory()
-    g.trajectory.joint_names = JOINT_NAMES
-    
-    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):
-        g.trajectory.points.append(
-            JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d)))
-        d += 1
-        g.trajectory.points.append(
-            JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d)))
-        d += 1
-        g.trajectory.points.append(
-            JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d)))
-        d += 2
-    client.send_goal(g)
     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)
         client.wait_for_result()
     except KeyboardInterrupt:
         client.cancel_goal()
         raise
+    except:
+        raise
+    
+def move_repeated():
+    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):
+            g.trajectory.points.append(
+                JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d)))
+            d += 1
+            g.trajectory.points.append(
+                JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d)))
+            d += 1
+            g.trajectory.points.append(
+                JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d)))
+            d += 2
+        client.send_goal(g)
+        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
-    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)
-    time.sleep(2.0)
-    print "Interrupting"
-    client.send_goal(g)
     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)
+        time.sleep(2.0)
+        print "Interrupting"
+        client.send_goal(g)
         client.wait_for_result()
     except KeyboardInterrupt:
         client.cancel_goal()
         raise
-        
-
-def joint_subscriber(msg):
-    global joints_pos
-    joints_pos = msg.position
+    except:
+        raise
    
 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):