5 roslib.load_manifest(
'naoqi_driver')
7 from rospy
import Duration
11 from actionlib_msgs.msg
import GoalStatus
12 import naoqi_bridge_msgs.msg
13 import trajectory_msgs.msg
14 from trajectory_msgs.msg
import JointTrajectoryPoint
26 rospy.loginfo(
"Waiting for joint_trajectory and joint_stiffness servers...")
27 client.wait_for_server()
28 stiffness_client.wait_for_server()
29 angle_client.wait_for_server()
30 rospy.loginfo(
"Done.")
34 goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal()
37 goal.trajectory.joint_names = [
"HeadYaw"]
38 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0), positions = [1.0]))
39 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.0), positions = [-1.0]))
40 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.5), positions = [0.0]))
43 rospy.loginfo(
"Sending goal...")
44 client.send_goal(goal)
45 client.wait_for_result()
46 result = client.get_result()
47 rospy.loginfo(
"Results: %s", str(result.goal_position.position))
50 rospy.loginfo(
"Sending goal again...")
51 client.send_goal(goal)
53 rospy.loginfo(
"Preempting goal...")
55 client.wait_for_result()
56 if client.get_state() != GoalStatus.PREEMPTED
or client.get_result() == result:
57 rospy.logwarn(
"Preemption does not seem to be working")
59 rospy.loginfo(
"Preemption seems okay")
62 goal.trajectory.joint_names = [
"Body"]
63 point = JointTrajectoryPoint()
64 point.time_from_start = Duration(1.5)
65 point.positions = [0.0,0.0,
66 1.545, 0.33, -1.57, -0.486, 0.0, 0.0,
67 -0.3, 0.057, -0.744, 2.192, -1.122, -0.035,
68 -0.3, 0.057, -0.744, 2.192, -1.122, -0.035,
69 1.545, -0.33, 1.57, 0.486, 0.0, 0.0]
70 goal.trajectory.points = [point]
72 rospy.loginfo(
"Sending goal...")
73 client.send_goal(goal)
74 client.wait_for_result()
75 rospy.loginfo(
"Getting results...")
76 result = client.get_result()
77 print "Result:",
', '.join([str(n)
for n
in result.goal_position.position])
81 goal.trajectory.joint_names = [
"HeadYaw",
"HeadPitch"]
82 goal.trajectory.points = []
83 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5),
84 positions = [1.0, 1.0]))
85 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0),
86 positions = [1.0, 0.0]))
87 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.5),
88 positions = [0.0, 0.0]))
90 rospy.loginfo(
"Sending goal...")
91 client.send_goal(goal)
92 client.wait_for_result()
93 rospy.loginfo(
"Getting results...")
94 result = client.get_result()
95 print "Result:",
', '.join([str(n)
for n
in result.goal_position.position])
99 goal.trajectory.joint_names = [
"HeadYaw",
"HeadPitch"]
100 goal.trajectory.points = []
101 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5),
102 positions = [0.5, 0.5]))
104 rospy.loginfo(
"Sending goal...")
105 client.send_goal(goal)
106 client.wait_for_result()
107 rospy.loginfo(
"Getting results...")
108 result = client.get_result()
109 print "Result:",
', '.join([str(n)
for n
in result.goal_position.position])
113 angle_goal = naoqi_bridge_msgs.msg.JointAnglesWithSpeedGoal()
114 angle_goal.joint_angles.relative =
False 115 angle_goal.joint_angles.joint_names = [
"HeadYaw",
"HeadPitch"]
116 angle_goal.joint_angles.joint_angles = [1.0, 0.0]
117 angle_goal.joint_angles.speed = 0.2
118 rospy.loginfo(
"Sending joint angles goal...")
119 angle_client.send_goal_and_wait(angle_goal)
120 result = angle_client.get_result()
121 rospy.loginfo(
"Angle results: %s", str(result.goal_position.position))
124 angle_goal.joint_angles.joint_angles = [-1.0, 0.0]
125 angle_goal.joint_angles.speed = 0.05
126 rospy.loginfo(
"Sending goal again...")
127 angle_client.send_goal(angle_goal)
129 rospy.loginfo(
"Preempting goal...")
130 angle_client.cancel_goal()
131 angle_client.wait_for_result()
132 if angle_client.get_state() != GoalStatus.PREEMPTED
or angle_client.get_result() == result:
133 rospy.logwarn(
"Preemption does not seem to be working")
135 rospy.loginfo(
"Preemption seems okay")
138 stiffness_goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal()
139 stiffness_goal.trajectory.joint_names = [
"Body"]
140 stiffness_goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [1.0]))
141 rospy.loginfo(
"Sending stiffness goal...")
142 stiffness_client.send_goal(stiffness_goal)
143 stiffness_client.wait_for_result()
144 result = stiffness_client.get_result()
145 rospy.loginfo(
"Stiffness results: %s", str(result.goal_position.position))
148 stiffness_goal.trajectory.points = [JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [0.0])]
149 rospy.loginfo(
"Sending goal again...")
150 stiffness_client.send_goal(stiffness_goal)
152 rospy.loginfo(
"Preempting goal...")
153 stiffness_client.cancel_goal()
154 stiffness_client.wait_for_result()
155 if stiffness_client.get_state() != GoalStatus.PREEMPTED
or stiffness_client.get_result() == result:
156 rospy.logwarn(
"Preemption does not seem to be working")
158 rospy.loginfo(
"Preemption seems okay")
165 if __name__ ==
'__main__':
167 rospy.init_node(
'joint_angle_client')
170 except rospy.ROSInterruptException:
171 print "program interrupted before completion"