[Documentation] [TitleIndex] [WordIndex

(!) 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

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]


2019-10-12 12:55