## page was renamed from controllers/Tutorials/Writing a realtime joint controller
## page was renamed from pr2_mechanism/Tutorials/Writing a realtime controller
## page was renamed from pr2_controller_interface/Tutorials/Writing a realtime controller
## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links: 
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0=[[ROS/Tutorials|ROS tutorials]]
## descriptive title for the tutorial
## title = Writing a realtime joint controller
## multi-line description to be displayed in search
## description = This tutorial teaches you how to write a joint space controller that can be executed in the realtime loop of [[pr2_controller_manager]]
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=[[pr2_mechanism/Tutorials/Running a realtime joint controller|Running a realtime joint controller]]
## next.1.link=
## what level user is this tutorial for
## level= BeginnerCategory
## keywords =
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== Introduction ==

To understand this and the follow-on tutorials, you should be familiar with the documentation for
 a. [[pr2_controller_interface]], which provides the code interface, i.e. how your code will be called.
 a. [[pr2_mechanism_model]] and in particular [[http://www.ros.org/doc/api/pr2_mechanism_model/html/classpr2__mechanism__model_1_1JointState.html|JointState]], which provides access to the joint position sensors and joint torque commands.
 a. [[pr2_controller_manager]], which load/start/stops and generally administers the execution of realtime code.

== Writing the code ==
{{attachment:controllers_1.png}}

First, let's create a package where you'll build your controller. The package needs to depend on the [[pr2_controller_interface]], the [[pr2_mechanism_model]] and the [[pluginlib]]. The [[pr2_controller_interface|controller interface]] package contains the base class for all controllers, the [[pr2_mechanism_model]] provides access to the robot joints, and the [[pluginlib]] package allows us to add our own controller as a plugin into the [[pr2_controller_manager|controller manager]]

{{{
$ roscd ros_pkg_tutorials
$ roscreate-pkg my_controller_pkg pr2_controller_interface pr2_mechanism_model pluginlib
$ roscd my_controller_pkg
}}}
And let's already build all the dependencies of our new package:
{{{
$ rosmake
}}}



=== The code ===
Now, inside our new package, create the directory '''include''', then '''include/my_controller_pkg''', fire up your editor, create a file called '''include/my_controller_pkg/my_controller_file.h''' and copy paste the following code into this file:
{{{
#!cplusplus block=controller_h
#include <pr2_controller_interface/controller.h>
#include <pr2_mechanism_model/joint.h>

namespace my_controller_ns{

class MyControllerClass: public pr2_controller_interface::Controller
{
private:
  pr2_mechanism_model::JointState* joint_state_;
  double init_pos_;

public:
  virtual bool init(pr2_mechanism_model::RobotState *robot,
                   ros::NodeHandle &n);
  virtual void starting();
  virtual void update();
  virtual void stopping();
};
} 
}}}

And also create a directory '''src''' and a file called '''src/my_controller_file.cpp'''. Copy paste the following code into this file:
{{{
#!cplusplus block=controller
#include "my_controller_pkg/my_controller_file.h"
#include <pluginlib/class_list_macros.h>

namespace my_controller_ns {


/// Controller initialization in non-realtime
bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
                            ros::NodeHandle &n)
{
  std::string joint_name;
  if (!n.getParam("joint_name", joint_name))
  {
    ROS_ERROR("No joint given in namespace: '%s')",
              n.getNamespace().c_str());
    return false;
  }

  joint_state_ = robot->getJointState(joint_name);
  if (!joint_state_)
  {
    ROS_ERROR("MyController could not find joint named '%s'",
              joint_name.c_str());
    return false;
  }
  return true;
}


/// Controller startup in realtime
void MyControllerClass::starting()
{
  init_pos_ = joint_state_->position_;
}


/// Controller update loop in realtime
void MyControllerClass::update()
{
  double desired_pos = init_pos_ + 15 * sin(ros::Time::now().toSec());
  double current_pos = joint_state_->position_;
  joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos);
}


/// Controller stopping in realtime
void MyControllerClass::stopping()
{}
} // namespace
}}}


