00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 import genmsg.msgs
00034
00035 try:
00036 from cStringIO import StringIO
00037 except ImportError:
00038 from io import StringIO
00039
00040 MSG_TYPE_TO_CPP = {'byte': 'int8_t',
00041 'char': 'uint8_t',
00042 'bool': 'uint8_t',
00043 'uint8': 'uint8_t',
00044 'int8': 'int8_t',
00045 'uint16': 'uint16_t',
00046 'int16': 'int16_t',
00047 'uint32': 'uint32_t',
00048 'int32': 'int32_t',
00049 'uint64': 'uint64_t',
00050 'int64': 'int64_t',
00051 'float32': 'float',
00052 'float64': 'double',
00053 'string': 'std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > ',
00054 'time': 'ros::Time',
00055 'duration': 'ros::Duration'}
00056
00057
00058 def msg_type_to_cpp(type):
00059 """
00060 Converts a message type (e.g. uint32, std_msgs/String, etc.) into the C++ declaration
00061 for that type (e.g. uint32_t, std_msgs::String_<ContainerAllocator>)
00062
00063 @param type: The message type
00064 @type type: str
00065 @return: The C++ declaration
00066 @rtype: str
00067 """
00068 (base_type, is_array, array_len) = genmsg.msgs.parse_type(type)
00069 cpp_type = None
00070 if (genmsg.msgs.is_builtin(base_type)):
00071 cpp_type = MSG_TYPE_TO_CPP[base_type]
00072 elif (len(base_type.split('/')) == 1):
00073 if (genmsg.msgs.is_header_type(base_type)):
00074 cpp_type = ' ::std_msgs::Header_<ContainerAllocator> '
00075 else:
00076 cpp_type = '%s_<ContainerAllocator> '%(base_type)
00077 else:
00078 pkg = base_type.split('/')[0]
00079 msg = base_type.split('/')[1]
00080 cpp_type = ' ::%s::%s_<ContainerAllocator> '%(pkg, msg)
00081
00082 if (is_array):
00083 if (array_len is None):
00084 return 'std::vector<%s, typename ContainerAllocator::template rebind<%s>::other > '%(cpp_type, cpp_type)
00085 else:
00086 return 'boost::array<%s, %s> '%(cpp_type, array_len)
00087 else:
00088 return cpp_type
00089
00090 def _escape_string(s):
00091 s = s.replace('\\', '\\\\')
00092 s = s.replace('"', '\\"')
00093 return s
00094
00095 def escape_message_definition(definition):
00096 lines = definition.splitlines()
00097 if not lines:
00098 lines.append('')
00099 s = StringIO()
00100 for line in lines:
00101 line = _escape_string(line)
00102 s.write('%s\\n\\\n'%(line))
00103
00104 val = s.getvalue()
00105 s.close()
00106 return val
00107
00108
00109 def cpp_message_declarations(name_prefix, msg):
00110 """
00111 Returns the different possible C++ declarations for a message given the message itself.
00112
00113 @param name_prefix: The C++ prefix to be prepended to the name, e.g. "std_msgs::"
00114 @type name_prefix: str
00115 @param msg: The message type
00116 @type msg: str
00117 @return: A tuple of 3 different names. cpp_message_decelarations("std_msgs::", "String") returns the tuple
00118 ("std_msgs::String_", "std_msgs::String_<ContainerAllocator>", "std_msgs::String")
00119 @rtype: str
00120 """
00121 pkg, basetype = genmsg.names.package_resource_name(msg)
00122 cpp_name = ' ::%s%s'%(name_prefix, msg)
00123 if (pkg):
00124 cpp_name = ' ::%s::%s'%(pkg, basetype)
00125 return ('%s_'%(cpp_name), '%s_<ContainerAllocator> '%(cpp_name), '%s'%(cpp_name))
00126
00127
00128 def is_fixed_length(spec, msg_context, includepath):
00129 """
00130 Returns whether or not the message is fixed-length
00131
00132 @param spec: The message spec
00133 @type spec: genmsg.msgs.MsgSpec
00134 @param package: The package of the
00135 @type package: str
00136 """
00137 types = []
00138 for field in spec.parsed_fields():
00139 if (field.is_array and field.array_len is None):
00140 return False
00141
00142 if (field.base_type == 'string'):
00143 return False
00144
00145 if (not field.is_builtin):
00146 types.append(field.base_type)
00147
00148 types = set(types)
00149 for t in types:
00150 t = genmsg.msgs.resolve_type(t, spec.package)
00151 assert isinstance(includepath, dict)
00152 new_spec = genmsg.msg_loader.load_msg_by_type(msg_context, t, includepath)
00153 if (not is_fixed_length(new_spec, msg_context, includepath)):
00154 return False
00155
00156 return True
00157
00158
00159 def default_value(type):
00160 """
00161 Returns the value to initialize a message member with. 0 for integer types, 0.0 for floating point, false for bool,
00162 empty string for everything else
00163
00164 @param type: The type
00165 @type type: str
00166 """
00167 if type in ['byte', 'int8', 'int16', 'int32', 'int64',
00168 'char', 'uint8', 'uint16', 'uint32', 'uint64']:
00169 return '0'
00170 elif type in ['float32', 'float64']:
00171 return '0.0'
00172 elif type == 'bool':
00173 return 'false'
00174
00175 return ""
00176
00177 def takes_allocator(type):
00178 """
00179 Returns whether or not a type can take an allocator in its constructor. False for all builtin types except string.
00180 True for all others.
00181
00182 @param type: The type
00183 @type: str
00184 """
00185 return not type in ['byte', 'int8', 'int16', 'int32', 'int64',
00186 'char', 'uint8', 'uint16', 'uint32', 'uint64',
00187 'float32', 'float64', 'bool', 'time', 'duration']
00188
00189 def escape_string(str):
00190 str = str.replace('\\', '\\\\')
00191 str = str.replace('"', '\\"')
00192 return str
00193
00194
00195 def generate_fixed_length_assigns(spec, container_gets_allocator, cpp_name_prefix):
00196 """
00197 Initialize any fixed-length arrays
00198
00199 @param s: The stream to write to
00200 @type s: stream
00201 @param spec: The message spec
00202 @type spec: genmsg.msgs.MsgSpec
00203 @param container_gets_allocator: Whether or not a container type (whether it's another message, a vector, array or string)
00204 should have the allocator passed to its constructor. Assumes the allocator is named _alloc.
00205 @type container_gets_allocator: bool
00206 @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::"
00207 @type cpp_name_prefix: str
00208 """
00209
00210 for field in spec.parsed_fields():
00211 if (not field.is_array or field.array_len is None):
00212 continue
00213
00214 val = default_value(field.base_type)
00215 if (container_gets_allocator and takes_allocator(field.base_type)):
00216
00217 if (field.base_type == "string"):
00218 string_cpp = msg_type_to_cpp("string")
00219 yield ' %s.assign(%s(_alloc));\n'%(field.name, string_cpp)
00220 else:
00221 (cpp_msg_unqualified, cpp_msg_with_alloc, _) = cpp_message_declarations(cpp_name_prefix, field.base_type)
00222 yield ' %s.assign(%s(_alloc));\n'%(field.name, cpp_msg_with_alloc)
00223 elif (len(val) > 0):
00224 yield ' %s.assign(%s);\n'%(field.name, val)
00225
00226
00227 def generate_initializer_list(spec, container_gets_allocator):
00228 """
00229 Writes the initializer list for a constructor
00230
00231 @param s: The stream to write to
00232 @type s: stream
00233 @param spec: The message spec
00234 @type spec: genmsg.msgs.MsgSpec
00235 @param container_gets_allocator: Whether or not a container type (whether it's another message, a vector, array or string)
00236 should have the allocator passed to its constructor. Assumes the allocator is named _alloc.
00237 @type container_gets_allocator: bool
00238 """
00239
00240 op = ':'
00241 for field in spec.parsed_fields():
00242 val = default_value(field.base_type)
00243 use_alloc = takes_allocator(field.base_type)
00244 if (field.is_array):
00245 if (field.array_len is None and container_gets_allocator):
00246 yield ' %s %s(_alloc)'%(op, field.name)
00247 else:
00248 yield ' %s %s()'%(op, field.name)
00249 else:
00250 if (container_gets_allocator and use_alloc):
00251 yield ' %s %s(_alloc)'%(op, field.name)
00252 else:
00253 yield ' %s %s(%s)'%(op, field.name, val)
00254 op = ','