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.

Moving the head

Overview and prerequisites

This tutorial shows how to move the head of the PR-2 robot. It assumes that you are interested in making the head point in a given direction, but do not care about how the movement is achieved internally, i.e. what the characteristics of the controller are.

Commanding the head to point in the desired direction requires three components:

• a trajectory controller that directly sends commands to the joint motors;
• an interface to this controller in the form of an Action that accepts as a command a point in Cartesian space to point to, and converts that to low-level commands for the controller;
• the higher level program that sends desired points to the action node.

The first two of these components are available in ROS. In this tutorial, we will show you how to use them by writing the third component, the higher level program.

Before you begin, bring up a robot, either on the hardware or in gazebo.

The Head Trajectory controller

The head trajectory controller is brought up on robot start-up. In general, a controller can be in one of three states:

• not loaded
• loaded, but not running (stopped)
• running

You can use the pr2_controller_manager to check which controllers are available. After you bring up the robot, use the following command:

rosrun pr2_controller_manager pr2_controller_manager list

In the list that is printed, look for a line starting with head_traj_controller . If you find:

• head_traj_controller (running) : the controller is running; you can skip to the next section of this tutorial

• head_traj_controller (stopped) : the controller is loaded, but not running. To start it, use again the pr2_controller_manager :

rosrun pr2_controller_manager pr2_controller_manager start head_traj_controller

• no mention of head_traj_controller : the controller has not been loaded; this probably means that something went wrong during robot start-up. Abort this tutorial and investigate the cause for that.

Note that this usage of the pr2_controller_manager also applies to other controllers, such as the arm trajectory controller or the gripper controller.

The Action interface to the Head Controller

The ROS node that provide the Action interface to the Head Trajectory controller is also brought up automatically on robot start-up. To check for it, you can look at the list of active nodes in the head_traj_controller namespace:

rosnode list head_traj_controller

Look for the line /head_traj_controller/point_head_action . If you don't find it, the action node was not brought up at robot start-up. Abort this tutorial and investigate the cause for that.

The goal message

The action interface to the head controller receives goals in the form of PointHeadGoal messages. These messages contain:

• a target in the form of a PointStamped, with the units in meters. This is a key point of using the tf transform library: you can specify your target in any coordinate frame that tf knows about, and all conversion will be performed by tf for you, behind the scenes. If you are unfamiliar with the PointStamped type, or with data stamped with tf frame ids in general, please review the tf tutorials.

• a pointing frame, which is the id of the frame you want pointing at the target .

• a pointing axis, which is the axis in your pointing frame that you want pointing at the target

• pointing_frame and pointing_axis ARE NOT IMPLEMENTED in Diamondback or before. The actual pointing frame in those distributions is head_tilt_link.

• EXAMPLE: assume you want the high def camera mounted on the head to be pointing at the point (5,0,1.2) expressed in the base_link coordinate frame. (The base_link coordinate frame is located at the bottom of the robot torso (about 5 cm off the ground), in the center of the four wheels, with x pointing forward, y to the left, and z up. Also, the robot is somewhere around 1.25 meters tall.) Your goal message would look like this:

  • pr2_controllers_msgs::PointHeadGoal goal;
    
    //the point to be looking at is expressed in the "base_link" frame
    geometry_msgs::PointStamped point;
    point.header.frame_id = "base_link";
    point.point.x = 5; 
    point.point.y = 0; 
    point.point.z = 1.2;
    goal.target = point;
    
    //we want the X axis of the camera frame to be pointing at the target
    goal.pointing_frame = "high_def_frame";
    goal.pointing_axis.x = 1;
    goal.pointing_axis.y = 0;
    goal.pointing_axis.z = 0;

• a min_duration field: limits the head speed by specifying a minimum time to reach the goal

• a max_velocity field: limits the head velocity directly (rad/s)

Either or both of the min_duration or the max_velocity can be left unspecified, in which case they are ignored. If both are unspecified, the head will reach its goal as fast as possible.

Creating Your Package

The first step is to create a package for the code in this tutorial.

roscreate-pkg simple_head actionlib roscpp pr2_controllers_msgs

Note the dependencies of the package: roscpp, actionlib (since we will be using the action interface to the head trajectory controller) and pr2_controllers_msgs which is where the goal messages for the action are defined.

Sending Position Commands

