Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 18:10:47 +02:00
Code Releases Activity

Changed test_move.py to use wait_for_message instead of global vars

Browse Source
This commit is contained in:
Thomas Timm Andersen
2016-02-23 09:47:37 +01:00
parent 70279d5ece
commit 91450da173
1 changed files with 61 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
test_move.py
View File

@@ -13,7 +13,6 @@ JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
Q1 = [2.2,0,-1.57,0,0,0] Q1 = [2.2,0,-1.57,0,0,0]
Q2 = [1.5,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] Q3 = [1.5,-0.2,-1.57,0,0,0]
joints_pos = [0.0]*6
client = None client = None
@@ -22,20 +21,23 @@ def move1():
g = FollowJointTrajectoryGoal() g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory() g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES 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: 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() client.wait_for_result()
except KeyboardInterrupt: except KeyboardInterrupt:
client.cancel_goal() client.cancel_goal()
raise raise
except:
raise
def move_disordered(): def move_disordered():
global joints_pos
order = [4, 2, 3, 1, 5, 0] order = [4, 2, 3, 1, 5, 0]
g = FollowJointTrajectoryGoal() g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory() g.trajectory = JointTrajectory()
@@ -43,64 +45,72 @@ def move_disordered():
q1 = [Q1[i] for i in order] q1 = [Q1[i] for i in order]
q2 = [Q2[i] for i in order] q2 = [Q2[i] for i in order]
q3 = [Q3[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: 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() client.wait_for_result()
except KeyboardInterrupt: except KeyboardInterrupt:
client.cancel_goal() client.cancel_goal()
raise 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(): def move_interrupt():
global joints_pos
g = FollowJointTrajectoryGoal() g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory() g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES 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: 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() client.wait_for_result()
except KeyboardInterrupt: except KeyboardInterrupt:
client.cancel_goal() client.cancel_goal()
raise raise
except:
raise
def joint_subscriber(msg):
global joints_pos
joints_pos = msg.position
def main(): def main():
global client global client
@@ -110,7 +120,6 @@ def main():
print "Waiting for server..." print "Waiting for server..."
client.wait_for_server() client.wait_for_server()
print "Connected to server" print "Connected to server"
rospy.Subscriber("joint_states", JointState, joint_subscriber)
parameters = rospy.get_param(None) parameters = rospy.get_param(None)
index = str(parameters).find('prefix') index = str(parameters).find('prefix')
if (index > 0): if (index > 0):