www.gusucode.com > robotsimulink 工具箱 matlab源码程序 > robotsimulink/robotslros/+robotics/+slros/+internal/+sim/ROSMsgToBusStructConverter.m

    classdef ROSMsgToBusStructConverter < handle
    %This class is for internal use only. It may be removed in the future.
        
    %  ROSMsgToBusStructConverter(MSGTYPE, MODEL) creates an object that
    %  converts a MATLAB ROS message, with message type MSGTYPE, to a
    %  corresponding Simulink bus struct. This conversion uses the maximum
    %  sizes of variable-length properties that is specific to MODEL.
    %
    %  Example:
    %   msg2bus = robotics.slros.internal.sim.ROSMsgToBusStructConverter('nav_msgs/Path');
    %   bus2msg = robotics.slros.internal.sim.BusStructToROSMsgConverter('nav_msgs/Path');
    %   busstruct = msg2bus.convert(pathMsg);
    %   msg2 = bus2msg.convert(busstruct); % convert back
    %
    % See also: sim.BusStructToROSMsgConverter
    
    %   Copyright 2014-2016 The MathWorks, Inc.
    
    properties
        ROSMessageType
        Model
        MsgInfoMap
    end
    
    methods
        function obj = ROSMsgToBusStructConverter(msgtype, model)
            assert(ischar(msgtype) && ~isempty(msgtype));            
            
            obj.ROSMessageType = msgtype;
            obj.Model = model;
            obj.MsgInfoMap = robotics.slros.internal.sim.createROSMsgInfoMap(struct(...
                'MessageType', obj.ROSMessageType, ...
                'ModelName', model, ...
                'MapKeyType', 'msgtype', ...
                'Recurse', true));
        end
        
        function busstruct = convert(obj, msg)            
            rosstruct = toStruct(msg);
            busstruct = rosstruct;
            busstruct = processStruct(rosstruct, busstruct, obj.MsgInfoMap, obj.ROSMessageType, obj.Model);            
        end
        
    end
    
end


%%
function busstruct = processStruct(rosstruct, busstruct, map, msgtype, modelname)

info = map(msgtype);

% For list of expected fields in info, see
% createROSMsgInfoMap:processBus()

% If any of the properties are a reserved name, the name in the bus is
% mangled. All the info fields (e.g., StringArrayProps, NestedMsgs)
% use the possibly-mangled name in the bus. The strategy here is to first
% ensure that
%   a) busstruct has the mangled name (e.g.  Vector_), and
%   b) rosstruct has the mangled name as well
% That way, the subsequent info field manipulations will work with
% rosstruct.Vector_ and copy it to busstruct.Vector_

if numel(info.ReservedWordProp) > 0
    rosNames = info.ReservedWordInfo.RosNames; % original names
    busNames = info.ReservedWordInfo.BusNames; % mangled names
    % rename the fields corresponding to the reserved words
    busstruct = cell2struct(struct2cell(rosstruct), info.ReservedWordInfo.AdjustedFieldNames, 1);
    % copy rosstruct.(rosNames) to rosstruct.(busNames) as well
    for j=1:length(busNames)
        rosstruct.(busNames{j}) = rosstruct.(rosNames{j});
    end
end


for i=1:numel(info.StringArrayProps)    
    % Cell arrays of strings
    % Convert the entire cell array to the toStruct equivalent of std_msgs/String[]
    % For variable-length string arrays, the VarLenArray processing loop
    %    (below) will do any required clipping 
    % For fixed-length string arrays, we assume that the incoming length of
    %    the string array matches the length of the Simulink bus array (i.e.,
    %    nothing extra to be done)
    prop = info.StringArrayProps{i};
    assert(iscell(rosstruct.(prop)));
    tmpStruct = struct('Data', rosstruct.(prop));
    rosstruct.(prop) = tmpStruct;
    busstruct.(prop) = tmpStruct;
end

if ~isempty(info.StdEmptyMsgProp)
    dummyprop = info.StdEmptyMsgProp{1};
    busstruct.(dummyprop) = false; % assign dummy value    
end

% Generic processing for all variable-length arrays (primitive numeric
% arrays, arrays of nested messages, string arrays). This mainly consists
% of setting the appropriate SL_Info values.
for i=1:numel(info.VarLenArrays)
   prop = info.VarLenArrays{i};
   infoprop = info.VarLenArrayInfoProps{i};
   maxlength = info.VarLenMaxLen{i};
   recvdlength = length(rosstruct.(prop));
   currentlength = min(maxlength, recvdlength);
   
   if (recvdlength > maxlength) && ...
           info.VarLenTruncateAction{i} == robotics.slros.internal.bus.VarLenArrayTruncationAction.EmitWarning       
       robotics.internal.warningNoBacktrace(...       
           message('robotics:robotslros:busconvert:TruncatedArray', ...
           prop, msgtype, maxlength, recvdlength, maxlength, modelname));       
   end
   
   busstruct.(infoprop) = ...
      robotics.slros.internal.bus.Util.getVarlenInfoStruct(...
       currentlength, recvdlength);
end