=== The code explained ===

 * To make a realtime controller, we created a class which inherits from the [[http://www.ros.org/doc/api/pr2_controller_interface/html/classcontroller_1_1Controller.html|controller::Controller]] class in the [[pr2_controller_interface]] package.  Overload certain methods of this base class and Mechanism Control will call these methods when your controller changes state.  You need to overload the `init`, `starting`, `update`, and `stopping` methods.
  * The `init` method will get called in non-realtime when the controller is loaded.   
  * When the controller gets started, `starting` will get called once in realtime, right before the first call of `update`. 
  * While the controller is running `update` gets called periodically (1000 Hz) from the realtime loop.
  * When the controller is stopped, `stopping` gets called once in realtime, right after the last `update` call.

 For a detailed description of the methods in the base class, take a look at the [[pr2_controller_interface|pr2_controller_interface package documentation]].


 * Access to joints is provided through the [[http://www.ros.org/doc/api/pr2_mechanism_model/html/classpr2__mechanism_1_1RobotState.html|RobotState]] object, passed into the `init` method.  You should lookup (by name) the joints you want in your `init` method using the `getJointState` method.  You may then read from and write to the returned [[http://www.ros.org/doc/api/pr2_mechanism_model/html/classpr2__mechanism_1_1JointState.html|JointState]].  See also the [[pr2_mechanism_model]] package documentation for details on how to access the signals/information.


== Compiling the code ==
Now that we created the code, lets compile it first. Open the '''CMakeLists.txt''' file, and add the following line on the bottom:
 
{{{
rosbuild_add_library(my_controller_lib src/my_controller_file.cpp)
}}}
and, try to build your package:
{{{
$ make
}}}
If everything went well, you should have a library file called '''(lib)my_controller_lib.so''' in your '''lib''' folder. 


== Register your controller as a plugin ==
{{attachment:controllers_2.png}}
The code, now compiled as a library, needs to run inside the realtime process. This linking can occur in two ways:  (a) automatically, as the realtime process starts.  When the process starts, it will search for and load your library. Or (b) manually, while the process is running. You can explicitly ask the process to load your library on the fly. For both cases, however, the code needs to be earmarked as destined for the realtime process, that is it needs to be registered as a plugin. Then the [[pr2_controller_manager]] can use the [[pluginlib]] to administer the linking, loading, and starting of the controller.

To make the controllers visible to the [[pr2_controller_manager]], [[pluginlib]], and the realtime process, the package containing the controller must export it as a loadable class. The first thing to do is to call the plugin registration macro from your '''src/my_controller_file.cpp''' file. Add the following lines to the bottom of this file:
{{{
/// Register controller to pluginlib
PLUGINLIB_DECLARE_CLASS(my_controller_pkg,MyControllerPlugin, 
                         my_controller_ns::MyControllerClass, 
                         pr2_controller_interface::Controller)
}}}
and rebuild your controller:
{{{
$ make
}}}

Next, to export the controller, you must also depend on the [[pr2_controller_interface]] and [[pluginlib]] packages and reference the plugin description file in the exports section.  The dependencies were automatically noted during the package creation in Section 2, but you need to explicitly add the following export statement to '''manifest.xml''' in the `<package> <\package>` scope:
{{{#!XML
  <export>
    <pr2_controller_interface plugin="${prefix}/controller_plugins.xml" />
  </export>
}}}
All together the '''manifest.xml''' will look something like:
{{{#!XML
<package>
  ...
  <depend package="pr2_controller_interface" />
  <depend package="pluginlib" />
  ...
  <export>
    <pr2_controller_interface plugin="${prefix}/controller_plugins.xml" />
  </export>
  ...
</package>
}}}
Finally, you need to create the plugin description file, which the manifest called '''controller_plugins.xml'''. The format is described in the [[pluginlib]] documentation. For our controller we need the following description in '''controller_plugins.xml''':

{{{#!XML
<library path="lib/libmy_controller_lib">
  <class name="my_controller_pkg/MyControllerPlugin" 
         type="my_controller_ns::MyControllerClass"           
         base_class_type="pr2_controller_interface::Controller" />
</library>
}}}


Now, let's ensure that the controller is correctly configured as a plugin and is visible to Mechanism Control.  Check that the plugin description file is visible to rospack:

{{{
$ rospack plugins --attrib=plugin pr2_controller_interface
}}}
Your controller_plugins.xml file should be in this list.  If it's not, then you probably did not add [[pr2_controller_interface]] as a dependency of your package.

{{{#!wiki comment
There's still no good way of checking that plugins are built without using `nm`.
https://code.ros.org/trac/ros-pkg/ticket/2822
}}}


Now you're ready for the next tutorial, that will teach you how to [[pr2_mechanism/Tutorials/Running a realtime joint controller|run your controller]].



## AUTOGENERATED DO NOT DELETE
## TutorialCategory
## WritingYourselfCategory
## JointSpaceCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE