Note: This tutorial assumes that you have completed the previous tutorials: Communicating with a realtime joint controller.
(!) 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.

Capturing data from a controller

Description: This tutorial teaches you how to store data in a controller and extract it for offline viewing, debugging, and tuning.

Tutorial Level: INTERMEDIATE

Next Tutorial: Plotting controller data in matlab

Introduction

Debugging and even tuning a controller can be very difficult if you can't observe signals. Running a debugger is impossible as it violates the realtime requirements. And missing signals for even one cycle can mean you are unable to reconstruct what happened.

So this tutorial adds the ability to store and extract signals, which can be viewed, analyzed, and manipulated offline to examine the system. In particular, we capture signals over a fixed time window (generally several seconds). We store the signals in a storage buffer filled from within the realtime update() function. When the buffer is full, we publish to a ROS topic from outside realtime. This indirection (store, then publish) is realtime safe and guarantees that every cycle will be captured.

Please note this tutorial follows the tutorial on Communicating with a realtime joint controller, as we will leverage this code for tuning in later tutorials. Also, if you are unfamiliar with a ROS topic, check out the ROS tutorials first.

Adding the ROS message

We define a ROS message both to give structure to our storage buffer and to allow easy extraction via a ROS topic.

In the package, create the directory msg, the file msg/MyStateMessage.msg, and copy-paste the following message definition:

float64  dt
float64  position
float64  desired_position
float64  velocity
float64  desired_velocity
float64  commanded_effort
float64  measured_effort

We will store these variables and be able to view them offline.

To auto-generate the necessary source and header files for this message, add the following line to your CmakeLists.txt:

rosbuild_genmsg()

Including the standard services

To trigger the entire behavior, capturing data and export via the ROS topic, we use a standard ROS service. In particular, we use the 'Empty' service, as we need no arguments.

This requires us to depend on the standard services package, which we declare in the manifest.xml file. You need to explicitly add the following depend statement to manifest.xml in the <package> <\package> scope:

  <depend package="std_srvs"/>

Changing the header file

We need to update the header file include/my_controller_pkg/my_controller_file.h to define the storage buffer and publisher. This result is (changes are given afterwards):

   1 #include <pr2_controller_interface/controller.h>
   2 #include <pr2_mechanism_model/joint.h>
   3 #include <ros/ros.h>
   4 #include <my_controller_pkg/SetAmplitude.h>
   5 #include <std_srvs/Empty.h>
   6 #include "my_controller_pkg/MyStateMessage.h"
   7 
   8 namespace my_controller_ns{
   9 
  10 enum
  11 {
  12   StoreLen = 5000
  13 };
  14 
  15 class MyControllerClass: public pr2_controller_interface::Controller
  16 {
  17 private:
  18   bool setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
  19                     my_controller_pkg::SetAmplitude::Response& resp);
  20 
  21   pr2_mechanism_model::JointState* joint_state_;
  22   double init_pos_;
  23   double amplitude_;
  24   ros::ServiceServer srv_;
  25 
  26   bool capture(std_srvs::Empty::Request& req,
  27                std_srvs::Empty::Response& resp);
  28   ros::ServiceServer capture_srv_;
  29   ros::Publisher     mystate_pub_;
  30   my_controller_pkg::MyStateMessage  storage_[StoreLen];
  31   volatile int storage_index_;
  32 
  33 public:
  34   bool init(pr2_mechanism_model::RobotState *robot,
  35             ros::NodeHandle &n);
  36   void starting();
  37   void update();
  38   void stopping();
  39 };
  40 }

The changes in the header file are:

   5 #include <std_srvs/Empty.h>
   6 #include "my_controller_pkg/MyStateMessage.h"
   7 

Here we include the empty service header file (which we will use to trigger the entire capture behavior), as well as the auto-generated message header file that defines the new message we just created.

  10 enum
  11 {
  12   StoreLen = 5000
  13 };

Here we declare the length of the storage buffer. 5000 samples corresponds to a 5 second time window.

  26   bool capture(std_srvs::Empty::Request& req,
  27                std_srvs::Empty::Response& resp);
  28   ros::ServiceServer capture_srv_;
  29   ros::Publisher     mystate_pub_;
  30   my_controller_pkg::MyStateMessage  storage_[StoreLen];
  31   volatile int storage_index_;

Here we

  1. declare the capture function that will execute the entire behavior,
  2. declare the service provider,
  3. declare the message publisher that will publish the messages once ready,
  4. define a storage buffer, using the message to give it structure,
  5. an index into the storage buffer. Note the index is declared as a volatile int, as it will be updated inside the realtime update() loop but also monitored from outside realtime.

Changing the code

The updated source code file src/my_controller_file.cpp in full is (changes are again given afterwards):

   1 #include "my_controller_pkg/my_controller_file.h"
   2 #include <pluginlib/class_list_macros.h>
   3 
   4 using 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 
  27   srv_ = n.advertiseService("set_amplitude", 
  28                             &MyControllerClass::setAmplitude, this);
  29 
  30   amplitude_ = 0.5;
  31 
  32   capture_srv_ = n.advertiseService("capture", &MyControllerClass::capture, this);
  33   mystate_pub_ = n.advertise<my_controller_pkg::MyStateMessage>("mystate_topic", StoreLen);
  34   storage_index_ = StoreLen;
  35 
  36   return true;
  37 }
  38 
  39 
  40 /// Controller startup in realtime
  41 void MyControllerClass::starting()
  42 {
  43   init_pos_ = joint_state_->position_;
  44 }
  45 
  46 
  47 /// Controller update loop in realtime
  48 void MyControllerClass::update()
  49 {
  50   double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
  51   double current_pos = joint_state_->position_;
  52   joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos);
  53 
  54   int index = storage_index_;
  55   if ((index >= 0) && (index < StoreLen))
  56     {
  57       storage_[index].dt               = 0.0;
  58       storage_[index].position         = joint_state_->position_;
  59       storage_[index].desired_position = desired_pos;
  60       storage_[index].velocity         = joint_state_->velocity_;
  61       storage_[index].desired_velocity = 0.0;
  62       storage_[index].commanded_effort = joint_state_->commanded_effort_;
  63       storage_[index].measured_effort  = joint_state_->measured_effort_;
  64 
  65       // Increment for the next cycle.
  66       storage_index_ = index+1;
  67      }
  68 }
  69 
  70 
  71 /// Controller stopping in realtime
  72 void MyControllerClass::stopping()
  73 {}
  74 
  75 
  76 /// Service call to set amplitude of sin
  77 bool MyControllerClass::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
  78                                      my_controller_pkg::SetAmplitude::Response& resp)
  79 {
  80   if (fabs(req.amplitude) < 2.0){
  81     amplitude_ = req.amplitude;
  82     ROS_INFO("Mycontroller: set amplitude too %f", req.amplitude);
  83   }
  84   else
  85     ROS_WARN("MyController: requested amplitude %f too large", req.amplitude);
  86 
  87   resp.amplitude = amplitude_;
  88   return true;
  89 }
  90 
  91 
  92 /// Service call to capture and extract the data
  93 bool MyControllerClass::capture(std_srvs::Empty::Request& req,
  94                                 std_srvs::Empty::Response& resp)
  95 {
  96   /* Record the starting time. */
  97   ros::Time started = ros::Time::now();
  98 
  99   /* Mark the buffer as clear (which will start storing). */
 100   storage_index_ = 0;
 101 
 102   /* Now wait until the buffer is full. */
 103   while (storage_index_ < StoreLen)
 104     {
 105       /* Sleep for 1ms as not to hog the CPU. */
 106       ros::Duration(0.001).sleep();
 107 
 108       /* Make sure we don't hang here forever. */
 109       if (ros::Time::now() - started > ros::Duration(20.0))
 110         {
 111           ROS_ERROR("Waiting for buffer to fill up took longer than 20 seconds!");
 112           return false;
 113         }
 114     }
 115 
 116   /* Then we can publish the buffer contents. */
 117   int  index;
 118   for (index = 0 ; index < StoreLen ; index++)
 119     mystate_pub_.publish(storage_[index]);
 120 
 121   return true;
 122 }
 123 
 124 
 125 /// Register controller to pluginlib
 126 PLUGINLIB_REGISTER_CLASS(MyControllerPlugin,
 127                          my_controller_ns::MyControllerClass,
 128                          pr2_controller_interface::Controller)

