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 controller for the gripper accelerometer.

Introduction

NOTE: THIS TUTORIAL HAS NOT YET BEEN TESTED AND MAY HAVE MINOR BUGS

For a more detailed explanation of the steps seen here for writing a real-time controller please refer to the excellent tutorial: Writing a Real-Time Joint Controller.

To understand this tutorial you should be familiar with the documentation for

1. pr2_hardware_interface, which provides the low-level c++ interface we will be calling functions from to communicate with the accelerometer.

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

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_hardware_interface and the pluginlib. The controller interface package contains the base class for all controllers, the pr2_hardware_interface provides access to the C++ API for the accelerometer, 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_hardware_interface 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_hardware_interface/hardware_interface.h>
   3 
   4 namespace my_controller_ns{
   5 
   6 class MyControllerClass: public pr2_controller_interface::Controller
   7 {
   8 private:
   9   pr2_hardware_interface::Accelerometer* accelerometer_handle_;
  10   double aX, aY, aZ;
  11 
  12 
  13 public:
  14   virtual bool init(pr2_mechanism_model::RobotState *robot,
  15                    ros::NodeHandle &n);
  16   virtual void starting();
  17   virtual void update();
  18   virtual void stopping();
  19 };
  20 } 

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   /* get a handle to the hardware interface */
  12   pr2_hardware_interface::HardwareInterface* hardwareInterface = robot->model_->hw_;
  13   if(!hardwareInterface)
  14       ROS_ERROR("Something wrong with the hardware interface pointer!");
  15 
  16   /* get a handle to the left gripper accelerometer */
  17   accelerometer_handle_ = hardwareInterface->getAccelerometer("l_gripper_motor");
  18   if(!accelerometer_handle_)
  19       ROS_ERROR("Something wrong with getting accelerometer handle");
  20 
  21   // set to 1.5 kHz bandwidth (should be the default)                       
  22   accelerometer_handle_->command_.bandwidth_ = 6;
  23 
  24   // set to +/- 8g range (0=2g,1=4g)
  25   accelerometer_handle_->command_.range_ = 2;
  26 
  27   return true;
  28 }
  29 
  30 
  31 /// Controller startup in realtime
  32 void MyControllerClass::starting()
  33 {
  34 }
  35 
  36 
  37 /// Controller update loop in realtime
  38 void MyControllerClass::update()
  39 {
  40   // retrieve our accelerometer data
  41   // it is most likely that this loop below will run 3 times,
  42   // this is because the ROS controller_manager calls this
  43   // function at 1khz and data is buffered from the accelerometer
  44   // at 3khz. 
  45   std::vector<geometry_msgs::Vector3> threeAccs = accelerometer_handle_->state_.samples_;
  46   for( uint  i = 0; i < threeAccs.size(); i++ )
  47   {
  48     // here is where you would do anything that you want with
  49     // the measured acceleration data. This is a good place to
  50     // do any filtering or other such manipulation. Below we simply
  51     // overwrite global variables that act as storage containers.
  52     aX = threeAccs[i].x;
  53     aY = threeAccs[i].y;
  54     aZ = threeAccs[i].z;
  55   }
  56 
  57 }
  58 
  59 
  60 /// Controller stopping in realtime
  61 void MyControllerClass::stopping()
  62 {}
  63 } // namespace
  64 

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.

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_REGISTER_CLASS(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="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 are ready to run your controller. If you need tips on doing that refer to the real-time joint controller tutorial here: run your controller.