MATLAB script for formatting data from a matlab_rosbag ros.Bag object as a struct

bag2struct.m

function [data, meta] = bag2struct(bag, varargin)
%BAG2STRUCT Formats data from topics in a rosbag file into a struct
%   DATA = BAG2STRUCT(BAG) returns a struct containing data for each of the
%   topics in BAG, where BAG is an object of class 'ros.Bag'.
%
%   [DATA,META] = BAG2STRUCT(BAG) additionally returns a struct containing
%   the metadata for each topic in the bag.
%
%   Example
%         bag = ros.Bag.load('rosbag.bag');
%         [data, meta] = bag2struct(bag);
%     Creates a ros.Bag object from the file rosbag.bag, then uses
%     BAG2STRUCT to format the data and metadata as structs.

% assert(exist('ros.Bag', 'class') == 8, 'Please ensure matlab_rosbag is in
% the search path') % shouldn't be necessary? (as long as the object is of
% the right class, we have access to the functions. But do need to check
% that ros.msgs2mat is a function, so we need some check of this kind)

%TODO pass in file name instead?
%TODO add option to make first message received correspond to time=0 (use bag.time_begin)
%TODO add option to specify which topics to extract (use bag.resetView)
%TODO add option to blacklist specific topics and/or message types
%TODO add option to specify time range (use bag.resetView)
%TODO add option to exclude images, pointclouds, etc.
%TODO add option to choose what character replaces the / in topic names
%TODO add option to convert quaternion to euler angles whenever one is encountered (use something like ros.pose2xyt?
%TODO add option to only convert numeric data
%TODO add option to put everything in columns instead of rows
%TODO convert all numeric types to doubles?

nonnumeric = true;

%--------------------------------------------------------------------
% parse inputs
%--------------------------------------------------------------------
p = inputParser;
p.FunctionName = 'bag2struct';

p.addRequired('bag', @(x) isa(x, 'ros.Bag'))
p.addParameter('Flatten', true, @islogical)
p.addParameter('Topics', {}, @(x) iscell(x) || ischar(x))
p.addParameter('SkipTopics', {}, @(x) iscell(x) || ischar(x))
p.addParameter('IncludeTime', true, @islogical)
p.addParameter('ZeroTime', true, @islogical)
p.addParameter('TrimStart', [], @isnumeric)
p.addParameter('TrimEnd', [], @isnumeric)

p.parse(bag, varargin{:})

%--------------------------------------------------------------------
% topic filters
%--------------------------------------------------------------------
if isempty(p.Results.Topics)
    topics = bag.topics;
else
    topics = p.Results.Topics;
    if ischar(topics)
        topics = {topics}; % force topics to be a cell array
    end
end
topics = setdiff(topics, p.Results.SkipTopics);

if isempty(topics)
    error('No topics are to be converted')
end

%--------------------------------------------------------------------
% time options
%--------------------------------------------------------------------
include_time = p.Results.IncludeTime;
zero_time = p.Results.ZeroTime;

start_time = [];
end_time = [];
if ~isempty(p.Results.TrimStart)
    start_time = bag.time_begin + p.Results.TrimStart;
end
if ~isempty(p.Results.TrimEnd)
    end_time = bag.time_end - p.Results.TrimEnd;
end

bag.resetView(topics, start_time, end_time);

%--------------------------------------------------------------------
% parse bag file
%--------------------------------------------------------------------
for topic = topics
    [msgs, metas] = bag.readAll(topic{1}, p.Results.Flatten);
    field_name = strrep(topic{1}(2:end), '/', '_');
    if ~isempty(msgs)
        
        % extract data
        data.(field_name) = recurse_struct(msgs, @(x) x, nonnumeric);
        
        % add time field
        if include_time
            if isfield(data.(field_name), 'time')
                warning('topic ''%s'' already has a field named ''time''')
            else
                time = cellfun(@(x) x.time.time, metas);
                if zero_time
                    time = time - bag.time_begin;
                end
                if isstruct(data.(field_name))
                    data.(field_name).time = time;
                else % need to shift the data down into a struct field
                    temp = data.(field_name);
                    data = rmfield(data, field_name);
                    data.(field_name) = struct( ...
                        'data', temp, ...
                        'time', time);
                end
            end
        end
        
        % extract metadata
        if nargout == 2
            meta.(field_name) = recurse_struct(metas, @(x) x, true);
        end
    end
end

end

%==========================================================================
function s = recurse_struct(msgs, accessor, nonnumeric)
%RECURSE_STRUCT Recursively extract data from nested structs

if isstruct(accessor(msgs{1}))
    for field = fieldnames(accessor(msgs{1}))'
        field_accessor = @(x) struct(accessor(x)).(field{1});
        s.(field{1}) = recurse_struct(msgs, field_accessor, nonnumeric);
    end
elseif isnumeric(accessor(msgs{1}))
    s = ros.msgs2mat(msgs, accessor);
elseif nonnumeric && islogical(accessor(msgs{1}))
    s = cell2mat(cellfun(accessor, msgs, 'UniformOutput', false));
elseif nonnumeric
    s = cellfun(accessor, msgs, 'UniformOutput', false);
end

end