From 91450da1739913c764e979409dece176d77d1055 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 23 Feb 2016 09:47:37 +0100 Subject: [PATCH] Changed test_move.py to use wait_for_message instead of global vars --- test_move.py | 113 +++++++++++++++++++++++++++------------------------ 1 file changed, 61 insertions(+), 52 deletions(-) diff --git a/test_move.py b/test_move.py index 83292a2..fb6b0c5 100755 --- a/test_move.py +++ b/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):