www.gusucode.com > robotsimulink 工具箱 matlab源码程序 > robotsimulink/robotslros/+robotics/+slros/+internal/+cgen/getBusToCppConversionFcns.m
function [bus2cppFcnMap, cpp2busFcnMap] = getBusToCppConversionFcns(emptyROSMsg, model) %This function is for internal use only. It may be removed in the future. %getBusToCppConversionFcns - Generate C++ conversion code for a message type % % [BUS2CPP, CPP2BUS] = getBusToCppConversionFcns(MSG, MODEL) generates % the C++ code for converting ROS->Bus and Bus->ROS for a single ROS % message MSG and any nested messages inside it, using the % variable-length array size information from MODEL. 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 []. % % Outputs % BUS2CPP, CPP2BUS : containers.Map objects that map a message type to % corresponding StringWriter objects with generated C++ code for % Bus->ROS (or ROS->Bus). % Copyright 2014-2016 The MathWorks, Inc. params.ConvertFromBusFcnStr = 'convertFromBus'; params.ConvertToBusFcnStr = 'convertToBus'; params.Indent = 2; params.ModelName = model; bus2cppFcnMap = containers.Map; generateConversionFcns('bus2cpp', emptyROSMsg, params, bus2cppFcnMap); cpp2busFcnMap = containers.Map; generateConversionFcns('cpp2bus', emptyROSMsg, params, cpp2busFcnMap); % There should be a 1-1 relation between % bus2cpp and cpp2bus conversion functions assert(all(strcmp(sort(keys(bus2cppFcnMap)), sort(keys(cpp2busFcnMap)))), ... 'Mismatch detected for %s', class(emptyROSMsg)); end %% function generateConversionFcns(direction, emptyROSMsg, params, fcnInfoMap) convertFromBus2Cpp = strcmp(direction, 'bus2cpp'); [cppRosClass, msgType] = robotics.slros.internal.cgen.Util.rosObjToCppClass(emptyROSMsg); if fcnInfoMap.isKey(cppRosClass) % information already present return; end params.MsgType = msgType; [bus, slBusName] = robotics.slros.internal.bus.Util.getBusObjectFromMsgType(... msgType, params.ModelName); busElementNames = {bus.Elements.Name}; % -- Generate function signature fcnSignature = StringWriter; if convertFromBus2Cpp fcnSignature.add('void %s(%s* msgPtr, %s const* busPtr)', ... params.ConvertFromBusFcnStr, cppRosClass, slBusName); else fcnSignature.add('void %s(%s* busPtr, %s const* msgPtr)', ... params.ConvertToBusFcnStr, slBusName, cppRosClass); end % -- Generate function body fcnBody = StringWriter; fcnBody.addcr('{'); fcnBody.Indent = params.Indent; upstreamDependencies = {}; nameChecker = robotics.slros.internal.bus.ReservedNamesChecker.getInstance(); [propList, rosPropList] = robotics.slros.internal.ROSUtil.getPropertyLists(emptyROSMsg); fcnBody.addcr('const std::string rosMessageType("%s");', params.MsgType); fcnBody.addcr; for idx=1:numel(propList) matlabPropName = propList{idx}; propinfo.MATLABPropName = matlabPropName; % If matlabPropName is reserved name, then the bus name has to be mangled propinfo.BusPropName = nameChecker.mangleNameIfNeeded(matlabPropName); propinfo.CppPropName = rosPropList{idx}; % property name used in C++ object busIdx = strcmpi(propinfo.BusPropName, busElementNames); assert(find(length(busIdx)) == 1); % should find exactly one element busElement = bus.Elements(busIdx); elemInfo = robotics.slros.internal.bus.BusItemInfo(busElement.Description); data = emptyROSMsg.(propinfo.MATLABPropName); propinfo.IsLogicalOrNumericType = isa(data,'logical') || isa(data,'numeric'); propinfo.IsStringType = strcmpi(elemInfo.PrimitiveROSType, 'string'); propinfo.IsStringArray = strcmpi(elemInfo.PrimitiveROSType, 'string[]'); propinfo.IsRosEntity = robotics.slros.internal.ROSUtil.isROSObj(data); propinfo.Int64Type = elemInfo.Int64Type; propinfo.IsVarsizeArray = elemInfo.IsVarLen; propinfo.IsFixedLenArray = ~elemInfo.IsVarLen; if elemInfo.IsVarLen && strcmpi(elemInfo.VarLenCategory, 'data') propinfo.ArrayInfoPropName = elemInfo.VarLenElem; else propinfo.ArrayInfoPropName = ''; end propinfo.WarnOnTruncate = strcmpi(elemInfo.TruncateAction, 'warn'); if propinfo.IsRosEntity % Get an instance of the ROS entity so that it can be used for % recursion. If data is an empty-variable length array, % getDataInstance will create an instance. % % Note: a string array is specifically not a ROS entity (i.e., a % nested ROS message). In Simulink it is mapped to an array of % std_msgs/String, but in ROS C++ there is no corresponding % std_msgs/String object. upstreamDependencies{end+1} = getDataInstance(data); %#ok<AGROW> end assert(all(size(busElement.Dimensions) == [1 1]), 'Expecting a scalar'); propinfo.BusArrayMaxLength = busElement.Dimensions; if propinfo.IsStringType % string (treated as variable-length array of uint8) copyArray(convertFromBus2Cpp, propinfo, params, fcnBody, 'PrimitiveArray'); elseif propinfo.IsStringArray % string[] is treated as a variable-length array of std_msgs/String % In the MATLAB ROS message, string[] is not a ROS message assert(~propinfo.IsRosEntity); copyArray(convertFromBus2Cpp, propinfo, params, fcnBody, 'StringArray'); elseif propinfo.IsFixedLenArray && numel(data)==1 % Scalar property: numeric, logical or nested message simpleAssignment(convertFromBus2Cpp, propinfo, params, fcnBody); elseif propinfo.IsLogicalOrNumericType % Array of primitive type (fixed or variable-length) copyArray(convertFromBus2Cpp, propinfo, params, fcnBody, 'PrimitiveArray'); else % Array of nested messages (fixed or variable-length) assert(propinfo.IsRosEntity); copyArray(convertFromBus2Cpp, propinfo, params, fcnBody, 'NestedArray'); end end fcnBody.Indent = fcnBody.Indent - params.Indent; fcnBody.addcr('}'); fcnInfo.FcnSignature = fcnSignature; fcnInfo.FcnBody = fcnBody; fcnInfo.CppRosClass = cppRosClass; fcnInfo.SLBusName = slBusName; fcnInfoMap(cppRosClass) = fcnInfo; for i=1:numel(upstreamDependencies) generateConversionFcns(direction, upstreamDependencies{i}, params, fcnInfoMap); end end %% function datainstance = getDataInstance(data) % Return need an instance of a ROS message. If data is empty (indicating a % empty variable-length array), construct an instance of the message. if isempty(data) datainstance = feval(class(data)); else datainstance = data(1); end end %% function [from,to] = getFromAndTo(convertFromBus2Cpp, propinfo, params) to.assignmentCastStr = ''; if convertFromBus2Cpp from.PtrStr = 'busPtr'; from.PropName = propinfo.BusPropName; to.PtrStr = 'msgPtr'; to.PropName = propinfo.CppPropName; to.conversionFcnStr = params.ConvertFromBusFcnStr; if ~isempty(propinfo.Int64Type) if strcmpi(propinfo.Int64Type, 'int64') to.assignmentCastStr = '(int64_t)'; else to.assignmentCastStr = '(uint64_t)'; end end else % cpp2bus from.PtrStr = 'msgPtr'; from.PropName = propinfo.CppPropName; to.PtrStr = 'busPtr'; to.PropName = propinfo.BusPropName; to.conversionFcnStr = params.ConvertToBusFcnStr; if propinfo.IsStringType to.assignmentCastStr = '(uint8_T)'; end if ~isempty(propinfo.Int64Type) to.assignmentCastStr = '(real_T)'; % real_T is 64-bit float end end end %% function simpleAssignment(convertFromBus2Cpp, propinfo, params, buf) [from,to] = getFromAndTo(convertFromBus2Cpp, propinfo, params); if propinfo.IsLogicalOrNumericType buf.addcr('%s->%s = %s %s->%s;', to.PtrStr, to.PropName, to.assignmentCastStr, from.PtrStr, from.PropName); elseif propinfo.IsRosEntity % nested message buf.addcr('%s(&%s->%s, &%s->%s);', to.conversionFcnStr, ... to.PtrStr, to.PropName, from.PtrStr, from.PropName); else assert(false); end end %% % Called for either fixed-length arrays (length > 1), or variable-lenth arrays function copyArray(convertFromBus2Cpp, propinfo, params, buf, fcnNameSuffix) validatestring(fcnNameSuffix, {'PrimitiveArray', 'StringArray', 'NestedArray'}); % This function will generate one of the following functions (these are % explicitly listed here for findability in a code search): % % For PrimitiveArray % convertToBusFixedPrimitiveArray % convertToBusVariablePrimitiveArray % convertFromBusFixedPrimitiveArray % convertFromBusVariablePrimitiveArray % % For StringArray % convertToBusFixedStringArray % convertToBusVariableStringArray % convertFromBusFixedStringArray % convertFromBusVariableStringArray % % For NestedArray % convertToBusFixedNestedArray % convertToBusVariableNestedArray % convertFromBusFixedNestedArray % convertFromBusVariableNestedArray [from,to] = getFromAndTo(convertFromBus2Cpp, propinfo, params); if convertFromBus2Cpp % convert from bus to roscpp if propinfo.IsVarsizeArray % Variable-length array buf.addcr('convertFromBusVariable%s(%s->%s, %s->%s, %s->%s);', ... fcnNameSuffix, ... to.PtrStr, to.PropName, ... from.PtrStr, from.PropName, ... from.PtrStr, propinfo.ArrayInfoPropName); else % Fixed-size array buf.addcr('convertFromBusFixed%s(%s->%s, %s->%s);', ... fcnNameSuffix, ... to.PtrStr, to.PropName, ... from.PtrStr, from.PropName); end else % convert from roscpp to bus if propinfo.IsVarsizeArray % Variable-length array if propinfo.WarnOnTruncate warnStr = sprintf('slros::EnabledWarning(rosMessageType, "%s")', propinfo.CppPropName); else warnStr = 'slros::DisabledWarning()'; end buf.addcr('convertToBusVariable%s(%s->%s, %s->%s, %s->%s, %s);', ... fcnNameSuffix, ... to.PtrStr, to.PropName, ... to.PtrStr, propinfo.ArrayInfoPropName, ... from.PtrStr, from.PropName, ... warnStr); else % Fixed-size array warnStr = 'slros::NoopWarning()'; buf.addcr('convertToBusFixed%s(%s->%s, %s->%s, %s);', ... fcnNameSuffix, ... to.PtrStr, to.PropName, ... from.PtrStr, from.PropName, ... warnStr); end end end