www.gusucode.com > robotsimulink 工具箱 matlab源码程序 > robotsimulink/robotslros/+robotics/+slros/+internal/+bus/Util.m
classdef Util
%This class is for internal use only. It may be removed in the future.
%BUS.UTIL - Utility functions for working with Simulink buses
% Copyright 2014-2016 The MathWorks, Inc.
properties(Constant)
BusNamePrefix = 'SL_Bus_'
VarlengthInfoBusName = 'SL_Bus_ROSVariableLengthArrayInfo'
end
%% Utility functions related to variable-length arrays
methods (Static)
function [datatype,busName] = varlenInfoBusDataTypeStr()
busName = robotics.slros.internal.bus.Util.VarlengthInfoBusName;
datatype = ['Bus: ' busName];
end
function s = getVarlenInfoStruct(curlen, recvdlen)
s.CurrentLength = uint32(curlen);
s.ReceivedLength = uint32(recvdlen);
end
function createVarlenInfoBusIfNeeded()
busName = robotics.slros.internal.bus.Util.VarlengthInfoBusName;
busExists = ~isempty(evalin('base', ['whos(''' busName ''')']));
if ~busExists
elems(1) = Simulink.BusElement;
elems(1).Name = 'CurrentLength';
elems(1).Dimensions = 1;
elems(1).DimensionsMode = 'Fixed';
elems(1).DataType = 'uint32';
elems(1).Complexity = 'real';
elems(1).SamplingMode = 'Sample based';
elems(1).Description = '';
elems(2) = Simulink.BusElement;
elems(2).Name = 'ReceivedLength';
elems(2).Dimensions = 1;
elems(2).DimensionsMode = 'Fixed';
elems(2).DataType = 'uint32';
elems(2).Complexity = 'real';
elems(2).SamplingMode = 'Sample based';
elems(2).Description = '';
bus = Simulink.Bus;
bus.Description = '';
bus.Elements = elems;
assignin('base', busName, bus);
end
end
function infostruct = getArrayInfoStructMetadata()
infostruct.PropertySuffix = '_SL_Info';
infostruct.CurLengthProp = 'CurrentLength';
infostruct.RcvdLengthProp = 'ReceivedLength';
infostruct.LengthTypeSL = 'uint32';
infostruct.LengthTypeCpp = 'uint32_T';
end
function arrayInfoElemName = getArrayInfoElementName(arrayElemName)
infostruct = robotics.slros.internal.bus.Util.getArrayInfoStructMetadata();
arrayInfoElemName = [arrayElemName infostruct.PropertySuffix];
end
end
%% Utility functions for handling std_msgs/Empty
methods (Static)
function name = getDummyElementNameForStdEmptyMsg()
name = 'SL_DummyData';
end
function elem = getDummyElementForStdEmptyMsg()
elem = Simulink.BusElement;
elem.Name = robotics.slros.internal.bus.Util.getDummyElementNameForStdEmptyMsg;
elem.Dimensions = 1;
elem.DimensionsMode = 'Fixed';
elem.DataType = 'boolean';
elem.Complexity = 'real';
elem.SamplingMode = 'Sample based';
elem.Description = '';
end
end
%% General Bus-related utilities
methods (Static)
function allMsgsMap = getAllMessageInfoMapForModel(model)
validateattributes(model, {'char'}, {'nonempty'});
topLevelMsgTypes = robotics.slros.internal.bus.Util.getBlockLevelMessageTypesInModel(model);
allMsgsMap = containers.Map;
for i=1:numel(topLevelMsgTypes)
map = robotics.slros.internal.sim.createROSMsgInfoMap(struct(...
'MessageType', topLevelMsgTypes{i}, ...
'ModelName', model, ...
'MapKeyType', 'msgtype', ...
'Recurse', true));
requiredMsgs = keys(map);
for j=1:numel(requiredMsgs)
if ~isKey(allMsgsMap, requiredMsgs{j})
allMsgsMap(requiredMsgs{j}) = map(requiredMsgs{j});
end
end
end
end
function msgTypes = getBlockLevelMessageTypesInModel(model)
validateattributes(model, {'char'}, {'nonempty'});
[~, ~, blockMsgTypes] = robotics.slros.internal.bus.Util.getROSBlocksInModel(model);
msgTypes = unique( blockMsgTypes );
end
function [blockList, paramBlockList, blockMsgTypes] = getROSBlocksInModel(model)
validateattributes(model, {'char'}, {'nonempty'});
lbdata = libinfo(bdroot(model), ...
'LookUnderMasks', 'all', ...
'Regexp', 'on', ...
'MaskType', 'ROS (Publish|Subscribe|Blank Message)');
blockList = {lbdata(:).Block};
blockList = blockList(:);
if nargout > 2
blockMsgTypes = get_param(blockList,'messageType');
end
% Find Parameter Blocks
lbdata = libinfo(bdroot(model), ...
'LookUnderMasks', 'all', ...
'Regexp', 'on', ...
'MaskType', 'ROS (Set Parameter|Get Parameter)');
paramBlockList = {lbdata(:).Block};
paramBlockList = paramBlockList(:);
end
function clearSLBusesInBaseWorkspace()
evalin('base', ['clear ' robotics.slros.internal.bus.Util.BusNamePrefix '*']);
end
function bus = getBusObjectFromBusName(busName)
bus = evalin('base', busName);
end
function [bus,busName] = getBusObjectFromMsgType(rosMsgType, model)
busName = robotics.slros.internal.bus.Util.createBusIfNeeded(rosMsgType, model);
bus = evalin('base', busName);
end
function busname = getBusNameFromDataTypeStr(dataTypeStr)
matches = regexp(dataTypeStr, 'Bus:[ ]\s*(.*)', 'tokens');
if ~isempty(matches)
busname = matches{1}{1};
else
busname = '';
end
end
function [busExists,busName] = checkForBus(rosMsgType, model)
busName = robotics.slros.internal.bus.Util.rosMsgTypeToBusName(rosMsgType, model);
busExists = ~isempty(evalin('base', ['whos(''' busName ''')']));
end
function busName = createBusIfNeeded(rosMsgType, model)
validateattributes(rosMsgType, {'char'}, {'nonempty'});
validateattributes(model, {'char'}, {});
[busExists,busName] = robotics.slros.internal.bus.Util.checkForBus(rosMsgType, model);
if busExists
return;
end
% If message class does not exist, rosmessage will error
% (robotics:ros:message:NoMatlabClass).
emptyRosMsg = rosmessage(rosMsgType); % create an empty rosmessage
robotics.slros.internal.bus.createBusDefnInBaseWorkspace(emptyRosMsg, model);
end
%%
function [datatype, busName, msgType] = rosTimeToDataTypeStr(rosClassName, model)
msgType = robotics.slros.internal.bus.Util.rosTimeToQuasiMsgType(rosClassName);
[datatype, busName] = robotics.slros.internal.bus.Util.rosMsgTypeToDataTypeStr(msgType, model);
end
function quasiMsgType = rosTimeToQuasiMsgType(rosClassName)
entity = robotics.slros.internal.ROSUtil.getTimeEntityType(rosClassName);
if ~isempty(entity)
quasiMsgType = ['ros_time/' entity]; % can be Time or Duration
else
assert(false, 'Unexpected classname: %s', rosClassName);
end
end
%%
function [datatype,busName] = rosMsgTypeToDataTypeStr(rosMsgType, model)
% This is used wherever a Simulink DataTypeStr is required
% (e.g., for specifying the output datatype of a Constant block)
% ** DOES NOT CREATE A BUS **
busName = robotics.slros.internal.bus.Util.rosMsgTypeToBusName(rosMsgType, model);
datatype = ['Bus: ' busName];
end
function busName = rosMsgTypeToBusName(rosMsgType, model)
%
% rosMsgTypeToBusName(MSGTYPE,MODEL) returns the bus name
% corresponding to a ROS message type MSGTYPE (e.g.,
% 'std_msgs/Int32') and a Simulink model MODEL. The function
% uses the following rules:
%
% Rule 1 - Generate a name using the format:
% SL_Bus_<modelname>_<msgtype>
%
% Rule 2 - If the result of Rule 1 is longer than 60
% characters, use the following general format:
% SL_Bus_<modelname(1:25)>_<msgtype(end-25:end)>_<hash>
% where <hash> is a base 36 hash of the full name (output of
% rule #1).
%
% ** THIS FUNCTION DOES NOT CREATE A BUS OBJECT **
validateattributes(rosMsgType, {'char'}, {'nonempty'});
assert(ischar(model));
if isempty(model)
modelnameSuffix = '';
else
modelnameSuffix = '_';
end
maxlen = 60; choplen=25;
assert(maxlen <= namelengthmax);
busName = [robotics.slros.internal.bus.Util.BusNamePrefix ...
model modelnameSuffix rosMsgType];
if length(busName) < maxlen
busName = matlab.lang.makeValidName(busName, 'ReplacementStyle', 'underscore');
else
% add a trailing hash string (5-6 chars) to make the busname unique
hashStr = robotics.slros.internal.bus.Util.hashString(busName);
idx = strfind(rosMsgType, '/');
if isempty(idx)
idx = 0;
else
idx = idx(1);
end
model = model(1:min(end,choplen)); % get first 25 chars
rosMsgType = rosMsgType(idx+1:min(idx+choplen,end)); % get first 25 chars
busName = matlab.lang.makeValidName(...
[robotics.slros.internal.bus.Util.BusNamePrefix ...
model modelnameSuffix rosMsgType '_' hashStr], ...
'ReplacementStyle', 'underscore');
end
end
end
methods(Static, Access=private)
function hashStr = hashString(str)
javaStr = java.lang.String(str);
hashStr = lower(dec2base(uint64(abs(javaStr.hashCode)), 36)); % usually 5-6 chars
end
end
end