We are now ready to send position commands using the Action interface to the Head Trajectory controller. Put the following into src/point_head.cpp:

   1 #include <ros/ros.h>
   2 
   3 #include <actionlib/client/simple_action_client.h>
   4 #include <pr2_controllers_msgs/PointHeadAction.h>
   5 
   6 // Our Action interface type, provided as a typedef for convenience
   7 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;
   8 
   9 class RobotHead
  10 {
  11 private:
  12   PointHeadClient* point_head_client_;
  13 
  14 public:
  15   //! Action client initialization 
  16   RobotHead()
  17   {
  18     //Initialize the client for the Action interface to the head controller
  19     point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);
  20 
  21     //wait for head controller action server to come up 
  22     while(!point_head_client_->waitForServer(ros::Duration(5.0))){
  23       ROS_INFO("Waiting for the point_head_action server to come up");
  24     }
  25   }
  26 
  27   ~RobotHead()
  28   {
  29     delete point_head_client_;
  30   }
  31 
  32   //! Points the high-def camera frame at a point in a given frame  
  33   void lookAt(std::string frame_id, double x, double y, double z)
  34   {
  35     //the goal message we will be sending
  36     pr2_controllers_msgs::PointHeadGoal goal;
  37 
  38     //the target point, expressed in the requested frame
  39     geometry_msgs::PointStamped point;
  40     point.header.frame_id = frame_id;
  41     point.point.x = x; point.point.y = y; point.point.z = z;
  42     goal.target = point;
  43 
  44     //we are pointing the high-def camera frame 
  45     //(pointing_axis defaults to X-axis)
  46     goal.pointing_frame = "high_def_frame";
  47     goal.pointing_axis.x = 1;
  48     goal.pointing_axis.y = 0;
  49     goal.pointing_axis.z = 0;
  50 
  51     //take at least 0.5 seconds to get there
  52     goal.min_duration = ros::Duration(0.5);
  53 
  54     //and go no faster than 1 rad/s
  55     goal.max_velocity = 1.0;
  56 
  57     //send the goal
  58     point_head_client_->sendGoal(goal);
  59 
  60     //wait for it to get there (abort after 2 secs to prevent getting stuck)
  61     point_head_client_->waitForResult(ros::Duration(2));
  62   }
  63 
  64   //! Shake the head from left to right n times  
  65   void shakeHead(int n)
  66   {
  67     int count = 0;
  68     while (ros::ok() && ++count <= n )
  69     {
  70       //Looks at a point forward (x=5m), slightly left (y=1m), and 1.2m up
  71       lookAt("base_link", 5.0, 1.0, 1.2);
  72 
  73       //Looks at a point forward (x=5m), slightly right (y=-1m), and 1.2m up
  74       lookAt("base_link", 5.0, -1.0, 1.2);
  75     }
  76   }
  77 };
  78 
  79 int main(int argc, char** argv)
  80 {
  81   //init the ROS node
  82   ros::init(argc, argv, "robot_driver");
  83 
  84   RobotHead head;
  85   head.shakeHead(3);
  86 }

Putting it All Together

To compile the code above into an executable, add the following line into your CMakeLists.txt :

rosbuild_add_executable(point_head src/point_head.cpp)

Compile your package using rosmake .

We are now ready to run. After you bring up the robot(either in simulation or on the hardware) remember to check that both the controller and the interface node are running. Then, run the point_head executable. You should see the head turning from side to side.

Icing: Make the Head Track the Gripper

Now we'll add some icing on the cake: make the head look at the gripper as the gripper moves around. Sounds complicated, right? Well, it's not. The Transform Library tf does all the heavy lifting for us.

We need to tell the head controller interface that we want the head to be pointing at the gripper. Our goal point for the head is thus the point (0,0,0) in the r_gripper_tool_frame. However, we don't have to know ourselves what the r_gripper_tool_frame actually is at a particular point in time; tf publishes that information, and the head controller will get it directly from tf. The loop in your main function can look like this:

   1 while (ros::ok())
   2 {
   3   head.lookAt("r_gripper_tool_frame", 0, 0, 0);
   4   usleep(50000);
   5 }

You can test how well the head tracks the gripper by manually moving the arm around with the arm controllers stopped while running your head tracking node.

For smoother tracking, you can also remove the min_duration field from your goals:

    //take at least 0.5 seconds to get there
    goal.min_duration = ros::Duration(0.5);

as well as the command that waits for completion of the previously sent goal:

    //wait for it to get there (abort after 2 secs to prevent getting stuck)
    point_head_client_->waitForResult(ros::Duration(2));

To move the arm and gripper around automatically instead of manually, see Moving the arm using the Joint Trajectory Action.