## page was renamed from pr2_mechanism/Tutorials/Storing and extracting data from a 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=[[pr2_mechanism/Tutorials/Communicating with a realtime joint controller|Communicating with a realtime joint controller]]
## descriptive title for the tutorial
## title = Capturing data from a controller
## multi-line description to be displayed in search 
## description = This tutorial teaches you how to store data in a controller and extract it for offline viewing, debugging, and tuning.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=[[pr2_mechanism/Tutorials/Plotting controller data in matlab|Plotting controller data in matlab]]
## next.1.link=
## what level user is this tutorial for 
## level= IntermediateCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== 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 [[Topics|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 [[pr2_mechanism/Tutorials/Communicating with a realtime joint controller|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 [[Topics|topic]], check out the [[ROS/Tutorials|ROS tutorials]] first.


== Adding the ROS message ==

We define a ROS [[Messages|message]] both to give structure to our storage buffer and to allow easy extraction via a ROS [[Topics|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 [[Topics|topic]], we use a standard ROS [[Services|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:
{{{#!XML
  <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):
{{{
#!cplusplus block=controller_h
#include <pr2_controller_interface/controller.h>
#include <pr2_mechanism_model/joint.h>
#include <ros/ros.h>
#include <my_controller_pkg/SetAmplitude.h>
#include <std_srvs/Empty.h>
#include "my_controller_pkg/MyStateMessage.h"

namespace my_controller_ns{

enum
{
  StoreLen = 5000
};

class MyControllerClass: public pr2_controller_interface::Controller
{
private:
  bool setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
                    my_controller_pkg::SetAmplitude::Response& resp);

  pr2_mechanism_model::JointState* joint_state_;
  double init_pos_;
  double amplitude_;
  ros::ServiceServer srv_;

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

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

The changes in the header file are:
<<CodeRef(controller_h, 5, 6)>>
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.

<<CodeRef(controller_h, 10, 13)>>
Here we declare the length of the storage buffer.  5000 samples corresponds to a 5 second time window.

<<CodeRef(controller_h, 26, 31)>>
Here we
 a. declare the capture function that will execute the entire behavior,
 a. declare the service provider,
 a. declare the message publisher that will publish the messages once ready,
 a. define a storage buffer, using the message to give it structure,
 a. 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):
{{{
#!cplusplus block=controller_cpp
#include "my_controller_pkg/my_controller_file.h"
#include <pluginlib/class_list_macros.h>

using 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;
  }

  srv_ = n.advertiseService("set_amplitude", 
                            &MyControllerClass::setAmplitude, this);

  amplitude_ = 0.5;

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

  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_ + amplitude_ * sin(ros::Time::now().toSec());
  double current_pos = joint_state_->position_;
  joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos);

  int index = storage_index_;
  if ((index >= 0) && (index < StoreLen))
    {
      storage_[index].dt               = 0.0;
      storage_[index].position         = joint_state_->position_;
      storage_[index].desired_position = desired_pos;
      storage_[index].velocity         = joint_state_->velocity_;
      storage_[index].desired_velocity = 0.0;
      storage_[index].commanded_effort = joint_state_->commanded_effort_;
      storage_[index].measured_effort  = joint_state_->measured_effort_;

      // Increment for the next cycle.
      storage_index_ = index+1;
     }
}


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


/// Service call to set amplitude of sin
bool MyControllerClass::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
                                     my_controller_pkg::SetAmplitude::Response& resp)
{
  if (fabs(req.amplitude) < 2.0){
    amplitude_ = req.amplitude;
    ROS_INFO("Mycontroller: set amplitude too %f", req.amplitude);
  }
  else
    ROS_WARN("MyController: requested amplitude %f too large", req.amplitude);

  resp.amplitude = amplitude_;
  return true;
}


/// Service call to capture and extract the data
bool MyControllerClass::capture(std_srvs::Empty::Request& req,
				std_srvs::Empty::Response& resp)
{
  /* Record the starting time. */
  ros::Time started = ros::Time::now();

  /* Mark the buffer as clear (which will start storing). */
  storage_index_ = 0;

  /* Now wait until the buffer is full. */
  while (storage_index_ < StoreLen)
    {
      /* Sleep for 1ms as not to hog the CPU. */
      ros::Duration(0.001).sleep();

      /* Make sure we don't hang here forever. */
      if (ros::Time::now() - started > ros::Duration(20.0))
	{
	  ROS_ERROR("Waiting for buffer to fill up took longer than 20 seconds!");
	  return false;
	}
    }

  /* Then we can publish the buffer contents. */
  int  index;
  for (index = 0 ; index < StoreLen ; index++)
    mystate_pub_.publish(storage_[index]);

  return true;
}


/// Register controller to pluginlib
PLUGINLIB_REGISTER_CLASS(MyControllerPlugin,
                         my_controller_ns::MyControllerClass,
                         pr2_controller_interface::Controller)
}}}

The changes in the cpp file are:
<<CodeRef(controller_cpp, 4, 4)>>

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

<<CodeRef(controller_cpp, 32, 34)>>
Here we
 a. create the service call, give it a name, and connect it with the capture method,
 a. 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),
 a. set the index to the full length, indicating not to do any storage.

<<CodeRef(controller_cpp, 54, 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.

<<CodeRef(controller_cpp, 92, 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 [[pr2_controller_manager|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, [[pr2_mechanism/Tutorials/Plotting controller data in matlab|Plotting controller data in matlab]].


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