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.

Writing a realtime joint controller

Introduction

To understand this and the follow-on tutorials, you should be familiar with the documentation for

1. pr2_controller_interface, which provides the code interface, i.e. how your code will be called.

2. pr2_mechanism_model and in particular JointState, which provides access to the joint position sensors and joint torque commands.

3. pr2_controller_manager, which load/start/stops and generally administers the execution of realtime code.

Writing the code

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

   1 #include <pr2_controller_interface/controller.h>
   2 #include <pr2_mechanism_model/joint.h>
   3 
   4 namespace my_controller_ns{
   5 
   6 class MyControllerClass: public pr2_controller_interface::Controller
   7 {
   8 private:
   9   pr2_mechanism_model::JointState* joint_state_;
  10   double init_pos_;
  11 
  12 public:
  13   virtual bool init(pr2_mechanism_model::RobotState *robot,
  14                    ros::NodeHandle &n);
  15   virtual void starting();
  16   virtual void update();
  17   virtual void stopping();
  18 };
  19 } 

And also create a directory src and a file called src/my_controller_file.cpp. Copy paste the following code into this file:

   1 #include "my_controller_pkg/my_controller_file.h"
   2 #include <pluginlib/class_list_macros.h>
   3 
   4 namespace my_controller_ns {
   5 
   6 
   7 /// Controller initialization in non-realtime
   8 bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
   9                             ros::NodeHandle &n)
  10 {
  11   std::string joint_name;
  12   if (!n.getParam("joint_name", joint_name))
  13   {
  14     ROS_ERROR("No joint given in namespace: '%s')",
  15               n.getNamespace().c_str());
  16     return false;
  17   }
  18 
  19   joint_state_ = robot->getJointState(joint_name);
  20   if (!joint_state_)
  21   {
  22     ROS_ERROR("MyController could not find joint named '%s'",
  23               joint_name.c_str());
  24     return false;
  25   }
  26   return true;
  27 }
  28 
  29 
  30 /// Controller startup in realtime
  31 void MyControllerClass::starting()
  32 {
  33   init_pos_ = joint_state_->position_;
  34 }
  35 
  36 
  37 /// Controller update loop in realtime
  38 void MyControllerClass::update()
  39 {
  40   double desired_pos = init_pos_ + 15 * sin(ros::Time::now().toSec());
  41   double current_pos = joint_state_->position_;
  42   joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos);
  43 }
  44 
  45 
  46 /// Controller stopping in realtime
  47 void MyControllerClass::stopping()
  48 {}
  49 } // namespace
  50 

The code explained

• To make a realtime controller, we created a class which inherits from the 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 package documentation.

• Access to joints is provided through the 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 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

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:

  <export>
    <pr2_controller_interface plugin="${prefix}/controller_plugins.xml" />
  </export>

All together the manifest.xml will look something like:

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

<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.

Now you're ready for the next tutorial, that will teach you how to run your controller.