00001
00002 import sys
00003
00004 import roslib
00005 roslib.load_manifest("rtt_ros_integration");
00006
00007 import roscpp
00008 import roscpp.msg_gen
00009 from roslib import packages,msgs
00010 import os
00011
00012 from cStringIO import StringIO
00013
00014 NAME='create_boost_headers'
00015
00016 def write_boost_includes(s, spec):
00017 """
00018 Writes the message-specific includes
00019
00020 @param s: The stream to write to
00021 @type s: stream
00022 @param spec: The message spec to iterate over
00023 @type spec: roslib.msgs.MsgSpec
00024 @param serializer: The serializer type for which to include headers
00025 @type serializer: str
00026 """
00027 for field in spec.parsed_fields():
00028 if (not field.is_builtin):
00029 if (field.is_header):
00030 s.write('#include <ros/common.h>\n')
00031 s.write('#if ROS_VERSION_MINIMUM(1,4,0)\n')
00032 s.write('#include <std_msgs/Header.h>\n')
00033 s.write('#else\n')
00034 s.write('#include <roslib/Header.h>\n')
00035 s.write('#endif\n')
00036 else:
00037 (pkg, name) = roslib.names.package_resource_name(field.base_type)
00038 pkg = pkg or spec.package
00039 s.write('#include "%s/boost/%s.h"\n'%(pkg, name))
00040
00041 s.write('\n')
00042
00043
00044 def write_boost_serialization(s, spec, cpp_name_prefix, file):
00045 """
00046 Writes the boost::serialize function for a message
00047
00048 @param s: Stream to write to
00049 @type s: stream
00050 @param spec: The message spec
00051 @type spec: roslib.msgs.MsgSpec
00052 @param cpp_name_prefix: The C++ prefix to prepend to a message to refer to it (e.g. "std_msgs::")
00053 @type cpp_name_prefix: str
00054 """
00055 (cpp_msg_unqualified, cpp_msg_with_alloc, _) = roscpp.msg_gen.cpp_message_declarations(cpp_name_prefix, spec.short_name)
00056
00057 s.write("/* Auto-generated by genmsg_cpp for file %s */\n"%(file))
00058 s.write('#ifndef %s_BOOST_SERIALIZATION_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
00059 s.write('#define %s_BOOST_SERIALIZATION_%s_H\n\n'%(spec.package.upper(), spec.short_name.upper()))
00060 s.write('#include <boost/serialization/serialization.hpp>\n')
00061 s.write('#include <boost/serialization/nvp.hpp>\n')
00062 s.write('#include <%s/%s.h>\n'%(spec.package,spec.short_name))
00063 write_boost_includes(s, spec)
00064 s.write('namespace boost\n{\n')
00065 s.write('namespace serialization\n{\n\n')
00066
00067 s.write('template<class Archive, class ContainerAllocator>\n')
00068
00069 s.write('void serialize(Archive& a, %s & m, unsigned int)\n{\n'%(cpp_msg_with_alloc))
00070
00071 for field in spec.parsed_fields():
00072 s.write(' a & make_nvp("%s",m.%s);\n'%(field.name,field.name))
00073 s.write('}\n\n')
00074
00075 s.write('} // namespace serialization\n')
00076 s.write('} // namespace boost\n\n')
00077 s.write('#endif // %s_BOOST_SERIALIZATION_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
00078
00079
00080
00081 def generate_boost_serialization(msg_path):
00082 """
00083 Generate a boost::serialization header
00084
00085 @param msg_path: The path to the .msg file
00086 @type msg_path: str
00087 """
00088 (package_dir, package) = roslib.packages.get_dir_pkg(msg_path)
00089 (_, spec) = roslib.msgs.load_from_file(msg_path, package)
00090 cpp_prefix = '%s::'%(package)
00091
00092 s = StringIO()
00093 write_boost_serialization(s, spec, cpp_prefix, msg_path)
00094
00095 output_dir = 'include/%s/boost'%(package)
00096 try:
00097 os.makedirs(output_dir)
00098 except OSError, e:
00099 pass
00100
00101 f = open('%s/%s.h'%(output_dir, spec.short_name), 'w')
00102 print >> f, s.getvalue()
00103
00104 s.close()
00105
00106
00107 def create_boost_headers(argv, stdout, stderr):
00108 from optparse import OptionParser
00109 parser = OptionParser(usage="usage: %prog [packages]", prog=NAME)
00110 (options, args) = parser.parse_args(argv)
00111
00112
00113 if len(args) < 2:
00114 parser.error("you must specify at least package")
00115 pkgs = args[1:]
00116 for pkg in pkgs:
00117 pp = roslib.packages.get_pkg_dir(pkg);
00118 msgs = roslib.msgs.list_msg_types(pkg,False)
00119 for msg in msgs:
00120 generate_boost_serialization(pp+'/msg/'+msg+'.msg')
00121
00122 if __name__ == "__main__":
00123 try:
00124 create_boost_headers(sys.argv, sys.stdout, sys.stderr)
00125 except Exception, e:
00126 print >> sys.stderr, e
00127 sys.exit(1)