## page was copied from pr2_mechanism/Tutorials/Plotting controller data in matlab
## page was renamed from pr2_mechanism/Tutorials/Plotting the state of a controller
## page was renamed from controllers/Tutorials/Plotting the state of a controller
## page was renamed from pr2_mechanism/Tutorials/Adding a PID controller
####################################
##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/Capturing data from a controller|Capturing data from a controller]]
## note.1
## descriptive title for the tutorial
## title = Plotting controller data in matlab or octave
## multi-line description to be displayed in search 
## description = This tutorial teaches you how to visualize the captured state of a controller in matlab or octave.
## the next tutorial description.
## the next tutorial description (optional)
## next = 
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= IntermediateCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== Introduction ==
We have previously seen an example of how to [[pr2_mechanism/Tutorials/Capturing data from a controller|capture data from a controller]].  This data, however, is usually only useful if it can be visualized, analyzed, or otherwise processed.  In this tutorial, we load the data into matlab for easy plotting and examination.  Wherever and whenever we refer to matlab, octave can be used in place.


== Saving the data to file ==
As a first step, we save the data published from the controller into a file.  This is best achieved via [[rostopic]] using the 'echo -p' command to format the data in matlab friendly form.

With the controller running (see the tutorial on [[pr2_mechanism/Tutorials/Capturing data from a controller|capturing data from a controller]]), listen to the topic with redirection into a file:
{{{
  $ rostopic echo -p /my_controller_name/mystate_topic > datafile.rtp
}}}
Notice we use a 'rtp' extension to denote the rostopic, but any other extension would be equally valid.  To capture the data, trigger the service from a separate prompt:
{{{
  $ rosservice call /my_controller_name/capture
}}}

If you are only interested in a portion of the data, for example one second, you may wish to pass through the unix 'head' command to limit the number of lines:
{{{
  $ rostopic echo -p /my_controller_name/mystate_topic | head -n 1001 > datafile.rtp
}}}
This will not only limit the size of the data, but also terminate the 'rostopic' command when completed.

If you examine the data file '''datafile.rtp''', you will notice a single header line.  This initial line describes the data format and is very useful when loading into matlab.  In our example, the header line will look like
{{{
%time,field.dt,field.position,...
}}}
This is why the 'head' command stored 1001 lines, to include the header and 1000 samples.


== Matlab function: rtpload ==
Note, this code only works for Matlab <2009b.

Loading the data into matlab can be very efficient/powerful, if we allow matlab to use the header line to automatically name the data.  To do this, create the file '''rtpload.m''' somewhere in your matlab path, for example in current directory.  Copy and paste the following function definition:
{{{
#!python
function  [time, data] = rtpload(filename)
%
%   [time, data] = rtpload(filename)
%
%   Load data from a ROS message file, created with
%   a 'rostopic echo -p topic > filename' command.
%
%   filename    name (including path) of data file
%   time        Nx1 vector of ROS times, when the
%                individual messages were received
%   data        structure of Nx1 vectors, corresponding
%                to the fields in the message data

% Note: rostopic saves a header line with information
% about the data.  The first column is the time the
% message was received, the rest are message fields.

% Get the header line - which includes the data format.
fid = fopen(filename);
if (fid < 0)
  error('Unable to open file %s', filename);
end
line = fgetl(fid);
fclose(fid);

% Make sure the file contains something.
if (line <0)
  error('Empty file %s', filename);
end

% Load the actual data.
raw = load(filename);

% Restructure the data.
column = 0;
while (~isempty(line))
  [token,line] = strtok(line,'%,');
  column = column+1;
  eval([token ' = raw(:,' num2str(column) ');']);
end

% Move to the correct output variables.  The first column
% is 'time', the rest are 'field.item1' 'field.item2' etc.
time = time;
data = field;

return;
}}}

This function
 a. reads the initial header line,
 a. loads the full data,
 a. restructures the data according to the header line.

For example, when looking at data from the controller using our previous message definition, we end up with two variables:
 1. 'time', which is simply a vector of the ROS times at which the individual messages were received.  As our controller publishes all data at once, this time is not very useful.
 1. 'data', which is a structure of the fields inside a message.  So we will see
{{{
data.dt
data.position
data.desired_position
data.velocity
data.desired_velocity
data.commanded_effort
data.measured_effort
}}}


== Loading and plotting the data ==
Using this 'rtpload' function, we can load the previously stored data.  Inside matlab execute:
{{{
  >> [time, data] = rtpload('datafile.rtp');
}}}

And then we can plot the signals.  For example
{{{
  >> plot([data.position data.desired_position]);
}}}

As the 'time' variable is not very useful when looking at data from a controller, we can reconstruct time using:
{{{
  >> t = cumsum(dt);
  >> plot(t, data.position);
}}}
except to date, the 'dt' variable was simply set to zero and hence the 't' vector will also be zero.  However, as we start tuning the system, we will use a valid 'dt'.


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