## page was renamed from pr2_controllers/Tutorials/Moving the Head
## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## 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 [[actionlib|ROS action library]]
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= 
## descriptive title for the tutorial
## title = Moving the head
## multi-line description to be displayed in search 
## description = This tutorial shows you how to set a desired pose of the robot head using the existing head trajectory controller.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= BeginnerCategory
## keywords= move head, joints, controller, gaze, stare, neck
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

=== 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 [[WgWiki:/PR2/StartRobot|on the hardware]] or [[simulator_gazebo/Tutorials/StartingGazebo|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 [[CodeAPI:pr2_controllers_msgs/html/msg/PointHeadGoal.html|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''':

{{{
#!cplusplus
#include <ros/ros.h>

#include <actionlib/client/simple_action_client.h>
#include <pr2_controllers_msgs/PointHeadAction.h>

// Our Action interface type, provided as a typedef for convenience
typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;

class RobotHead
{
private:
  PointHeadClient* point_head_client_;

public:
  //! Action client initialization 
  RobotHead()
  {
    //Initialize the client for the Action interface to the head controller
    point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);

    //wait for head controller action server to come up 
    while(!point_head_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the point_head_action server to come up");
    }
  }

  ~RobotHead()
  {
    delete point_head_client_;
  }

  //! Points the high-def camera frame at a point in a given frame  
  void lookAt(std::string frame_id, double x, double y, double z)
  {
    //the goal message we will be sending
    pr2_controllers_msgs::PointHeadGoal goal;

    //the target point, expressed in the requested frame
    geometry_msgs::PointStamped point;
    point.header.frame_id = frame_id;
    point.point.x = x; point.point.y = y; point.point.z = z;
    goal.target = point;

    //we are pointing the high-def camera frame 
    //(pointing_axis defaults to X-axis)
    goal.pointing_frame = "high_def_frame";
    goal.pointing_axis.x = 1;
    goal.pointing_axis.y = 0;
    goal.pointing_axis.z = 0;

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

    //and go no faster than 1 rad/s
    goal.max_velocity = 1.0;

    //send the goal
    point_head_client_->sendGoal(goal);

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

  //! Shake the head from left to right n times  
  void shakeHead(int n)
  {
    int count = 0;
    while (ros::ok() && ++count <= n )
    {
      //Looks at a point forward (x=5m), slightly left (y=1m), and 1.2m up
      lookAt("base_link", 5.0, 1.0, 1.2);

      //Looks at a point forward (x=5m), slightly right (y=-1m), and 1.2m up
      lookAt("base_link", 5.0, -1.0, 1.2);
    }
  }
};

int main(int argc, char** argv)
{
  //init the ROS node
  ros::init(argc, argv, "robot_driver");

  RobotHead head;
  head.shakeHead(3);
}
}}}

=== 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:

{{{
#!cplusplus
while (ros::ok())
{
  head.lookAt("r_gripper_tool_frame", 0, 0, 0);
  usleep(50000);
}
}}}

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 [[pr2_controllers/Tutorials/Moving the arm using the Joint Trajectory Action|Moving the arm using the Joint Trajectory Action]].


## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## PR2HeadActionTutorial