www.gusucode.com > robotsimulink 工具箱 matlab源码程序 > robotsimulink/robotslros/+robotics/+slros/+internal/+bus/getBusDefnForROSMsg.m
function requiredBuses = getBusDefnForROSMsg(emptyRosMsg, model) %This function is for internal use only. It may be removed in the future. %getBusDefnForROSMsg - Create Simulink bus object corresponding to a ROS message % % getBusDefnForROSMsg(MSG) returns an array of Simulink.Bus objects % corresponding to ROS message MSG and any nested messages inside it. % MSG should be an empty message (i.e., created using ROSMESSAGE without % any subsequent assignments), since the only way to determine if a % property is a variable-length array is to check if its value is []. % % Note: % * This function does not create the bus objects in the base workspace % Consequently, this function does not define the "name" of the bus % (i.e., the name of the bus in the base workspace). % % * If the ROS message has variable-size array properties, these are % converted to fixed-length arrays (with length of 1), and the % associated metadata element is added to the bus object. % % * This function returns a bus object (which is a bus definition), & % not an instance of a bus (a bus signal). % Copyright 2014-2016 The MathWorks, Inc. % Good test for this function: % getBusDefnForROSMsg( rosmessage('calibration_msgs/RobotMeasurement') ) map = containers.Map; processBus(emptyRosMsg, model, map); requiredBusesCellArray = values(map); requiredBuses = [requiredBusesCellArray{:}]; end %% function processBus(emptyRosMsg, model, map) if isempty(emptyRosMsg) % nothing to do return; end % should not be making any recursive calls with arrays of messages assert(numel(emptyRosMsg)==1); if robotics.slros.internal.ROSUtil.isROSTimeEntityObj(emptyRosMsg) msgType = robotics.slros.internal.bus.Util.rosTimeToQuasiMsgType(class(emptyRosMsg)); else assert(all(isprop(emptyRosMsg, 'MessageType'))); msgType = emptyRosMsg.MessageType; end if isKey(map, msgType) % nothing to do return; end propertyList = fieldnames(emptyRosMsg); % During a simulation, we will use msg.toStruct() to convert the ROS % message to a Simulink bus struct. Consequently, it is important for the % **order of elements** in the bus struct to be as close as possible to the % order of fields returned by msg.toStruct(). Hence, call toStruct() here % to get a "canonical order", and add elements to the bus definition in % that order. isRosTimeEntity = robotics.slros.internal.ROSUtil.isROSTimeEntityObj(emptyRosMsg); if isRosTimeEntity % no toStruct() method defined for time entities canonicalOrder = propertyList; else canonicalOrder = fieldnames(toStruct(emptyRosMsg)); % The toStruct() leaves out some fields. Splice these back in (though % these will likely just be MessageType and Constant properties, which % will be discarded later on anyway). canonicalOrder = [setdiff(propertyList, canonicalOrder) ; canonicalOrder]; end % Use metaclass, as we need to distinguish Constant properties below rosmsgMetaClass = metaclass(emptyRosMsg); rosmsgAllProps = {rosmsgMetaClass.PropertyList.Name}; busElements = Simulink.BusElement.empty; if robotics.slros.internal.ROSUtil.isStdEmptyMsgType(msgType) % If std_msgs/Empty, the only field should be 'MessageType' assert( (numel(canonicalOrder) == 1) && strcmpi(canonicalOrder{1}, 'MessageType') ); % Add in the dummy field (the loop below over canonicalOrder is effectively a no-op) elem = robotics.slros.internal.bus.Util.getDummyElementForStdEmptyMsg; busElements(end+1) = elem; end nameChecker = robotics.slros.internal.bus.ReservedNamesChecker.getInstance; for i =1:length(canonicalOrder) propertyName = canonicalOrder{i}; if strcmp(propertyName,'MessageType') continue; end metaprop = rosmsgMetaClass.PropertyList(strcmp(propertyName, rosmsgAllProps)); isConstant = metaprop.Constant; if isConstant % Constants are not yet supported continue; end [elemName,isReservedName] = nameChecker.mangleNameIfNeeded(propertyName); elem = Simulink.BusElement; elem.Name = elemName; elem.Dimensions = 1; elem.SampleTime = -1; elem.Complexity = 'real'; elem.SamplingMode = 'Sample based'; elem.Min = []; elem.Max = []; elem.DocUnits = ''; elem.Description = ''; elemInfo = robotics.slros.internal.bus.BusItemInfo; if isReservedName % remember the original (unmangled) name elemInfo.ROSPropName = propertyName; end data = emptyRosMsg.(propertyName); isLogicalOrNumericType = isa(data,'logical') || isa(data,'numeric'); isStringType = isa(data, 'char'); isStringArrayType = isa(data, 'cell'); isRosTimeEntity = robotics.slros.internal.ROSUtil.isROSTimeEntityObj(data); % isROSMsgObj returns true even if data is zero-length array of expected type isRosMessage = robotics.slros.internal.ROSUtil.isROSMsgObj(data); %------ % Handle primitive and complex message properties if isStringArrayType % Variable or fixed-length array of strings. % Replace the 'data' cell array with an array of std_msgs/String numStrings = numel(data); tmpStringMsg = robotics.slros.internal.ROSUtil.getStdStringObj(); if numStrings == 0 % variable-length array of strings data = tmpStringMsg.empty(0,1); else % fixed-length array of strings % Note % 1) We are only using the 'data' variable to set up datatypes; % the actual content of 'data' is not used or modified. % 2) We can't just assign to data(1:numStrings,1); that syntax % requires LHS and RHS to have matching types, whereas here we % are replacing a cell array with a new data type. data = tmpStringMsg; data(2:numStrings,1) = tmpStringMsg; end rosMessageType = tmpStringMsg.MessageType; dataTypeStr = robotics.slros.internal.bus.Util.rosMsgTypeToDataTypeStr(rosMessageType, model); elem.DataType = dataTypeStr; elemInfo.MsgType = rosMessageType; elemInfo.PrimitiveROSType = 'string[]'; processBus(tmpStringMsg, model, map); % recursive call elseif isLogicalOrNumericType if isa(data,'logical') elem.DataType = 'boolean'; elseif isa(data, 'int64') || isa(data, 'uint64') elem.DataType = 'double'; elemInfo.Int64Type = class(data); robotics.internal.warningNoBacktrace(... message('robotics:robotslros:busconvert:Int64NotSupported', ... propertyName, msgType, class(data), 'double')); else elem.DataType = class(data); end elseif isStringType elem.DataType = 'uint8'; elemInfo.PrimitiveROSType = 'string'; % remember that this uint8 array is a string elseif isRosTimeEntity datainstance = getDataInstance(data); [dataTypeStr, ~, timeMsgType] = robotics.slros.internal.bus.Util.rosTimeToDataTypeStr(class(data), model); elem.DataType = dataTypeStr; elemInfo.MsgType = timeMsgType; processBus(datainstance, model, map); % recursive call elseif isRosMessage datainstance = getDataInstance(data); assert(isprop(datainstance, 'MessageType')); rosMessageType = datainstance.MessageType; dataTypeStr = robotics.slros.internal.bus.Util.rosMsgTypeToDataTypeStr(rosMessageType, model); elem.DataType = dataTypeStr; elemInfo.MsgType = rosMessageType; processBus(datainstance, model, map); % recursive call else assert(false); end %------ % Add metadata related to variable vs. fixed-length arrays isVarsizeArray = isempty(data) || isStringType; if isVarsizeArray elemInfo.IsVarLen = true; elem.DimensionsMode = 'Variable'; % SL buses require non-zero value for dimensions, so set it to 1 % for now. Conversion from Variable-length to Fixed-length array % will be done later (after getting user data on maximum lengths) elem.Dimensions = max(numel(data),1); elem.Description = elemInfo.toDescription(); % Add the array and the corresponding SL_Info element to the bus [elem, arrayInfoElem] = addArrayInfoElement(elem); busElements(end+1:end+2) = [elem ; arrayInfoElem]; else elemInfo.IsVarLen = false; elem.DimensionsMode = 'Fixed'; elem.Dimensions = max(numel(data),1); elem.Description = elemInfo.toDescription(); busElements(end+1) = elem; %#ok<AGROW> end end businfo = robotics.slros.internal.bus.BusItemInfo; businfo.MsgType = msgType; mybus = Simulink.Bus; mybus.HeaderFile = ''; mybus.Description = businfo.toDescription(); mybus.DataScope = 'Auto'; mybus.Alignment = -1; mybus.Elements = busElements; map(msgType) = struct('MessageType', msgType, 'Bus', mybus); %#ok<NASGU> end %% function [arrayElem, arrayInfoElem] = addArrayInfoElement(arrayElem) assert(strcmp(arrayElem.DimensionsMode, 'Variable')); infoElemName = robotics.slros.internal.bus.Util.getArrayInfoElementName(arrayElem.Name); busItemInfo = robotics.slros.internal.bus.BusItemInfo(arrayElem.Description); busItemInfo.IsVarLen = true; busItemInfo.VarLenCategory = 'data'; busItemInfo.VarLenElem = infoElemName; arrayElem.Dimensions = 1; % temporary length arrayElem.DimensionsMode = 'Fixed'; arrayElem.Description = busItemInfo.toDescription(); busItemInfo2 = robotics.slros.internal.bus.BusItemInfo; busItemInfo2.IsVarLen = true; busItemInfo2.VarLenCategory = 'length'; busItemInfo2.VarLenElem = arrayElem.Name; arrayInfoElem = Simulink.BusElement; arrayInfoElem.Name = infoElemName; arrayInfoElem.DataType = robotics.slros.internal.bus.Util.varlenInfoBusDataTypeStr(); arrayInfoElem.DimensionsMode = 'Fixed'; arrayInfoElem.Dimensions = 1; arrayInfoElem.SamplingMode = 'Sample based'; arrayInfoElem.Description = busItemInfo2.toDescription(); end %% function datainstance = getDataInstance(data) if isempty(data) datainstance = feval(class(data)); else datainstance = data(1); end end