% convert int64 and uint64 to doubles (note that these can be arrays)
for i=1:numel(info.Int64Props)
   prop = info.Int64Props{i};
   busstruct.(prop) = double(rosstruct.(prop)).';  % note the transpose to column vec
end

% convert strings to uint8
for i=1:numel(info.StringProps)
   prop = info.StringProps{i};
   busstruct.(prop) = uint8(rosstruct.(prop)).';  % note the transpose to column vec
end

% Specific handling for Variable-length primitive arrays
%
% For actual fixed-length arrays (those that are defined fixed by ROS),
% we can assume they will come in at the correct length. But for
% var-length arrays of primitive types, we need to enforce that the
% length exactly matches the max-length. Note: the SL_Info property is
% already set in the generic processing for all variable-length arrays
for i=1:numel(info.VarLenPrimitiveArrays)
    prop = info.VarLenPrimitiveArrays{i};
    maxlen = info.VarLenPrimitiveMaxLen{i};
    
    len = numel(busstruct.(prop));
    if len < maxlen
        % ros array too short; zero-pad to the max length of the bus array
         busstruct.(prop)(len+1:maxlen) = 0; % 0 will get typecast appropriately
    elseif len  > maxlen
        % ros array too long; crop the bus array
        busstruct.(prop)(maxlen+1:end) = [];
    else
        % nothing to do
    end
end

% Specific handling for Variable-length message arrays
% This is exactly analogous to the handling of primitive arrays.
for i=1:numel(info.VarLenMsgArrays)
    prop = info.VarLenMsgArrays{i};
    maxlen = info.VarLenMsgMaxLen{i};
    
    len = numel(busstruct.(prop));
    if len == 0
        % ros array has zero length. This is a special case --
        % busstruct.(prop) doesn't have the expected struct type, so it
        % can't just be extended to the maximum length. Solution: use the
        % VarLenMsgEmpty to set up the array of desired type & length
         emptystruct = info.VarLenMsgEmpty{i};
         emptystruct(2:maxlen) = emptystruct(1);
         busstruct.(prop) = emptystruct;
         rosstruct.(prop) = info.VarLenMsgEmpty{i};
    elseif len < maxlen
        % ros array too short; zero-pad to the max length of the bus array
        % Can't just set busstruct.(prop)(maxlen), since the intervening
        % structs would get all fields set to [] (empty double array)        
        busstruct.(prop)(len+1:maxlen) = info.VarLenMsgEmpty{i};
    elseif len  > maxlen
        % ros array too long; crop the bus array
        busstruct.(prop)(maxlen+1:end) = [];
    else
        % nothing to do
    end
end


% Specific handling for nested messages (inclding scalar, fixed-length
% arrays, and variable-length arrays). We need to recurse over each of the
% nested message instances to ensure that their data is appropriately
% fixed-up.
% 
% Note -- 
% 1) All the bus arrays in this message have been fixed-up alread
%   (padded/trimmed to the maximum length, corresponding SL_Info values have
%    been set).
%
% 2) busstruct is a pure struct data structure (i.e., all the nested arrays
%    are struct arrays). This means they cannot be fixed-up "in-place"
%    (esp. because the fixing-up will involve adding, deleting, and
%    reordering struct fields). We need to get the fixed-up struct and replace
%    the old struct with it.
%
for i=1:numel(info.NestedMsgs)
   prop = info.NestedMsgs{i};
   nestedMsgType = info.NestedMsgType{i};
   
   % Note: the following logic works handles scalar nested messges as well.
   % Also, we don't account for multi-dimensional arrays, since ROS
   % arrays are 1-d by definition.
   roslen = numel(rosstruct.(prop)); 
   buslen = numel(busstruct.(prop));  
   
   % Can't initialize substructs with busstruct.(prop). The type of the
   % struct may not match type of fixed-up struct (e.g., due to added or
   % removed fields). We need to grow substruct with the correct fixed-up
   % struct type
   substructs = struct.empty;      
   
   validLength = max(min(buslen,roslen),1);
   for j=1:validLength
       tmpout = processStruct( ...
           rosstruct.(prop)(j), ...
           busstruct.(prop)(j), ...
           map, ...
           nestedMsgType, ...
           modelname);
       if isempty(substructs)
           substructs = tmpout;
       else
           substructs(j) = tmpout;
       end              
   end
   
   if validLength < buslen
       % Replicate the last element of the ROS array in all the padding
       % elements (the bus array structs need to be fixed-up even if they
       % are all "null").
       substructs(validLength+1:buslen) = tmpout;
   end
   
   busstruct.(prop) = substructs;
end


% Permute the order of properties in busstruct to the expected order in the
% Simulink bus (e.g., to put the SL_Info properties next to the
% corresponding variablen-length arrays).
if info.propertyReorderInfo.doPermute
    % Note - the ArrayInfo properties have already been added to busstruct    
    values   = struct2cell(busstruct);
    perm = info.propertyReorderInfo.permutation;
    busstruct = cell2struct(values(perm), ...
        info.propertyReorderInfo.augmentedPropertyList(perm), 1);
end

end