generate.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2014, JSK Robotics Laboratory.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of JSK Robotics Laboratory. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 
00034 ## ROS message source code generation for EusLisp(roseus)
00035 ##
00036 ## Converts ROS .msg and .srv files in a package into EUsLisp source code
00037 
00038 import sys
00039 import os
00040 import traceback
00041 import re
00042 
00043 from genmsg import SrvSpec, MsgSpec, MsgContext
00044 from genmsg.msg_loader import load_srv_from_file, load_msg_by_type
00045 import genmsg.gentools
00046 
00047 try:
00048     from cStringIO import StringIO #Python 2.x
00049 except ImportError:
00050     from io import StringIO #Python 3.x
00051 
00052 ############################################################
00053 # Built in types
00054 ############################################################
00055 
00056 def is_integer(t):
00057     return t in ['byte', 'char', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64'] #t char/byte  deprecated alias for uint8/int8, see http://wiki.ros.org/msg#Fields
00058 
00059 def is_signed_int(t):
00060     return t in ['byte', 'int8', 'int16', 'int32', 'int64']
00061 
00062 def is_unsigned_int(t):
00063     return t in ['char', 'uint8', 'uint16', 'uint32', 'uint64']
00064 
00065 def is_bool(t):
00066     return t == 'bool'
00067 
00068 def is_string(t):
00069     return t == 'string'
00070 
00071 def is_float(t):
00072     return t in ['float16', 'float32', 'float64']
00073 
00074 def is_time(t):
00075     return t in ['time', 'duration']
00076 
00077 def field_type(f):
00078     if f.is_builtin:
00079         elt_type = lisp_type(f.base_type, f.is_array)
00080     else:
00081         elt_type = msg_type(f)
00082     return elt_type
00083 
00084 def parse_msg_type(f):
00085     "returns (package, msg_or_srv)"
00086     if f.base_type == 'Header':
00087         return ('std_msgs', 'Header')
00088     else:
00089         return f.base_type.split('/')
00090 
00091 def msg_type(f):
00092     "returns roseus namespace package::msg_or_srv"
00093     (pkg, msg) = parse_msg_type(f)
00094     return '%s::%s'%(pkg, msg)
00095 
00096 def lisp_type(t, array):
00097     if t == 'uint8' and array:
00098         return 'char'
00099     if is_integer(t):
00100         return 'integer'
00101     elif is_bool(t):
00102         return 'object'
00103     elif is_float(t):
00104         return 'float'
00105     elif is_time(t):
00106         return 'ros::time'
00107     elif is_string(t):
00108         return 'string'
00109     else:
00110         raise ValueError('%s is not a recognized primitive type'%t)
00111 
00112 def field_initform(f):
00113     var = "__%s"%f.name
00114     if f.is_builtin and not f.is_array:
00115         if is_integer(f.base_type):
00116             return '(round %s)'%var
00117         elif is_float(f.base_type):
00118             return '(float %s)'%var
00119         elif is_string(f.base_type):
00120             return '(string %s)'%var
00121     return var
00122 
00123 def field_initvalue(f):
00124     initvalue = lisp_initvalue(f.base_type)
00125     elt_type = lisp_type(f.base_type, f.is_array)
00126     if not is_time(f.base_type):
00127         elt_type = ':'+elt_type
00128     if f.is_array:
00129         len = f.array_len or 0
00130         if f.is_builtin and not is_string(f.base_type) and not is_bool(f.base_type) and not is_time(f.base_type):
00131             return '(make-array %s :initial-element %s :element-type %s)'%(len, initvalue, elt_type)
00132         else:
00133             return '(let (r) (dotimes (i %s) (push %s r)) r)'%(len, initvalue)
00134     else:
00135         return initvalue
00136 
00137 def lisp_initvalue(t):
00138     if is_integer(t):
00139         return '0'
00140     elif is_bool(t):
00141         return 'nil'
00142     elif is_float(t):
00143         return '0.0'
00144     elif is_time(t):
00145         return '(instance ros::time :init)'
00146     elif is_string(t):
00147         return '\"\"'
00148     else:
00149         raise ValueError('%s is not a recognized primitive type'%t)
00150 
00151 def lisp_initform(t):
00152     if is_integer(t):
00153         return 'round'
00154     elif is_bool(t):
00155         return 'nil'
00156     elif is_float(t):
00157         return 'float'
00158     elif is_time(t):
00159         return 'ros::time'
00160     elif is_string(t):
00161         return 'string'
00162     else:
00163         raise ValueError('%s is not a recognized primitive type'%t)
00164 
00165 NUM_BYTES = {'byte': 1, 'int8': 1, 'int16': 2, 'int32': 4, 'int64': 8,
00166              'char': 1, 'uint8': 1, 'uint16': 2, 'uint32': 4, 'uint64': 8}
00167              
00168              
00169 
00170 ############################################################
00171 # Indented writer
00172 ############################################################
00173 
00174 class IndentedWriter():
00175 
00176     def __init__(self, s):
00177         self.str = s
00178         self.indentation = 0
00179         self.block_indent = False
00180 
00181     def write(self, s, indent=True, newline=True):
00182         if not indent:
00183             newline = False
00184         if self.block_indent:
00185             self.block_indent = False
00186         else:
00187             if newline:
00188                 self.str.write('\n')
00189             if indent:
00190                 for i in range(self.indentation):
00191                     self.str.write(' ')
00192         self.str.write(s)
00193 
00194     def newline(self):
00195         self.str.write('\n')
00196 
00197     def inc_indent(self, inc=2):
00198         self.indentation += inc
00199 
00200     def dec_indent(self, dec=2):
00201         self.indentation -= dec
00202 
00203     def reset_indent(self):
00204         self.indentation = 0
00205 
00206     def block_next_indent(self):
00207         self.block_indent = True
00208 
00209 class Indent():
00210 
00211     def __init__(self, w, inc=2, indent_first=True):
00212         self.writer = w
00213         self.inc = inc
00214         self.indent_first = indent_first
00215 
00216     def __enter__(self):
00217         self.writer.inc_indent(self.inc)
00218         if not self.indent_first:
00219             self.writer.block_next_indent()
00220 
00221     def __exit__(self, type, val, traceback):
00222         self.writer.dec_indent(self.inc)
00223 
00224 def write_begin(s, spec, is_service=False):
00225     "Writes the beginning of the file: a comment saying it's auto-generated and the in-package form"
00226 
00227     s.write(';; Auto-generated. Do not edit!\n\n', newline=False)
00228     suffix = 'srv' if is_service else 'msg'
00229     if is_service:
00230         spec.actual_name=spec.short_name
00231     s.write('(when (boundp \'%s::%s)'%(spec.package, spec.actual_name))
00232     s.write('  (if (not (find-package "%s"))'%(spec.package.upper()))
00233     s.write('    (make-package "%s"))'%(spec.package.upper()))
00234     s.write('  (shadow \'%s (find-package "%s")))'%(spec.actual_name, spec.package.upper()))
00235     s.write('(unless (find-package "%s::%s")'%(spec.package.upper(), spec.actual_name.upper()))
00236     s.write('  (make-package "%s::%s"))'%(spec.package.upper(), spec.actual_name.upper()))
00237     if is_service:
00238         s.write('(unless (find-package "%s::%sREQUEST")'%(spec.package.upper(), spec.actual_name.upper()))
00239         s.write('  (make-package "%s::%sREQUEST"))'%(spec.package.upper(), spec.actual_name.upper()))
00240         s.write('(unless (find-package "%s::%sRESPONSE")'%(spec.package.upper(), spec.actual_name.upper()))
00241         s.write('  (make-package "%s::%sRESPONSE"))'%(spec.package.upper(), spec.actual_name.upper()))
00242     s.write('')
00243     s.write('(in-package "ROS")')
00244     s.newline()
00245 
00246 def write_include(s, spec, is_srv=False):
00247     if not is_srv:
00248         s.write(';;//! \\htmlinclude %s.msg.html'%spec.actual_name, newline=False) # t2
00249     for msg_type in sorted(set([parse_msg_type(field)[0] for field in spec.parsed_fields() if not field.is_builtin and parse_msg_type(field)[0] != spec.package])):
00250         s.write('(if (not (find-package "%s"))'%msg_type.upper())
00251         s.write('  (ros::roseus-add-msgs "%s"))'%msg_type)
00252     s.newline()
00253     s.newline()
00254 
00255 def write_slot_definition(s, field):
00256     "Write the definition of a slot corresponding to a single message field"
00257     s.write('_%s '%field.name, indent=False, newline=False)
00258 
00259 
00260 def write_slot_argument(s, field):
00261     "Write the key arguments of a slot corresponding to a single message field"
00262     var = field.name
00263     if field.is_builtin:
00264         s.write('((:%s __%s) %s)'%(var, var, field_initvalue(field)))
00265     else:
00266         if field.is_array:
00267             len = field.array_len or 0
00268             s.write('((:%s __%s) (let (r) (dotimes (i %s) (push (instance %s :init) r)) r))'%(var, var, len, field_type(field))) ## FIX??? need to use len = f.array_len or 0
00269         else:
00270             s.write('((:%s __%s) (instance %s :init))'%(var, var, field_type(field)))
00271 
00272 def write_slot_initialize(s, field):
00273     "Write the initialization of a slot corresponding to a single message field"
00274     s.write('(setq _%s %s)'%(field.name, field_initform(field)))
00275 
00276 def write_defclass(s, spec):
00277     "Writes the defclass that defines the message type"
00278     s.write('(defclass %s::%s'%(spec.package, spec.actual_name))
00279     with Indent(s):
00280         s.write(':super ros::object')
00281         s.write(':slots (')
00282         with Indent(s, inc=1, indent_first=False):
00283             for field in spec.parsed_fields():
00284                 write_slot_definition(s, field)
00285         s.write('))', indent=False)
00286     s.newline()
00287 
00288 def write_defmethod(s, spec):
00289     s.write('(defmethod %s::%s'%(spec.package, spec.actual_name))
00290     with Indent(s):
00291         s.write('(:init')
00292         with Indent(s, inc=1):
00293             s.write('(&key')
00294             with Indent(s, inc=1):
00295                 for field in spec.parsed_fields():
00296                     write_slot_argument(s, field)
00297                 s.write(')')
00298             s.write('(send-super :init)')
00299             for field in spec.parsed_fields():
00300                 write_slot_initialize(s, field)
00301             s.write('self)')
00302 
00303 
00304 def write_accessors(s, spec):
00305     with Indent(s):
00306         for field in spec.parsed_fields():
00307             s.write('(:%s'%field.name)
00308             var = '_%s'%field.name
00309             with Indent(s, inc=1):
00310                 if field.is_builtin:
00311                     s.write('(&optional _%s)'%var)
00312                     s.write('(if _%s (setq %s _%s)) %s)'%(var,var,var,var))
00313                 else:
00314                     s.write('(&rest _%s)'%var)
00315                     s.write('(if (keywordp (car _%s))'%var)
00316                     s.write('    (send* %s _%s)'%(var,var))
00317                     with Indent(s, inc=2):
00318                         s.write('(progn')
00319                         s.write('  (if _%s (setq %s (car _%s)))'%(var,var,var))
00320                         s.write('  %s)))'%var)
00321 
00322 def write_serialize_length(s, v, is_array=False):
00323     if is_array:
00324         s.write('(write-long (length %s) s)'%(v))
00325     else:
00326         s.write('(write-long (length %s) s) (princ %s s)'%(v,v))
00327 
00328 
00329 def write_serialize_bits(s, v, num_bytes): 
00330     if num_bytes == 1:
00331         s.write('(write-byte %s s)'%v)
00332     elif num_bytes == 2:
00333         s.write('(write-word %s s)'%v)
00334     elif num_bytes == 4:
00335         s.write('(write-long %s s)'%v)
00336     else:
00337         s.write('\n', indent=False)
00338         s.write('#+(or :alpha :irix6 :x86_64)', indent=False, newline=False)
00339         s.write('(progn (sys::poke %s (send s :buffer) (send s :count) :long) (incf (stream-count s) 8))'%v)
00340         s.write('\n', indent=False)
00341         s.write('#-(or :alpha :irix6 :x86_64)', indent=False)
00342         s.write('(cond ((and (class %s) (= (length (%s . bv)) 2)) ;; bignum'%(v,v))
00343         s.write('       (write-long (ash (elt (%s . bv) 0) 0) s)'%v)
00344         s.write('       (write-long (ash (elt (%s . bv) 1) -1) s))'%v)
00345         s.write('      ((and (class %s) (= (length (%s . bv)) 1)) ;; big1'%(v,v))
00346         s.write('       (write-long (elt (%s . bv) 0) s)'%v)
00347         s.write('       (write-long (if (>= %s 0) 0 #xffffffff) s))'%v)
00348         s.write('      (t                                         ;; integer')
00349         s.write('       (write-long %s s)(write-long (if (>= %s 0) 0 #xffffffff) s)))'%(v,v))
00350 
00351 
00352 def write_serialize_bits_signed(s, v, num_bytes):
00353     write_serialize_bits(s, v, num_bytes)
00354 
00355 def write_serialize_builtin(s, f, v):
00356     if f.base_type == 'string':
00357         write_serialize_length(s, v)
00358     elif f.base_type == 'float32':
00359         s.write('(sys::poke %s (send s :buffer) (send s :count) :float) (incf (stream-count s) 4)'%v)
00360     elif f.base_type == 'float64':
00361         s.write('(sys::poke %s (send s :buffer) (send s :count) :double) (incf (stream-count s) 8)'%v)
00362     elif f.base_type == 'bool':
00363         s.write('(if %s (write-byte -1 s) (write-byte 0 s))'%v)
00364     elif f.base_type in ['byte', 'char']:
00365         s.write('(write-byte %s s)'%v)
00366     elif f.base_type in ['duration', 'time']:
00367         s.write('(write-long (send %s :sec) s) (write-long (send %s :nsec) s)'%(v,v))
00368     elif is_signed_int(f.base_type):
00369         write_serialize_bits_signed(s, v, NUM_BYTES[f.base_type])
00370     elif is_unsigned_int(f.base_type):
00371         write_serialize_bits(s, v, NUM_BYTES[f.base_type])
00372     else:
00373         raise ValueError('Unknown type: %s', f.base_type)
00374 
00375 def write_serialize_field(s, f):
00376     s.write(';; %s _%s'%(f.type, f.name))
00377     slot = '_%s'%f.name
00378     var = slot
00379     if f.is_array and f.base_type == 'uint8':
00380         if not f.array_len:
00381             s.write('(write-long (length %s) s)'%slot)
00382         s.write('(princ %s s)'%slot)
00383     elif f.is_array and is_string(f.base_type):
00384         s.write('(write-long (length %s) s)'%slot)
00385         s.write('(dolist (elem %s)'%slot)
00386         var = 'elem'
00387     elif f.is_array:
00388         if not f.array_len:
00389             write_serialize_length(s, slot, True)
00390         if f.is_builtin and f.array_len:
00391             s.write('(dotimes (i %s)'%f.array_len)
00392         elif f.is_builtin and not f.array_len:
00393             s.write('(dotimes (i (length %s))'%var)
00394         else:
00395             s.write('(dolist (elem %s)'%slot)
00396         slot = 'elem'
00397         var = '(elt %s i)'%var
00398         s.block_next_indent()
00399         s.write('')
00400 
00401     if f.is_array and f.base_type == 'uint8':
00402         pass
00403     elif f.is_builtin:
00404         with Indent(s):
00405             write_serialize_builtin(s, f, var)
00406     else:
00407         with Indent(s):
00408             s.write('(send %s :serialize s)'%slot)
00409 
00410     if f.is_array and f.base_type != 'uint8':
00411         s.write('  )')
00412  
00413 def write_serialize(s, spec):
00414     """
00415     Write the serialize method
00416     """
00417     with Indent(s):
00418         s.write('(:serialize')
00419         with Indent(s,inc=1):
00420             s.write('(&optional strm)')
00421             s.write('(let ((s (if strm strm')
00422             s.write('           (make-string-output-stream (send self :serialization-length)))))')
00423             with Indent(s):
00424                 for f in spec.parsed_fields():
00425                     write_serialize_field(s, f)
00426                 s.write(';;')
00427                 s.write('(if (null strm) (get-output-stream-string s))))')
00428 
00429 def write_deserialize_length(s, f, v, is_array=False):
00430     if is_array:
00431         s.write('(let (n)') ## TODO should not be here
00432         with Indent(s):
00433             s.write('(setq n (sys::peek buf ptr- :integer)) (incf ptr- 4)')
00434             s.write('(setq %s (let (r) (dotimes (i n) (push (instance %s :init) r)) r))'%(v,field_type(f)))
00435     else:
00436         set = 'setf' if v[0] == '(' else 'setq'
00437         s.write('(let (n) (setq n (sys::peek buf ptr- :integer)) (incf ptr- 4) (%s %s (subseq buf ptr- (+ ptr- n))) (incf ptr- n))'%(set, v))
00438 
00439 def write_deserialize_bits(s, v, num_bytes):
00440     if num_bytes == 1:
00441         type = ':char'
00442     elif num_bytes == 2:
00443         type = ':short'
00444     elif num_bytes == 4:
00445         type = ':integer'
00446     elif num_bytes == 8:
00447         type = ':long'
00448         s.write('')
00449         return write_deserialize_bits_signed(s,v,num_bytes)
00450     else:
00451         raise ValueError('Unknown size: %s', num_bytes)
00452 
00453     set = 'setf' if v[0] == '(' else 'setq'
00454     s.write('(%s %s (sys::peek buf ptr- %s)) (incf ptr- %s)'%(set,v,type,num_bytes))
00455 
00456 def write_deserialize_bits_signed(s, v, num_bytes):
00457     if num_bytes in [1,2,4]:
00458         write_deserialize_bits(s, v, num_bytes)
00459     else:
00460         s.write('\n', indent=False)
00461         s.write('#+(or :alpha :irix6 :x86_64)', indent=False)
00462         s.write(' (setf %s (prog1 (sys::peek buf ptr- :long) (incf ptr- 8)))\n'%v)
00463         s.write('#-(or :alpha :irix6 :x86_64)', indent=False)
00464         s.write(' (setf %s (let ((b0 (prog1 (sys::peek buf ptr- :integer) (incf ptr- 4)))'%v)
00465         s.write('             (b1 (prog1 (sys::peek buf ptr- :integer) (incf ptr- 4))))')
00466         s.write('         (cond ((= b1 -1) b0)')
00467         s.write('                ((and (= b1  0)')
00468         s.write('                      (<= lisp::most-negative-fixnum b0 lisp::most-positive-fixnum))')
00469         s.write('                 b0)')
00470         s.write('               ((= b1  0) (make-instance bignum :size 1 :bv (integer-vector b0)))')
00471         s.write('               (t (make-instance bignum :size 2 :bv (integer-vector b0 (ash b1 1)))))))')
00472 
00473 def write_deserialize_builtin(s, f, v):
00474     set = 'setf' if v[0] == '(' else 'setq'
00475     if f.base_type == 'string':
00476         write_deserialize_length(s,f,v)
00477     elif f.base_type == 'float32':
00478         s.write('(%s %s (sys::peek buf ptr- :float)) (incf ptr- 4)'%(set, v))
00479     elif f.base_type == 'float64':
00480         s.write('(%s %s (sys::peek buf ptr- :double)) (incf ptr- 8)'%(set, v))
00481     elif f.base_type == 'bool':
00482         s.write('(%s %s (not (= 0 (sys::peek buf ptr- :char)))) (incf ptr- 1)'%(set, v))
00483     elif f.base_type in ['duration', 'time']:
00484         s.write('(send %s :sec (sys::peek buf ptr- :integer)) (incf ptr- 4)  (send %s :nsec (sys::peek buf ptr- :integer)) (incf ptr- 4)'%(v,v))
00485     elif is_signed_int(f.base_type):
00486         write_deserialize_bits_signed(s, v, NUM_BYTES[f.base_type])
00487         if NUM_BYTES[f.base_type] == 1: # if signed byte, we have to convert to -128-127
00488             s.write('(if (> %s 127) (%s %s (- %s 256)))'%(v,set,v,v))
00489     elif is_unsigned_int(f.base_type):
00490         write_deserialize_bits(s, v, NUM_BYTES[f.base_type])
00491     else:
00492         raise ValueError('%s unknown'%f.base_type)
00493 
00494 def write_deserialize_field(s, f, pkg):
00495     var = '_%s'%f.name
00496     s.write(';; %s %s'%(f.type, var))
00497     if f.is_array:
00498         if f.is_builtin:
00499             if f.base_type == 'uint8':
00500                 if f.array_len:
00501                     s.write('(setq %s (make-array %d :element-type :char))'%(var,f.array_len))
00502                     s.write('(replace %s buf :start2 ptr-) (incf ptr- %d)'%(var,f.array_len))
00503                 else:
00504                     s.write('(let ((n (sys::peek buf ptr- :integer))) (incf ptr- 4)')
00505                     s.write('  (setq %s (make-array n :element-type :char))'%var)
00506                     s.write('  (replace %s buf :start2 ptr-) (incf ptr- n))'%(var))
00507             elif f.array_len:
00508                 s.write('(dotimes (i (length %s))'%var)
00509                 var = '(elt %s i)'%var
00510             else:
00511                 if is_float(f.base_type) or is_integer(f.base_type) or is_string(f.base_type) or is_bool(f.base_type):
00512                     s.write('(let (n)')
00513                     with Indent(s):
00514                         s.write('(setq n (sys::peek buf ptr- :integer)) (incf ptr- 4)')
00515                         if is_string(f.base_type) or is_bool(f.base_type):
00516                             s.write('(setq %s (make-list n))'%var)
00517                         else:
00518                             s.write('(setq %s (instantiate %s-vector n))'%(var, lisp_type(f.base_type, f.is_array)))
00519                         s.write('(dotimes (i n)')
00520                         var = '(elt %s i)'%var
00521                 else:
00522                     write_deserialize_length(s, f, var, True)
00523                     var = 'elem-'
00524                     with Indent(s):
00525                         s.write('(dolist (%s _%s)'%(var, f.name))
00526         else: # array but not builtin
00527             if f.array_len:
00528                 s.write('(dotimes (i %s)'%f.array_len)
00529                 var = '(elt _%s i)'%f.name
00530             else:
00531                 write_deserialize_length(s, f, var, True)
00532                 var = 'elem-'
00533                 with Indent(s):
00534                     s.write('(dolist (%s _%s)'%(var, f.name))
00535     if f.is_array and f.base_type == 'uint8':
00536         pass
00537     elif f.is_builtin:
00538         with Indent(s):
00539             write_deserialize_builtin(s, f, var)
00540     else:
00541         with Indent(s):
00542             s.write('(send %s :deserialize buf ptr-) (incf ptr- (send %s :serialization-length))'%(var, var))
00543 
00544     if f.is_array and not f.base_type == 'uint8':
00545         with Indent(s):
00546             if f.array_len:
00547                 s.write(')')
00548             else:
00549                 s.write('))')
00550 
00551 
00552 def write_deserialize(s, spec):
00553     """
00554     Write the deserialize method
00555     """
00556     with Indent(s):
00557         s.write('(:deserialize')
00558         with Indent(s,inc=1):
00559             s.write('(buf &optional (ptr- 0))')
00560             for f in spec.parsed_fields():
00561                 write_deserialize_field(s, f, spec.package)
00562             s.write(';;')
00563             s.write('self)')
00564         s.write(')')
00565         s.newline()
00566 
00567 def write_md5sum(s, msg_context, spec, parent=None):
00568     md5sum = genmsg.compute_md5(msg_context, parent or spec)
00569     s.write('(setf (get %s::%s :md5sum-) "%s")'%(spec.package, spec.actual_name, md5sum))
00570 
00571 def write_ros_datatype(s, spec):
00572     s.write('(setf (get %s::%s :datatype-) "%s/%s")'%(spec.package, spec.actual_name, spec.package, spec.actual_name))
00573 
00574 def write_message_definition(s, msg_context, spec):
00575     s.write('(setf (get %s::%s :definition-)'%(spec.package, spec.actual_name))
00576     with Indent(s,6):
00577         s.write('"')
00578         definition = genmsg.compute_full_text(msg_context, spec)
00579         lines = definition.split('\n')
00580         for line in lines:
00581             l = line.replace('\\', '\\\\')
00582             l = l.replace('"', '\\"')
00583             s.write('%s\n'%l, indent=False, newline=False)
00584     s.write('")', newline=False)
00585     s.write('\n\n')
00586 
00587 def write_service_definition(s, msg_context, spec, parent):
00588     s.write('(setf (get %s::%s :definition-)'%(parent.package, parent.actual_name))
00589     with Indent(s,6):
00590         s.write('"')
00591         for spec_service in [spec.request, spec.response]:
00592             definition = genmsg.compute_full_text(msg_context, spec_service)
00593             lines = definition.split('\n')
00594             for line in lines[:-1]:
00595                 l = line.replace('\\', '\\\\')
00596                 l = l.replace('"', '\\"')
00597                 s.write('%s\n'%l, indent=False, newline=False)
00598             if spec_service == spec.request:
00599                 s.write('---\n', indent=False, newline=False)
00600     s.write('")', newline=False)
00601 
00602 def write_builtin_length(s, f, var='msg'):
00603     if f.base_type in ['int8', 'uint8']:
00604         s.write('1')
00605     elif f.base_type in ['int16', 'uint16']:
00606         s.write('2')
00607     elif f.base_type in ['int32', 'uint32', 'float32']:
00608         s.write('4')
00609     elif f.base_type in ['int64', 'uint64', 'float64', 'duration', 'time']:
00610         s.write('8')
00611     elif f.base_type == 'string':
00612         s.write('4 (length _%s)'%f.name)
00613     elif f.base_type in ['bool', 'byte', 'char']:
00614         s.write('1')
00615     else:
00616         raise ValueError('Unknown: %s', f.base_type)
00617 
00618 def write_serialization_length(s, spec):
00619     with Indent(s):
00620         s.write('(:serialization-length')
00621         with Indent(s, inc=1):
00622             s.write('()')
00623             s.write('(+')
00624             with Indent(s, 1):
00625                 if not spec.parsed_fields():
00626                     s.write('0')
00627                 for field in spec.parsed_fields():
00628                     s.write(';; %s _%s'%(field.type, field.name))
00629                     if field.is_array:
00630                         if field.is_builtin and not is_string(field.base_type):
00631                             s.write('(* ')
00632                         else:
00633                             s.write('(apply #\'+ ')
00634                         s.block_next_indent()
00635 
00636                         if field.is_builtin:
00637                             if not field.array_len:
00638                                 if is_string(field.base_type):
00639                                     s.write('(mapcar #\'(lambda (x) (+ 4 (length x))) _%s)) 4'%(field.name))
00640                                 else:
00641                                     write_builtin_length(s, field)
00642                                     s.write('(length _%s)) 4'%field.name, newline=False)
00643                             else:
00644                                 write_builtin_length(s, field)
00645                                 s.write('%s)'%field.array_len, newline=False)
00646                         else:
00647                             if field.array_len:
00648                                 s.write('(send-all _%s :serialization-length))'%field.name)
00649                             else:
00650                                 s.write('(send-all _%s :serialization-length)) 4'%field.name)
00651                     else:
00652                         if field.is_builtin:
00653                             write_builtin_length(s, field)
00654                         else:
00655                             s.write('(send _%s :serialization-length)'%field.name)
00656 
00657                 s.write('))')
00658 
00659 
00660 def write_provide(s, msg_context, spec):
00661     md5sum = genmsg.compute_md5(msg_context, spec)
00662     s.write('(provide :%s/%s "%s")'%(spec.package, spec.actual_name,md5sum))
00663     s.write('\n')
00664 
00665 def write_constants(s, spec):
00666     if spec.constants:
00667         for c in spec.constants:
00668             s.write('(intern "*%s*" (find-package "%s::%s"))'%(c.name.upper(), spec.package.upper(), spec.actual_name.upper()))
00669             s.write('(shadow \'*%s* (find-package "%s::%s"))'%(c.name.upper(), spec.package.upper(), spec.actual_name.upper()))
00670             if c.type == 'string':
00671                 s.write('(defconstant %s::%s::*%s* "%s")'%(spec.package, spec.actual_name, c.name.upper(), c.val.replace('"', '\\"')))
00672             elif c.type == 'bool':
00673                 s.write('(defconstant %s::%s::*%s* %s)'%(spec.package, spec.actual_name, c.name.upper(), "t" if c.val == "True" else "nil"))
00674             else:
00675                 s.write('(defconstant %s::%s::*%s* %s)'%(spec.package, spec.actual_name, c.name.upper(), c.val))
00676 
00677 def write_srv_component(s, spec, context, parent):
00678     spec.component_type='service'
00679     write_constants(s, spec)
00680     write_defclass(s, spec)
00681     write_defmethod(s, spec)
00682     write_accessors(s, spec)
00683     write_serialization_length(s, spec)
00684     write_serialize(s, spec)
00685     write_deserialize(s, spec)
00686 
00687 def write_service_specific_methods(s, context, spec):
00688     ### this should be move to previsou definition section ???
00689     s.write('(defclass %s::%s'%(spec.package, spec.actual_name))
00690     with Indent(s):
00691         s.write(':super ros::object')
00692         s.write(':slots ())')
00693     s.newline()
00694     write_md5sum(s, context, spec, parent=spec)
00695     write_ros_datatype(s, spec)
00696     s.write('(setf (get %s::%s :request) %s::%s)'%(spec.package, spec.actual_name, spec.request.package, spec.request.actual_name))
00697     s.write('(setf (get %s::%s :response) %s::%s)'%(spec.package, spec.actual_name, spec.response.package, spec.response.actual_name))
00698     s.newline()
00699     s.write('(defmethod %s::%s'%(spec.request.package, spec.request.actual_name))
00700     s.write('  (:response () (instance %s::%s :init)))'%(spec.response.package, spec.response.actual_name))
00701     s.newline()
00702     for spec_service in [spec.request, spec.response]:
00703         write_md5sum(s, context, spec_service, parent=spec)
00704         write_ros_datatype(s, spec_service)
00705         write_service_definition(s, context, spec, spec_service)
00706         s.newline()
00707     s.write('\n')
00708     write_provide(s, context, spec)
00709     s.write('\n', newline=False)
00710 
00711 def generate_msg(pkg, files, out_dir, search_path):
00712     """
00713     Generate euslisp code for all messages in a package
00714     """
00715     msg_context = MsgContext.create_default()
00716     for f in files:
00717         f = os.path.abspath(f)
00718         infile = os.path.basename(f)
00719         full_type = genmsg.gentools.compute_full_type_name(pkg, infile)
00720         spec = genmsg.msg_loader.load_msg_from_file(msg_context, f, full_type)
00721         generate_msg_from_spec(msg_context, spec, search_path, out_dir, pkg)
00722 
00723 def generate_srv(pkg, files, out_dir, search_path):
00724     """
00725     Generate euslisp code for all services in a package
00726     """
00727     msg_context = MsgContext.create_default()
00728     for f in files:
00729         f = os.path.abspath(f)
00730         infile = os.path.basename(f)
00731         full_type = genmsg.gentools.compute_full_type_name(pkg, infile)
00732         spec = genmsg.msg_loader.load_srv_from_file(msg_context, f, full_type)
00733         generate_srv_from_spec(msg_context, spec, search_path, out_dir, pkg, f)
00734 
00735 def msg_list(pkg, search_path, ext):
00736     dir_list = search_path[pkg]
00737     files = []
00738     for d in dir_list:
00739         files.extend([f for f in os.listdir(d) if f.endswith(ext)])
00740     return [f[:-len(ext)] for f in files]
00741 
00742 def generate_msg_from_spec(msg_context, spec, search_path, output_dir, package):
00743     """
00744     Generate a message
00745     
00746     @param msg_path: The path to the .msg file
00747     @type msg_path: str
00748     """
00749     genmsg.msg_loader.load_depends(msg_context, spec, search_path)
00750     spec.actual_name=spec.short_name
00751     spec.component_type='message'
00752     msgs = msg_list(package, search_path, '.msg')
00753     for m in msgs:
00754         genmsg.load_msg_by_type(msg_context, '%s/%s'%(package, m), search_path)
00755     
00756 
00757     ########################################
00758     # 1. Write the .l file
00759     ########################################
00760     
00761     io = StringIO()
00762     s =  IndentedWriter(io)
00763     write_begin(s, spec)
00764     write_include(s, spec)
00765     write_constants(s, spec)
00766     write_defclass(s, spec)
00767     write_defmethod(s, spec)
00768     write_accessors(s, spec)
00769     write_serialization_length(s, spec)
00770     write_serialize(s, spec)
00771     write_deserialize(s, spec)
00772     write_md5sum(s, msg_context, spec)
00773     write_ros_datatype(s, spec)
00774     write_message_definition(s, msg_context, spec)
00775     write_provide(s, msg_context, spec)
00776     
00777     if (not os.path.exists(output_dir)):
00778         # if we're being run concurrently, the above test can report false but os.makedirs can still fail if
00779         # another copy just created the directory
00780         try:
00781             os.makedirs(output_dir)
00782         except OSError as e:
00783             pass
00784 
00785     with open('%s/%s.l'%(output_dir, spec.short_name), 'w') as f:
00786         f.write(io.getvalue() + "\n")
00787     io.close()
00788 
00789 
00790 # t0 most of this could probably be refactored into being shared with messages
00791 def generate_srv_from_spec(msg_context, spec, search_path, output_dir, package, path):
00792     "Generate code from .srv file"
00793     genmsg.msg_loader.load_depends(msg_context, spec, search_path)
00794     ext = '.srv'
00795     srvs = [f[:-len(ext)] for f in os.listdir(os.path.dirname(path)) if f.endswith(ext)]
00796     for s in srvs:
00797         load_srv_from_file(msg_context, path, '%s/%s'%(package, s))
00798 
00799     ########################################
00800     # 1. Write the .l file
00801     ########################################
00802 
00803     io = StringIO()
00804     s = IndentedWriter(io)
00805     write_begin(s, spec, True)
00806     write_include(s, spec.request, is_srv=True)
00807     write_include(s, spec.response, is_srv=True)
00808     spec.request.actual_name='%sRequest'%spec.short_name
00809     spec.response.actual_name='%sResponse'%spec.short_name
00810     write_srv_component(s, spec.request, msg_context, spec)
00811     write_srv_component(s, spec.response, msg_context, spec)
00812     write_service_specific_methods(s, msg_context, spec)
00813 
00814     with open('%s/%s.l'%(output_dir, spec.short_name), 'w') as f:
00815         f.write(io.getvalue())
00816     io.close()
00817 
00818 


geneus
Author(s): Kei Okada
autogenerated on Tue Sep 5 2017 02:39:32