[Documentation] [TitleIndex] [WordIndex

Note: This tutorial requires basic understanding of ROS concepts, such as nodes, packages and launch files, the Transform Library tf, and basic understanding of the ROS action library.
(!) 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

Description: This tutorial shows you how to set a desired pose of the robot head using the existing head trajectory controller.

Keywords: move head, joints, controller, gaze, stare, neck

Tutorial Level: BEGINNER

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:

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:

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:

rosrun pr2_controller_manager pr2_controller_manager start head_traj_controller

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:

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.


2019-08-24 13:07