www.gusucode.com > robotsimulink 工具箱 matlab源码程序 > robotsimulink/robotslros/+robotics/+slros/+internal/+sim/BusStructToROSMsgConverter.m
classdef BusStructToROSMsgConverter < handle %This class is for internal use only. It may be removed in the future. % BusStructToROSMsgConverter(MSGTYPE, MODEL) creates an object that % converts a Simulink bus struct, corresponding to a ROS message type % MSGTYPE, to a MATLAB ROS message. 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.ROSMsgToBusStructConverter % Copyright 2014-2016 The MathWorks, Inc. properties ROSMessageType Model MsgInfoMap end methods function obj = BusStructToROSMsgConverter(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)); assertInfoStructNames(); end function rosmsg = convert(obj, busstruct) busstruct = processStruct(busstruct, obj.MsgInfoMap, obj.ROSMessageType); rosmsg = rosmessage(obj.ROSMessageType); % blank message rosmsg.fromStruct(busstruct); end end end %% %% function assertInfoStructNames() % processStruct() assumes the the variable-array info struct has a field % name named 'CurrentLength' (doesn't look up the fieldname at runtime). % Verify that this assumption is valid. infostruct = robotics.slros.internal.bus.Util.getArrayInfoStructMetadata(); assert(strcmp(infostruct.CurLengthProp, 'CurrentLength')); end %% function busstruct = processStruct(busstruct, map, msgtype) info = map(msgtype); % For list of expected fields in info, see % createROSMsgInfoMap:processBus() % if ~isempyt(info.StdEmptyMsgProp) % Nothing to do. We silently ignore (throw away) the dummy property % end % Ensure variable-length array information is captured in the % metadata (_SL_Info) field for i=1:numel(info.VarLenArrays) prop = info.VarLenArrays{i}; infoprop = info.VarLenArrayInfoProps{i}; maxlen = info.VarLenMaxLen{i}; actuallen = numel(busstruct.(prop)); validlen = busstruct.(infoprop).CurrentLength; % Note: % 1) busstruct.(lengthprop).ReceivedLength will be discarded % (not meaningful during bus -> ROS message conversion) % % 2) In practice, actuallen will be equal to validlen (there is no % way in Simulink to extend the max length of an array during a % simulation). However, this function receives an anonymous struct % & can't make this assumption, so we handle the case where % actuallen > validlen assert((validlen <= actuallen) && (actuallen <= maxlen)); if validlen < actuallen busstruct.(prop)(validlen+1:actuallen) = []; % remove extra elements end end % Uint64 and Int64 values will be handled by fromStruct() for the ROS % Message (i.e., it will cast them appropriately, so no need for explicit % casts back to double) % convert strings to uint8 for i=1:numel(info.StringProps) prop = info.StringProps{i}; busstruct.(prop) = reshape(char(busstruct.(prop)), 1, []); % ensure it is row-vector end % no need to remove "CurrentLength" fields; they are ignored % Future optimization: we don't have to process all arrays, only those % with a variable-length array under them (and this is discoverable through % static analysis) % note that we've already trimmed the length of variable-length arrays, so % they have the correct length. for i=1:numel(info.NestedMsgs) prop = info.NestedMsgs{i}; nestedMsgType = info.NestedMsgType{i}; len = numel(busstruct.(prop)); % how about multi-dimensional arrays? substructs = struct.empty; for j=1:len tmpout = processStruct( ... busstruct.(prop)(j), ... map, ... nestedMsgType); if isempty(substructs) substructs = tmpout; else substructs(j) = tmpout; % extend as row or column? end end busstruct.(prop) = substructs; end % map back to a cell array of strings for i=1:numel(info.StringArrayProps) prop = info.StringArrayProps{i}; proplen = info.StringArrayLen{i}; if proplen == 0 % variable-length array infoprop = info.StringArrayInfoProps{i}; validlen = busstruct.(infoprop).CurrentLength; else % fixed-length array validlen = proplen; end % Note that busstruct.(prop) has already been trimmed to the correct % length, and the busstruct.(prop).Data has already been converted to % a MATLAB string -- this is just the standard processing for any % array of std_msgs/String[]. if validlen == 0 busstruct.(prop) = {}; else busstruct.(prop) = {busstruct.(prop).Data}; end end % If there are reserved names, make sure to have non-mangled % property names and remove the mangled property names. for i=1:numel(info.ReservedWordProp) busName = info.ReservedWordProp(i).BusName; rosName = info.ReservedWordProp(i).RosName; % Copy the content to the non-mangled property name, so it can be used % to reconstruct the ROS message object busstruct.(rosName) = busstruct.(busName); % Remove the mangled property name busstruct = rmfield(busstruct, busName); end end