44 import roslib.packages
45 import roslib.gentools
46 from rospkg
import RosPack
49 from cStringIO
import StringIO
51 from io
import StringIO
53 MSG_TYPE_TO_CPP = {
'byte':
'int8_t',
'char':
'uint8_t',
55 'uint8':
'uint8_t',
'int8':
'int8_t',
56 'uint16':
'uint16_t',
'int16':
'int16_t',
57 'uint32':
'uint32_t',
'int32':
'int32_t',
58 'uint64':
'uint64_t',
'int64':
'int64_t',
61 'string':
'std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > ',
63 'duration':
'ros::Duration'}
67 Converts a message type (e.g. uint32, std_msgs/String, etc.) into the C++ declaration
68 for that type (e.g. uint32_t, std_msgs::String_<ContainerAllocator>)
70 @param type: The message type
72 @return: The C++ declaration
75 (base_type, is_array, array_len) = roslib.msgs.parse_type(type)
77 if (roslib.msgs.is_builtin(base_type)):
78 cpp_type = MSG_TYPE_TO_CPP[base_type]
79 elif (len(base_type.split(
'/')) == 1):
80 if (roslib.msgs.is_header_type(base_type)):
81 cpp_type =
' ::std_msgs::Header_<ContainerAllocator> '
83 cpp_type =
'%s_<ContainerAllocator> '%(base_type)
85 pkg = base_type.split(
'/')[0]
86 msg = base_type.split(
'/')[1]
87 cpp_type =
' ::%s::%s_<ContainerAllocator> '%(pkg, msg)
90 if (array_len
is None):
91 return 'std::vector<%s, typename ContainerAllocator::template rebind<%s>::other > '%(cpp_type, cpp_type)
93 return 'boost::array<%s, %s> '%(cpp_type, array_len)
99 Returns the different possible C++ declarations for a message given the message itself.
101 @param name_prefix: The C++ prefix to be prepended to the name, e.g. "std_msgs::"
102 @type name_prefix: str
103 @param msg: The message type
105 @return: A tuple of 3 different names. cpp_message_decelarations("std_msgs::", "String") returns the tuple
106 ("std_msgs::String_", "std_msgs::String_<ContainerAllocator>", "std_msgs::String")
109 pkg, basetype = roslib.names.package_resource_name(msg)
110 cpp_name =
' ::%s%s'%(name_prefix, msg)
112 cpp_name =
' ::%s::%s'%(pkg, basetype)
113 return (
'%s_'%(cpp_name),
'%s_<ContainerAllocator> '%(cpp_name),
'%s'%(cpp_name))
117 Writes the beginning of the header file: a comment saying it's auto-generated and the include guards
119 @param s: The stream to write to
121 @param spec: The spec
122 @type spec: roslib.msgs.MsgSpec
123 @param file: The file this message is being generated for
126 s.write(
"/* Auto-generated by genmsg_cpp for file %s */\n"%(file))
127 s.write(
'#ifndef %s_MESSAGE_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
128 s.write(
'#define %s_MESSAGE_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
132 Writes the end of the header file: the ending of the include guards
134 @param s: The stream to write to
136 @param spec: The spec
137 @type spec: roslib.msgs.MsgSpec
139 s.write(
'#endif // %s_MESSAGE_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
143 Writes the includes that all messages need
145 @param s: The stream to write to
148 s.write(
'#include <string>\n')
149 s.write(
'#include <vector>\n')
150 s.write(
'#include <map>\n')
151 s.write(
'#include <ostream>\n')
152 s.write(
'#include "ros/serialization.h"\n')
153 s.write(
'#include "ros/builtin_message_traits.h"\n')
154 s.write(
'#include "ros/message_operations.h"\n')
155 s.write(
'#include "ros/time.h"\n\n')
156 s.write(
'#include "ros/macros.h"\n\n')
157 s.write(
'#include "ros/assert.h"\n\n')
161 Writes the message-specific includes
163 @param s: The stream to write to
165 @param spec: The message spec to iterate over
166 @type spec: roslib.msgs.MsgSpec
168 for field
in spec.parsed_fields():
169 if (
not field.is_builtin):
170 if (field.is_header):
171 s.write(
'#include "std_msgs/Header.h"\n')
173 (pkg, name) = roslib.names.package_resource_name(field.base_type)
174 pkg = pkg
or spec.package
175 s.write(
'#include "%s/%s.h"\n'%(pkg, name))
180 def write_struct(s, spec, cpp_name_prefix, extra_deprecated_traits = {}):
182 Writes the entire message struct: declaration, constructors, members, constants and (deprecated) member functions
183 @param s: The stream to write to
185 @param spec: The message spec
186 @type spec: roslib.msgs.MsgSpec
187 @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::"
188 @type cpp_name_prefix: str
191 msg = spec.short_name
192 s.write(
'template <class ContainerAllocator>\n')
193 s.write(
'struct %s_ {\n'%(msg))
194 s.write(
' typedef %s_<ContainerAllocator> Type;\n\n'%(msg))
208 s.write(
' typedef boost::shared_ptr<%s> Ptr;\n'%(cpp_msg_with_alloc))
209 s.write(
' typedef boost::shared_ptr<%s const> ConstPtr;\n'%(cpp_msg_with_alloc))
211 s.write(
'}; // struct %s\n'%(msg))
213 s.write(
'typedef %s_<std::allocator<void> > %s;\n\n'%(cpp_msg_base, msg))
214 s.write(
'typedef boost::shared_ptr<%s> %sPtr;\n'%(cpp_msg_base, msg))
215 s.write(
'typedef boost::shared_ptr<%s const> %sConstPtr;\n\n'%(cpp_msg_base, msg))
219 Returns the value to initialize a message member with. 0 for integer types, 0.0 for floating point, false for bool,
220 empty string for everything else
222 @param type: The type
225 if type
in [
'byte',
'int8',
'int16',
'int32',
'int64',
226 'char',
'uint8',
'uint16',
'uint32',
'uint64']:
228 elif type
in [
'float32',
'float64']:
237 Returns whether or not a type can take an allocator in its constructor. False for all builtin types except string.
240 @param type: The type
243 return not type
in [
'byte',
'int8',
'int16',
'int32',
'int64',
244 'char',
'uint8',
'uint16',
'uint32',
'uint64',
245 'float32',
'float64',
'bool',
'time',
'duration']
249 Writes the initializer list for a constructor
251 @param s: The stream to write to
253 @param spec: The message spec
254 @type spec: roslib.msgs.MsgSpec
255 @param container_gets_allocator: Whether or not a container type (whether it's another message, a vector, array or string)
256 should have the allocator passed to its constructor. Assumes the allocator is named _alloc.
257 @type container_gets_allocator: bool
261 for field
in spec.parsed_fields():
270 if (field.array_len
is None and container_gets_allocator):
271 s.write(
'%s(_alloc)\n'%(field.name))
273 s.write(
'%s()\n'%(field.name))
275 if (container_gets_allocator
and use_alloc):
276 s.write(
'%s(_alloc)\n'%(field.name))
278 s.write(
'%s(%s)\n'%(field.name, val))
283 Initialize any fixed-length arrays
285 @param s: The stream to write to
287 @param spec: The message spec
288 @type spec: roslib.msgs.MsgSpec
289 @param container_gets_allocator: Whether or not a container type (whether it's another message, a vector, array or string)
290 should have the allocator passed to its constructor. Assumes the allocator is named _alloc.
291 @type container_gets_allocator: bool
292 @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::"
293 @type cpp_name_prefix: str
296 for field
in spec.parsed_fields():
297 if (
not field.is_array
or field.array_len
is None):
303 if (field.base_type ==
"string"):
305 s.write(
' %s.assign(%s(_alloc));\n'%(field.name, string_cpp))
308 s.write(
' %s.assign(%s(_alloc));\n'%(field.name, cpp_msg_with_alloc))
310 s.write(
' %s.assign(%s);\n'%(field.name, val))
314 Writes any necessary constructors for the message
316 @param s: The stream to write to
318 @param spec: The message spec
319 @type spec: roslib.msgs.MsgSpec
320 @param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::"
321 @type cpp_name_prefix: str
324 msg = spec.short_name
327 s.write(
' %s_()\n'%(msg))
334 s.write(
' %s_(const ContainerAllocator& _alloc)\n'%(msg))
342 Writes a single member's declaration and type typedef
344 @param s: The stream to write to
346 @param type: The member type
348 @param name: The name of the member
352 s.write(
' typedef %s _%s_type;\n'%(cpp_type, field.name))
353 s.write(
' %s %s;\n\n'%(cpp_type, field.name))
357 Write all the member declarations
359 @param s: The stream to write to
361 @param spec: The message spec
362 @type spec: roslib.msgs.MsgSpec
364 [
write_member(s, field)
for field
in spec.parsed_fields()]
367 str = str.replace(
'\\',
'\\\\')
368 str = str.replace(
'"',
'\\"')
373 Write a constant value as a static member
375 @param s: The stream to write to
377 @param constant: The constant
378 @type constant: roslib.msgs.Constant
382 if (constant.type
in [
'byte',
'int8',
'int16',
'int32',
'int64',
'char',
'uint8',
'uint16',
'uint32',
'uint64']):
383 s.write(
' enum { %s = %s };\n'%(constant.name, constant.val))
385 s.write(
' static const %s %s;\n'%(
msg_type_to_cpp(constant.type), constant.name))
389 Write all the constants from a spec as static members
391 @param s: The stream to write to
393 @param spec: The message spec
394 @type spec: roslib.msgs.MsgSpec
401 Write a constant value as a static member
403 @param s: The stream to write to
405 @param constant: The constant
406 @type constant: roslib.msgs.Constant
410 if (constant.type
not in [
'byte',
'int8',
'int16',
'int32',
'int64',
'char',
'uint8',
'uint16',
'uint32',
'uint64',
'string']):
411 s.write(
'template<typename ContainerAllocator> const %s %s_<ContainerAllocator>::%s = %s;\n'%(
msg_type_to_cpp(constant.type), spec.short_name, constant.name, constant.val))
412 elif (constant.type ==
'string'):
413 s.write(
'template<typename ContainerAllocator> const %s %s_<ContainerAllocator>::%s = "%s";\n'%(
msg_type_to_cpp(constant.type), spec.short_name, constant.name,
escape_string(constant.val)))
417 Write all the constants from a spec as static members
419 @param s: The stream to write to
421 @param spec: The message spec
422 @type spec: roslib.msgs.MsgSpec
429 Returns whether or not the message is fixed-length
431 @param spec: The message spec
432 @type spec: roslib.msgs.MsgSpec
433 @param package: The package of the
437 for field
in spec.parsed_fields():
438 if (field.is_array
and field.array_len
is None):
441 if (field.base_type ==
'string'):
444 if (
not field.is_builtin):
445 types.append(field.base_type)
449 type = roslib.msgs.resolve_type(type, spec.package)
450 (_, new_spec) = roslib.msgs.load_by_type(type, spec.package)
458 Writes the deprecated member functions for backwards compatibility
460 for field
in spec.parsed_fields():
462 s.write(
' ROS_DEPRECATED uint32_t get_%s_size() const { return (uint32_t)%s.size(); }\n'%(field.name, field.name))
464 if (field.array_len
is None):
465 s.write(
' ROS_DEPRECATED void set_%s_size(uint32_t size) { %s.resize((size_t)size); }\n'%(field.name, field.name))
466 s.write(
' ROS_DEPRECATED void get_%s_vec(%s& vec) const { vec = this->%s; }\n'%(field.name,
msg_type_to_cpp(field.type), field.name))
467 s.write(
' ROS_DEPRECATED void set_%s_vec(const %s& vec) { this->%s = vec; }\n'%(field.name,
msg_type_to_cpp(field.type), field.name))
469 for k, v
in traits.items():
470 s.write(
'private:\n')
471 s.write(
' static const char* __s_get%s_() { return "%s"; }\n'%(k, v))
473 s.write(
' ROS_DEPRECATED static const std::string __s_get%s() { return __s_get%s_(); }\n\n'%(k, k))
474 s.write(
' ROS_DEPRECATED const std::string __get%s() const { return __s_get%s_(); }\n\n'%(k, k))
476 s.write(
' ROS_DEPRECATED virtual uint8_t *serialize(uint8_t *write_ptr, uint32_t seq) const\n {\n')
477 s.write(
' ros::serialization::OStream stream(write_ptr, 1000000000);\n')
478 for field
in spec.parsed_fields():
479 s.write(
' ros::serialization::serialize(stream, %s);\n'%(field.name))
480 s.write(
' return stream.getData();\n }\n\n')
482 s.write(
' ROS_DEPRECATED virtual uint8_t *deserialize(uint8_t *read_ptr)\n {\n')
483 s.write(
' ros::serialization::IStream stream(read_ptr, 1000000000);\n');
484 for field
in spec.parsed_fields():
485 s.write(
' ros::serialization::deserialize(stream, %s);\n'%(field.name))
486 s.write(
' return stream.getData();\n }\n\n')
488 s.write(
' ROS_DEPRECATED virtual uint32_t serializationLength() const\n {\n')
489 s.write(
' uint32_t size = 0;\n');
490 for field
in spec.parsed_fields():
491 s.write(
' size += ros::serialization::serializationLength(%s);\n'%(field.name))
492 s.write(
' return size;\n }\n\n')
496 Same as roslib.gentools.compute_full_text, except that the
497 resulting text is escaped to be safe for C++ double quotes
499 @param get_deps_dict: dictionary returned by get_dependencies call
500 @type get_deps_dict: dict
501 @return: concatenated text for msg/srv file and embedded msg/srv types. Text will be escaped for double quotes
504 definition = roslib.gentools.compute_full_text(gen_deps_dict)
505 lines = definition.split(
'\n')
509 s.write(
'%s\\n\\\n'%(line))
517 if c
not in '0123456789abcdefABCDEF':
524 Writes a class trait for traits which have static value() members that return const char*
526 e.g. write_trait_char_class(s, "MD5Sum", "std_msgs::String_<ContainerAllocator>", "hello") yields:
527 template<class ContainerAllocator>
528 struct MD5Sum<std_msgs::String_<ContainerAllocator> >
530 static const char* value() { return "hello"; }
531 static const char* value(const std_msgs::String_<ContainerAllocator>&) { return value(); }
534 @param s: The stream to write to
536 @param class_name: The name of the trait class to write
537 @type class_name: str
538 @param cpp_msg_with_alloc: The C++ declaration of the message, including the allocator template
539 @type cpp_msg_with_alloc: str
540 @param value: The value to return in the string
542 @param write_static_hex_value: Whether or not to write a set of compile-time-checkable static values. Useful for,
543 for example, MD5Sum. Writes static const uint64_t static_value1... static_valueN
544 @type write_static_hex_value: bool
545 @raise ValueError if write_static_hex_value is True but value contains characters invalid in a hex value
547 s.write(
'template<class ContainerAllocator>\nstruct %s<%s> {\n'%(class_name, cpp_msg_with_alloc))
548 s.write(
' static const char* value() \n {\n return "%s";\n }\n\n'%(value))
549 s.write(
' static const char* value(const %s&) { return value(); } \n'%(cpp_msg_with_alloc))
550 if (write_static_hex_value):
552 raise ValueError(
'%s is not a hex value'%(value))
554 iter_count = len(value) / 16
555 for i
in range(0, int(iter_count)):
557 s.write(
' static const uint64_t static_value%s = 0x%sULL;\n'%((i+1), value[start:start+16]))
562 Writes a true/false trait class
564 @param s: stream to write to
566 @param class_name: Name of the trait class
567 @type class_name: str
568 @param cpp_msg_with_alloc: The C++ declaration of the message, including the allocator template
569 @type cpp_msg_with_alloc: str
571 s.write(
'template<class ContainerAllocator> struct %s<%s> : public TrueType {};\n'%(class_name, cpp_msg_with_alloc))
573 def write_traits(s, spec, cpp_name_prefix, datatype = None, rospack=None):
575 Writes all the traits classes for a message
577 @param s: The stream to write to
579 @param spec: The message spec
580 @type spec: roslib.msgs.MsgSpec
581 @param cpp_name_prefix: The C++ prefix to prepend to a message to refer to it (e.g. "std_msgs::")
582 @type cpp_name_prefix: str
583 @param datatype: The string to write as the datatype of the message. If None (default), pkg/msg is used.
587 gendeps_dict = roslib.gentools.get_dependencies(spec, spec.package, compute_files=
False, rospack=rospack)
588 md5sum = roslib.gentools.compute_md5(gendeps_dict, rospack=rospack)
591 if (datatype
is None):
592 datatype =
'%s'%(spec.full_name)
595 s.write(
'namespace ros\n{\n')
596 s.write(
'namespace message_traits\n{\n')
605 if (spec.has_header()):
612 s.write(
'} // namespace message_traits\n')
613 s.write(
'} // namespace ros\n\n')
617 s.write(
'namespace ros\n{\n')
618 s.write(
'namespace message_operations\n{\n')
621 s.write(
'\ntemplate<class ContainerAllocator>\nstruct Printer<%s>\n{\n'%(cpp_msg_with_alloc))
622 s.write(
' template<typename Stream> static void stream(Stream& s, const std::string& indent, const %s& v) \n {\n'%cpp_msg_with_alloc)
623 for field
in spec.parsed_fields():
626 s.write(
' s << indent << "%s[]" << std::endl;\n'%(field.name))
627 s.write(
' for (size_t i = 0; i < v.%s.size(); ++i)\n {\n'%(field.name))
628 s.write(
' s << indent << " %s[" << i << "]: ";\n'%field.name)
629 indent_increment =
' '
630 if (
not field.is_builtin):
631 s.write(
' s << std::endl;\n')
632 s.write(
' s << indent;\n')
633 indent_increment =
' ';
634 s.write(
' Printer<%s>::stream(s, indent + "%s", v.%s[i]);\n'%(cpp_type, indent_increment, field.name))
637 s.write(
' s << indent << "%s: ";\n'%field.name)
638 indent_increment =
' '
639 if (
not field.is_builtin
or field.is_array):
640 s.write(
's << std::endl;\n')
641 s.write(
' Printer<%s>::stream(s, indent + "%s", v.%s);\n'%(cpp_type, indent_increment, field.name))
647 s.write(
'} // namespace message_operations\n')
648 s.write(
'} // namespace ros\n\n')
652 Writes the Serializer class for a message
654 @param s: Stream to write to
656 @param spec: The message spec
657 @type spec: roslib.msgs.MsgSpec
658 @param cpp_name_prefix: The C++ prefix to prepend to a message to refer to it (e.g. "std_msgs::")
659 @type cpp_name_prefix: str
663 s.write(
'namespace ros\n{\n')
664 s.write(
'namespace serialization\n{\n\n')
666 s.write(
'template<class ContainerAllocator> struct Serializer<%s>\n{\n'%(cpp_msg_with_alloc))
668 s.write(
' template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)\n {\n')
669 for field
in spec.parsed_fields():
670 s.write(
' stream.next(m.%s);\n'%(field.name))
673 s.write(
' ROS_DECLARE_ALLINONE_SERIALIZER\n')
675 s.write(
'}; // struct %s_\n'%(spec.short_name))
677 s.write(
'} // namespace serialization\n')
678 s.write(
'} // namespace ros\n\n')
682 s.write(
'template<typename ContainerAllocator>\nstd::ostream& operator<<(std::ostream& s, const %s& v)\n{\n'%(cpp_msg_with_alloc))
683 s.write(
' ros::message_operations::Printer<%s>::stream(s, "", v);\n return s;}\n\n'%(cpp_msg_with_alloc))
689 @param msg_path: The path to the .msg file
692 (package_dir, package) = roslib.packages.get_dir_pkg(msg_path)
693 (_, spec) = roslib.msgs.load_from_file(msg_path, package)
700 cpp_prefix =
'%s::'%(package)
702 s.write(
'namespace %s\n{\n'%(package))
706 s.write(
'} // namespace %s\n\n'%(package))
716 if (package ==
"std_msgs" and spec.short_name ==
"Header"):
717 s.write(
"#define STD_MSGS_INCLUDING_HEADER_DEPRECATED_DEF 1\n")
718 s.write(
"#include <std_msgs/header_deprecated_def.h>\n")
719 s.write(
"#undef STD_MSGS_INCLUDING_HEADER_DEPRECATED_DEF\n\n")
723 output_dir =
'%s/msg_gen/cpp/include/%s'%(package_dir, package)
724 if (
not os.path.exists(output_dir)):
728 os.makedirs(output_dir)
732 f = open(
'%s/%s.h'%(output_dir, spec.short_name),
'w')
733 f.write(s.getvalue() +
"\n")
741 if __name__ ==
"__main__":
742 roslib.msgs.set_verbose(
False)