 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Getting the current joint angles for the PR2
Description: This tutorial shows you how to get the current joint angles for the PR2 robot.Keywords: joint angles display joint_states mechanism state
Tutorial Level: BEGINNER
Contents
Overview
In this tutorial, we will show you how to get the current joint angles for the PR2 robot (arm, head, torso, etc.) from within a ROS node, using sensor_msgs/JointState messages.
Before you begin, bring up a robot, either on the hardware or in gazebo.
The joint_states message
The current positions and angles for the robot are by default being broadcast on a message topic called joint_states, of type sensor_msgs/JointState. This message contains arrays of joint names (string[] name), along with the corresponding positions (float64[] position), velocities (float64[] velocity), and efforts (float64[] effort). If you type
rostopic echo joint_states
at the command line after a real or simulated robot has been launched, you can see these messages as they are published.
A handy server
If you happen to want to filter out only a few joint angles and obtain their joint values, you might want to write a server such as this one.
The node we present in this tutorial listens to and keeps track of the most recent joint_states message, and offers a service that returns the current position, velocity, and effort for any set of desired joint names.
The first thing we'll need to do is create a package. To do this we'll use the handy roscreate-pkg command where we want to create the package directory:
roscreate-pkg joint_states_listener rospy sensor_msgs
After this is done we'll need to roscd to the package we created, since we'll be using it as our workspace.
roscd joint_states_listener
Creating the service message
Now we need to define a service message that we can use to ask for specific joint angles, velocities, and efforts. Put the following into srv/ReturnJointStates.srv:
string[] name --- uint32[] found float64[] position float64[] velocity float64[] effort
Now uncomment the line
rosbuild_gensrv()
in your CMakeLists.txt and type make in the joint_states_listener directory to generate the service message files.
Creating the service node
Put the following into nodes/joint_states_listener.py:
1 #!/usr/bin/env python
2 #spins off a thread to listen for joint_states messages
3 #and provides the same information (or subsets of) as a service
4
5 import roslib
6 roslib.load_manifest('joint_states_listener')
7 import rospy
8 from joint_states_listener.srv import *
9 from sensor_msgs.msg import JointState
10 import threading
11
12
13 #holds the latest states obtained from joint_states messages
14 class LatestJointStates:
15
16 def __init__(self):
17 rospy.init_node('joint_states_listener')
18 self.lock = threading.Lock()
19 self.name = []
20 self.position = []
21 self.velocity = []
22 self.effort = []
23 self.thread = threading.Thread(target=self.joint_states_listener)
24 self.thread.start()
25
26 s = rospy.Service('return_joint_states', ReturnJointStates, self.return_joint_states)
27
28
29 #thread function: listen for joint_states messages
30 def joint_states_listener(self):
31 rospy.Subscriber('joint_states', JointState, self.joint_states_callback)
32 rospy.spin()
33
34
35 #callback function: when a joint_states message arrives, save the values
36 def joint_states_callback(self, msg):
37 self.lock.acquire()
38 self.name = msg.name
39 self.position = msg.position
40 self.velocity = msg.velocity
41 self.effort = msg.effort
42 self.lock.release()
43
44
45 #returns (found, position, velocity, effort) for the joint joint_name
46 #(found is 1 if found, 0 otherwise)
47 def return_joint_state(self, joint_name):
48
49 #no messages yet
50 if self.name == []:
51 rospy.logerr("No robot_state messages received!\n")
52 return (0, 0., 0., 0.)
53
54 #return info for this joint
55 self.lock.acquire()
56 if joint_name in self.name:
57 index = self.name.index(joint_name)
58 position = self.position[index]
59 velocity = self.velocity[index]
60 effort = self.effort[index]
61
62 #unless it's not found
63 else:
64 rospy.logerr("Joint %s not found!", (joint_name,))
65 self.lock.release()
66 return (0, 0., 0., 0.)
67 self.lock.release()
68 return (1, position, velocity, effort)
69
70
71 #server callback: returns arrays of position, velocity, and effort
72 #for a list of joints specified by name
73 def return_joint_states(self, req):
74 joints_found = []
75 positions = []
76 velocities = []
77 efforts = []
78 for joint_name in req.name:
79 (found, position, velocity, effort) = self.return_joint_state(joint_name)
80 joints_found.append(found)
81 positions.append(position)
82 velocities.append(velocity)
83 efforts.append(effort)
84 return ReturnJointStatesResponse(joints_found, positions, velocities, efforts)
85
86
87 #run the server
88 if __name__ == "__main__":
89
90 latestjointstates = LatestJointStates()
91
92 print "joints_states_listener server started, waiting for queries"
93 rospy.spin()
Test program
Make the service node script executable. You can then run it using rosrun.
rosrun joint_states_listener joint_states_listener.py
Because our joint_states_listener program runs a service that waits for service calls to come in, we need to write a client program to send it queries.
Put the following in scripts/joint_states_listener_test.py:
1 #!/usr/bin/env python
2 #test client for joint_states_listener
3
4 import roslib
5 roslib.load_manifest('joint_states_listener')
6 import rospy
7 from joint_states_listener.srv import ReturnJointStates
8 import time
9 import sys
10
11 def call_return_joint_states(joint_names):
12 rospy.wait_for_service("return_joint_states")
13 try:
14 s = rospy.ServiceProxy("return_joint_states", ReturnJointStates)
15 resp = s(joint_names)
16 except rospy.ServiceException, e:
17 print "error when calling return_joint_states: %s"%e
18 sys.exit(1)
19 for (ind, joint_name) in enumerate(joint_names):
20 if(not resp.found[ind]):
21 print "joint %s not found!"%joint_name
22 return (resp.position, resp.velocity, resp.effort)
23
24
25 #pretty-print list to string
26 def pplist(list):
27 return ' '.join(['%2.3f'%x for x in list])
28
29
30 #print out the positions, velocities, and efforts of the right arm joints
31 if __name__ == "__main__":
32 joint_names = ["r_shoulder_pan_joint",
33 "r_shoulder_lift_joint",
34 "r_upper_arm_roll_joint",
35 "r_elbow_flex_joint",
36 "r_forearm_roll_joint",
37 "r_wrist_flex_joint",
38 "r_wrist_roll_joint"]
39
40 while(1):
41 (position, velocity, effort) = call_return_joint_states(joint_names)
42 print "position:", pplist(position)
43 print "velocity:", pplist(velocity)
44 print "effort:", pplist(effort)
45 time.sleep(1)
This test client prints the current right arm angles every second.
python scripts/joint_states_listener_test.py position: -0.000 0.025 -0.004 -0.408 -0.002 -0.670 -0.001 velocity: -0.000 -0.003 -0.002 0.001 0.000 0.000 0.001 effort: -0.013 0.022 0.003 -0.007 -0.007 -0.006 -0.005
You can substitute any joint you want (for instance, "torso_lift_joint" to get the height of the torso joint) values for.
Instead of writing a test script, you may also call the service from the command line:
rosservice call /return_joint_states [head_pan_joint,head_tilt_joint] found: [1, 1] position: [4.5344168134064944e-05, 0.95674491330041445] velocity: [-0.0063855156578163227, -0.00031156358650886524] effort: [0.0049046000755979943, -0.95260131666459591]