The changes in the cpp file are:

   4 using namespace my_controller_ns;

A small change in namespace usage, compared to the previous "namespace my_controller_ns {...}"

  32   capture_srv_ = n.advertiseService("capture", &MyControllerClass::capture, this);
  33   mystate_pub_ = n.advertise<my_controller_pkg::MyStateMessage>("mystate_topic", StoreLen);
  34   storage_index_ = StoreLen;

Here we

  1. create the service call, give it a name, and connect it with the capture method,
  2. create the publisher, give it a name, and specify the queue length to equal the buffer length (this assures that no outgoing message will be lost),
  3. set the index to the full length, indicating not to do any storage.

  54   int index = storage_index_;
  55   if ((index >= 0) && (index < StoreLen))
  56     {
  57       storage_[index].dt               = 0.0;
  58       storage_[index].position         = joint_state_->position_;
  59       storage_[index].desired_position = desired_pos;
  60       storage_[index].velocity         = joint_state_->velocity_;
  61       storage_[index].desired_velocity = 0.0;
  62       storage_[index].commanded_effort = joint_state_->commanded_effort_;
  63       storage_[index].measured_effort  = joint_state_->measured_effort_;
  64 
  65       // Increment for the next cycle.
  66       storage_index_ = index+1;
  67      }

At the end of the update loop, we fill in the current buffer entries. We check to verify the index is in range and then populate the data. Note for now, the time step and desired velocity are meaningless and set to zero. We could have removed them from the message definition, but are expecting to use them soon. Finally, we increment the index.

  92 /// Service call to capture and extract the data
  93 bool MyControllerClass::capture(std_srvs::Empty::Request& req,
  94                                 std_srvs::Empty::Response& resp)
  95 {
  96   /* Record the starting time. */
  97   ros::Time started = ros::Time::now();
  98 
  99   /* Mark the buffer as clear (which will start storing). */
 100   storage_index_ = 0;
 101 
 102   /* Now wait until the buffer is full. */
 103   while (storage_index_ < StoreLen)
 104     {
 105       /* Sleep for 1ms as not to hog the CPU. */
 106       ros::Duration(0.001).sleep();
 107 
 108       /* Make sure we don't hang here forever. */
 109       if (ros::Time::now() - started > ros::Duration(20.0))
 110         {
 111           ROS_ERROR("Waiting for buffer to fill up took longer than 20 seconds!");
 112           return false;
 113         }
 114     }
 115 
 116   /* Then we can publish the buffer contents. */
 117   int  index;
 118   for (index = 0 ; index < StoreLen ; index++)
 119     mystate_pub_.publish(storage_[index]);
 120 
 121   return true;
 122 }

Here we implement the service call itself. Normally, the storage index sits at the full length and the update loop does not change it's value. This service resets the index to zero, which will initiate storage in the update loop. It then waits until the storage loop has completed the data collection. It sleeps to avoid hogging the CPU and times out at 20 seconds in case the controller is not running. Finally, it publishes the full buffer contents, one message at a time.

Testing the data capture

Now we're ready to test the data capture. First re-build the controller:

  $ make

In order for this new code to get loaded into the controller manager, you either need to restart gazebo or the robot, OR re-load the controller libraries with

  $  rosrun pr2_controller_manager pr2_controller_manager reload-libraries

And now, launch the udpated controller:

  $ roslaunch my_controller.launch

To see whether the storage is successful, we need to listen to the new topic:

  $ rostopic echo /my_controller_name/mystate_topic

This should initially report nothing. In a separate window trigger the data capture:

  $ rosservice call /my_controller_name/capture

and the rostopic command should (after 5 seconds) display lots of data.

To analyze the data, you may want to save and plot the signals, as explained in the next tutorial, Plotting controller data in matlab.

Wiki: pr2_mechanism/Tutorials/Capturing data from a controller (last edited 2011-10-05 11:25:01 by TimAssman)