generate.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2009, Willow Garage, Inc.
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 Willow Garage, Inc. 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 Lisp
00035 ## 
00036 ## Converts ROS .msg and .srv files in a package into Lisp source code
00037 
00038 ## t0: needed for script to work
00039 ## t1: for reference; remove once running
00040 ## t2: can be changed once we remove strict diff-compatibility requirement with old version of genmsg_lisp
00041 
00042 import sys
00043 import os
00044 import traceback
00045 import re
00046 
00047 #import roslib.msgs
00048 #import roslib.srvs
00049 #import roslib.packages
00050 #import roslib.gentools
00051 from genmsg import SrvSpec, MsgSpec, MsgContext
00052 from genmsg.msg_loader import load_srv_from_file, load_msg_by_type
00053 import genmsg.gentools
00054 
00055 try:
00056     from cStringIO import StringIO #Python 2.x
00057 except ImportError:
00058     from io import StringIO #Python 3.x
00059 
00060 ############################################################
00061 # Built in types
00062 ############################################################
00063 
00064 def is_fixnum(t):
00065     return t in ['int8', 'uint8', 'int16', 'uint16']
00066 
00067 def is_integer(t):
00068     return is_fixnum(t) or t in ['byte', 'char', 'int32', 'uint32', 'int64', 'uint64'] #t2 byte, char can be fixnum
00069 
00070 def is_signed_int(t):
00071     return t in ['int8', 'int16', 'int32', 'int64']
00072 
00073 def is_unsigned_int(t):
00074     return t in ['uint8', 'uint16', 'uint32', 'uint64']
00075 
00076 def is_bool(t):
00077     return t == 'bool'
00078 
00079 def is_string(t):
00080     return t == 'string'
00081 
00082 def is_float(t):
00083     return t in ['float16', 'float32', 'float64']
00084 
00085 def is_time(t):
00086     return t in ['time', 'duration']
00087 
00088 def field_type(f):
00089     if f.is_builtin:
00090         elt_type = lisp_type(f.base_type)
00091     else:
00092         elt_type = msg_type(f)
00093     if f.is_array:
00094         return '(cl:vector %s)'%elt_type
00095     else:
00096         return elt_type
00097 
00098 def parse_msg_type(f):
00099     if f.base_type == 'Header':
00100         return ('std_msgs', 'Header')
00101     else:
00102         return f.base_type.split('/')
00103 
00104 # t2 no need for is_array    
00105 def msg_type(f):
00106     (pkg, msg) = parse_msg_type(f)
00107     return '%s-msg:%s'%(pkg, msg)
00108 
00109 def lisp_type(t):
00110     if is_fixnum(t):
00111         return 'cl:fixnum'
00112     elif is_integer(t):
00113         return 'cl:integer'
00114     elif is_bool(t):
00115         return 'cl:boolean'
00116     elif is_float(t):
00117         return 'cl:float'
00118     elif is_time(t):
00119         return 'cl:real'
00120     elif is_string(t):
00121         return 'cl:string'
00122     else:
00123         raise ValueError('%s is not a recognized primitive type'%t)
00124 
00125 def field_initform(f):
00126     if f.is_builtin:
00127         initform = lisp_initform(f.base_type)
00128         elt_type = lisp_type(f.base_type)
00129     else:
00130         initform = '(cl:make-instance \'%s)'%msg_type(f)
00131         elt_type = msg_type(f)
00132     if f.is_array:
00133         len = f.array_len or 0
00134         return '(cl:make-array %s :element-type \'%s :initial-element %s)'%(len, elt_type, initform)
00135     else:
00136         return initform
00137 
00138 def lisp_initform(t):
00139     if is_integer(t):
00140         return '0'
00141     elif is_bool(t):
00142         return 'cl:nil'
00143     elif is_float(t):
00144         return '0.0'
00145     elif is_time(t):
00146         return 0
00147     elif is_string(t):
00148         return '\"\"'
00149     else:
00150         raise ValueError('%s is not a recognized primitive type'%t)
00151 
00152 NUM_BYTES = {'int8': 1, 'int16': 2, 'int32': 4, 'int64': 8,
00153              'uint8': 1, 'uint16': 2, 'uint32': 4, 'uint64': 8}
00154              
00155              
00156 
00157 ############################################################
00158 # Indented writer
00159 ############################################################
00160 
00161 class IndentedWriter():
00162 
00163     def __init__(self, s):
00164         self.str = s
00165         self.indentation = 0
00166         self.block_indent = False
00167 
00168     def write(self, s, indent=True, newline=True):
00169         if not indent:
00170             newline = False
00171         if self.block_indent:
00172             self.block_indent = False
00173         else:
00174             if newline:
00175                 self.str.write('\n')
00176             if indent:
00177                 for i in range(self.indentation):
00178                     self.str.write(' ')
00179         self.str.write(s)
00180 
00181     def newline(self):
00182         self.str.write('\n')
00183 
00184     def inc_indent(self, inc=2):
00185         self.indentation += inc
00186 
00187     def dec_indent(self, dec=2):
00188         self.indentation -= dec
00189 
00190     def reset_indent(self):
00191         self.indentation = 0
00192 
00193     def block_next_indent(self):
00194         self.block_indent = True
00195 
00196 class Indent():
00197 
00198     def __init__(self, w, inc=2, indent_first=True):
00199         self.writer = w
00200         self.inc = inc
00201         self.indent_first = indent_first
00202 
00203     def __enter__(self):
00204         self.writer.inc_indent(self.inc)
00205         if not self.indent_first:
00206             self.writer.block_next_indent()
00207 
00208     def __exit__(self, type, val, traceback):
00209         self.writer.dec_indent(self.inc)
00210 
00211     
00212 
00213 def write_begin(s, spec, is_service=False):
00214     "Writes the beginning of the file: a comment saying it's auto-generated and the in-package form"
00215     
00216     s.write('; Auto-generated. Do not edit!\n\n\n', newline=False)
00217     suffix = 'srv' if is_service else 'msg'
00218     s.write('(cl:in-package %s-%s)\n\n\n'%(spec.package, suffix), newline=False)
00219 
00220 def write_html_include(s, spec, is_srv=False):
00221     
00222     s.write(';//! \\htmlinclude %s.msg.html\n'%spec.actual_name, newline=False) # t2
00223 
00224 def write_slot_definition(s, field):
00225     "Write the definition of a slot corresponding to a single message field"
00226 
00227     s.write('(%s'%field.name)
00228     with Indent(s, 1):
00229         s.write(':reader %s'%field.name)
00230         s.write(':initarg :%s'%field.name)
00231         s.write(':type %s'%field_type(field))
00232     i = 0 if field.is_array else 1 # t2
00233     with Indent(s, i):
00234         s.write(':initform %s)'%field_initform(field))
00235 
00236 def write_deprecated_readers(s, spec):
00237     suffix = 'srv' if spec.component_type == 'service' else 'msg'
00238     for field in spec.parsed_fields():
00239         s.newline()
00240         s.write('(cl:ensure-generic-function \'%s-val :lambda-list \'(m))' % field.name)
00241         s.write('(cl:defmethod %s-val ((m %s))'%(field.name, message_class(spec)))
00242         with Indent(s):
00243             s.write('(roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader %s-%s:%s-val is deprecated.  Use %s-%s:%s instead.")'%(spec.package, suffix, field.name, spec.package, suffix, field.name))
00244             s.write('(%s m))'%field.name)
00245             
00246             
00247 
00248 def write_defclass(s, spec):
00249     "Writes the defclass that defines the message type"
00250     cl = message_class(spec)
00251     new_cl = new_message_class(spec)
00252     suffix = 'srv' if spec.component_type == 'service' else 'msg'
00253     s.write('(cl:defclass %s (roslisp-msg-protocol:ros-message)'%cl)
00254     with Indent(s):
00255         s.write('(')
00256         with Indent(s, inc=1, indent_first=False):
00257             for field in spec.parsed_fields():
00258                 write_slot_definition(s, field)
00259         s.write(')', indent=False)
00260     s.write(')')
00261     s.newline()
00262     s.write('(cl:defclass %s (%s)'%(new_cl, cl))
00263     with Indent(s):
00264         s.write('())')
00265     s.newline()
00266     s.write('(cl:defmethod cl:initialize-instance :after ((m %s) cl:&rest args)'%cl)
00267     with Indent(s):
00268         s.write('(cl:declare (cl:ignorable args))')
00269         s.write('(cl:unless (cl:typep m \'%s)'%new_cl)
00270         with Indent(s):
00271             s.write('(roslisp-msg-protocol:msg-deprecation-warning "using old message class name %s-%s:%s is deprecated: use %s-%s:%s instead.")))'%(spec.package, suffix, cl, spec.package, suffix, new_cl))
00272     
00273     
00274 
00275 def message_class(spec):
00276     """
00277     Return the CLOS class name for this message type
00278     """
00279     return '<%s>'%spec.actual_name
00280 
00281 def new_message_class(spec):
00282     return spec.actual_name
00283 
00284     
00285 def write_serialize_length(s, v, is_array=False):
00286     #t2
00287     var = '__ros_arr_len' if is_array else '__ros_str_len'
00288     
00289     s.write('(cl:let ((%s (cl:length %s)))'%(var, v))
00290     with Indent(s):
00291         for x in range(0, 32, 8):
00292             s.write('(cl:write-byte (cl:ldb (cl:byte 8 %s) %s) ostream)'%(x, var))
00293     s.write(')', indent=False)
00294 
00295 
00296 def write_serialize_bits(s, v, num_bytes): 
00297     for x in range(0, num_bytes*8, 8):
00298         s.write('(cl:write-byte (cl:ldb (cl:byte 8 %s) %s) ostream)'%(x, v))
00299 
00300 def write_serialize_bits_signed(s, v, num_bytes):
00301     num_bits = num_bytes*8
00302     s.write('(cl:let* ((signed %s) (unsigned (cl:if (cl:< signed 0) (cl:+ signed %s) signed)))'%(v, 2**num_bits))
00303     with Indent(s):
00304         write_serialize_bits(s, 'unsigned', num_bytes)
00305         s.write(')')
00306 
00307 
00308 
00309 # t2: can get rid of this lookup_slot stuff        
00310 def write_serialize_builtin(s, f, var='msg', lookup_slot=True):
00311     v = '(cl:slot-value %s \'%s)'%(var, f.name) if lookup_slot else var
00312     if f.base_type == 'string':
00313         write_serialize_length(s, v)
00314         s.write('(cl:map cl:nil #\'(cl:lambda (c) (cl:write-byte (cl:char-code c) ostream)) %s)'%v)
00315     elif f.base_type == 'float32':
00316         s.write('(cl:let ((bits %s))'%'(roslisp-utils:encode-single-float-bits %s)'%v)
00317         with Indent(s):
00318             write_serialize_bits(s, 'bits', 4)
00319         s.write(')', False)
00320     elif f.base_type == 'float64':
00321         s.write('(cl:let ((bits %s))'%'(roslisp-utils:encode-double-float-bits %s)'%v)
00322         with Indent(s):
00323             write_serialize_bits(s, 'bits', 8)
00324         s.write(')', False)
00325     elif f.base_type == 'bool':
00326         s.write('(cl:write-byte (cl:ldb (cl:byte 8 0) (cl:if %s 1 0)) ostream)'%v)
00327     elif f.base_type in ['byte', 'char']:
00328         s.write('(cl:write-byte (cl:ldb (cl:byte 8 0) %s) ostream)'%v)
00329     elif f.base_type in ['duration', 'time']:
00330         s.write('(cl:let ((__sec (cl:floor %s))'%v)
00331         s.write('      (__nsec (cl:round (cl:* 1e9 (cl:- %s (cl:floor %s))))))'%(v,v))
00332         with Indent(s):
00333             write_serialize_bits(s, '__sec', 4)
00334             write_serialize_bits(s, '__nsec', 4)
00335             s.write(')', False)
00336     elif is_signed_int(f.base_type):
00337         write_serialize_bits_signed(s, v, NUM_BYTES[f.base_type])
00338     elif is_unsigned_int(f.base_type):
00339         write_serialize_bits(s, v, NUM_BYTES[f.base_type])        
00340     else:
00341         raise ValueError('Unknown type: %s', f.base_type)
00342     
00343 def write_serialize_field(s, f):
00344     slot = '(cl:slot-value msg \'%s)'%f.name
00345     if f.is_array:
00346         if not f.array_len:
00347             write_serialize_length(s, slot, True)
00348         s.write('(cl:map cl:nil #\'(cl:lambda (ele) ')
00349         var = 'ele'
00350         s.block_next_indent()
00351         lookup_slot = False
00352     else:
00353         var='msg'
00354         lookup_slot = True
00355 
00356     if f.is_builtin:
00357         write_serialize_builtin(s, f, var, lookup_slot=lookup_slot)
00358     else:
00359         to_write = slot if lookup_slot else var #t2
00360         s.write('(roslisp-msg-protocol:serialize %s ostream)'%to_write)
00361 
00362     if f.is_array:
00363         s.write(')', False)
00364         s.write(' %s)'%slot)
00365     
00366 def write_serialize(s, spec):
00367     """
00368     Write the serialize method
00369     """
00370     s.write('(cl:defmethod roslisp-msg-protocol:serialize ((msg %s) ostream)'%message_class(spec))
00371     with Indent(s):
00372         s.write('"Serializes a message object of type \'%s"'%message_class(spec))
00373         for f in spec.parsed_fields():
00374             write_serialize_field(s, f)
00375     s.write(')')
00376     
00377 
00378 # t2 can get rid of is_array
00379 def write_deserialize_length(s, is_array=False):
00380     var = '__ros_arr_len' if is_array else '__ros_str_len'
00381     s.write('(cl:let ((%s 0))'%var)
00382     with Indent(s):
00383         for x in range(0, 32, 8):
00384             s.write('(cl:setf (cl:ldb (cl:byte 8 %s) %s) (cl:read-byte istream))'%(x, var))
00385 
00386 def write_deserialize_bits(s, v, num_bytes):
00387     for x in range(0, num_bytes*8, 8):
00388         s.write('(cl:setf (cl:ldb (cl:byte 8 %s) %s) (cl:read-byte istream))'%(x, v))
00389 
00390 def write_deserialize_bits_signed(s, v, num_bytes):
00391     s.write('(cl:let ((unsigned 0))')
00392     num_bits = 8*num_bytes
00393     with Indent(s):
00394         write_deserialize_bits(s, 'unsigned', num_bytes)
00395         s.write('(cl:setf %s (cl:if (cl:< unsigned %s) unsigned (cl:- unsigned %s))))'%(v, 2**(num_bits-1), 2**num_bits))
00396 
00397     
00398     
00399 def write_deserialize_builtin(s, f, v):
00400     if f.base_type == 'string':
00401         write_deserialize_length(s)
00402         with Indent(s):
00403             s.write('(cl:setf %s (cl:make-string __ros_str_len))'%v)
00404             s.write('(cl:dotimes (__ros_str_idx __ros_str_len msg)')
00405             with Indent(s):
00406                 s.write('(cl:setf (cl:char %s __ros_str_idx) (cl:code-char (cl:read-byte istream)))))'%v)
00407     elif f.base_type == 'float32':
00408         s.write('(cl:let ((bits 0))')
00409         with Indent(s):
00410             write_deserialize_bits(s, 'bits', 4)
00411         s.write('(cl:setf %s (roslisp-utils:decode-single-float-bits bits)))'%v)
00412     elif f.base_type == 'float64':
00413         s.write('(cl:let ((bits 0))')
00414         with Indent(s):
00415             write_deserialize_bits(s, 'bits', 8)
00416         s.write('(cl:setf %s (roslisp-utils:decode-double-float-bits bits)))'%v)
00417     elif f.base_type == 'bool':
00418         s.write('(cl:setf %s (cl:not (cl:zerop (cl:read-byte istream))))'%v)
00419     elif f.base_type in ['byte', 'char']:
00420         s.write('(cl:setf (cl:ldb (cl:byte 8 0) %s) (cl:read-byte istream))'%v)
00421     elif f.base_type in ['duration', 'time']:
00422         s.write('(cl:let ((__sec 0) (__nsec 0))')
00423         with Indent(s):
00424             write_deserialize_bits(s, '__sec', 4)
00425             write_deserialize_bits(s, '__nsec', 4)
00426             s.write('(cl:setf %s (cl:+ (cl:coerce __sec \'cl:double-float) (cl:/ __nsec 1e9))))'%v)
00427     elif is_signed_int(f.base_type):
00428         write_deserialize_bits_signed(s, v, NUM_BYTES[f.base_type])
00429     elif is_unsigned_int(f.base_type):
00430         write_deserialize_bits(s, v, NUM_BYTES[f.base_type])
00431     else:
00432         raise ValueError('%s unknown'%f.base_type)
00433 
00434 
00435 def write_deserialize_field(s, f, pkg):
00436     slot = '(cl:slot-value msg \'%s)'%f.name
00437     var = slot
00438     if f.is_array:
00439         if not f.array_len:
00440             write_deserialize_length(s, True)
00441             length = '__ros_arr_len'
00442         else:
00443             length = '%s'%f.array_len
00444             
00445         s.write('(cl:setf %s (cl:make-array %s))'%(slot, length))
00446         s.write('(cl:let ((vals %s))'%slot) # t2
00447         var = '(cl:aref vals i)'
00448         with Indent(s):
00449             s.write('(cl:dotimes (i %s)'%length)
00450 
00451     if f.is_builtin:
00452         with Indent(s):
00453             write_deserialize_builtin(s, f, var)
00454     else:
00455         if f.is_array:
00456             with Indent(s):
00457                 s.write('(cl:setf %s (cl:make-instance \'%s))'%(var, msg_type(f)))
00458         s.write('(roslisp-msg-protocol:deserialize %s istream)'%var)
00459 
00460     if f.is_array:
00461         s.write('))', False)
00462         if not f.array_len:
00463             s.write(')', False)
00464 
00465 
00466 def write_deserialize(s, spec):
00467     """
00468     Write the deserialize method
00469     """
00470     s.write('(cl:defmethod roslisp-msg-protocol:deserialize ((msg %s) istream)'%message_class(spec))
00471     with Indent(s):
00472         s.write('"Deserializes a message object of type \'%s"'%message_class(spec))
00473         for f in spec.parsed_fields():
00474             write_deserialize_field(s, f, spec.package)
00475         s.write('msg')
00476     s.write(')')
00477 
00478 def write_class_exports(s, msgs, pkg):
00479     "Write the _package.lisp file"
00480     s.write('(cl:defpackage %s-msg'%pkg, False)
00481     with Indent(s):
00482         s.write('(:use )')
00483         s.write('(:export')
00484         with Indent(s, inc=1):
00485             for m in msgs:
00486                 msg_class = '<%s>'%m
00487                 s.write('"%s"'%msg_class.upper())
00488                 s.write('"%s"'%m.upper())
00489         s.write('))\n\n')
00490 
00491 def write_srv_exports(s, srvs, pkg):
00492     "Write the _package.lisp file for a service directory"
00493     s.write('(cl:defpackage %s-srv'%pkg, False)
00494     with Indent(s):
00495         s.write('(:use )')
00496         s.write('(:export')
00497         with Indent(s, inc=1):
00498             for srv in srvs:
00499                 s.write('"%s"'%srv.upper())
00500                 s.write('"<%s-REQUEST>"'%srv.upper())
00501                 s.write('"%s-REQUEST"'%srv.upper())
00502                 s.write('"<%s-RESPONSE>"'%srv.upper())
00503                 s.write('"%s-RESPONSE"'%srv.upper())
00504         s.write('))\n\n')
00505 
00506 
00507 def write_asd_deps(s, deps, msgs):
00508     with Indent(s):
00509         s.write(':depends-on (:roslisp-msg-protocol :roslisp-utils ')
00510         with Indent(s, inc=13, indent_first=False):
00511             for d in sorted(deps):
00512                 s.write(':%s-msg'%d)
00513     s.write(')') #t2 indentation
00514     with Indent(s):
00515         s.write(':components ((:file "_package")')
00516         with Indent(s):
00517             for name in msgs:
00518                 s.write('(:file "%s" :depends-on ("_package_%s"))'%(name, name))
00519                 s.write('(:file "_package_%s" :depends-on ("_package"))'%name)
00520         s.write('))')
00521                 
00522 
00523 
00524 def write_srv_asd(s, pkg, srvs, context):
00525     s.write('(cl:in-package :asdf)')
00526     s.newline()
00527     s.write('(defsystem "%s-srv"'%pkg)
00528 
00529     # Figure out set of depended-upon ros packages
00530     deps = set()
00531     for srv in srvs:
00532         req_spec = context.get_registered('%s/%sRequest'%(pkg, srv))
00533         resp_spec = context.get_registered('%s/%sResponse'%(pkg, srv))
00534         for f in req_spec.parsed_fields():
00535             if not f.is_builtin:
00536                 (p, _) = parse_msg_type(f)
00537                 deps.add(p)
00538         for f in resp_spec.parsed_fields():
00539             if not f.is_builtin:
00540                 (p, _) = parse_msg_type(f)
00541                 deps.add(p)
00542 
00543     write_asd_deps(s, deps, srvs)
00544 
00545 
00546 def write_asd(s, pkg, msgs, context):
00547     s.write('(cl:in-package :asdf)')
00548     s.newline()
00549     s.write('(defsystem "%s-msg"'%pkg)
00550 
00551     # Figure out set of depended-upon ros packages
00552     deps = set()
00553     for m in msgs:
00554         spec = context.get_registered('%s/%s'%(pkg, m))
00555         for f in spec.parsed_fields():
00556             if not f.is_builtin:
00557                 (p, _) = parse_msg_type(f)
00558                 deps.add(p)
00559     if pkg in deps:
00560         deps.remove(pkg)
00561     write_asd_deps(s, deps, msgs)   
00562 
00563 def write_accessor_exports(s, spec):
00564     "Write the package exports for this message/service"
00565     is_srv = isinstance(spec, SrvSpec)
00566     suffix = 'srv' if is_srv else 'msg'
00567     s.write('(cl:in-package %s-%s)'%(spec.package, suffix), indent=False)
00568     s.write('(cl:export \'(')
00569     if is_srv:
00570         fields = spec.request.parsed_fields()[:]
00571         fields.extend(spec.response.parsed_fields())
00572     else:
00573         fields = spec.parsed_fields()
00574     
00575     with Indent(s, inc=10, indent_first=False):
00576         for f in fields:
00577             accessor = '%s-val'%f.name
00578             s.write('%s'%accessor.upper())
00579             s.write('%s'%f.name.upper())
00580     s.write('))')
00581 
00582 
00583 def write_ros_datatype(s, spec):
00584     for c in (message_class(spec), new_message_class(spec)):
00585         s.write('(cl:defmethod roslisp-msg-protocol:ros-datatype ((msg (cl:eql \'%s)))'%c)
00586         with Indent(s):
00587             s.write('"Returns string type for a %s object of type \'%s"'%(spec.component_type, c))
00588             s.write('"%s")'%spec.full_name)
00589 
00590 def write_md5sum(s, msg_context, spec, parent=None):
00591     md5sum = genmsg.compute_md5(msg_context, parent or spec)
00592     for c in (message_class(spec), new_message_class(spec)):
00593         s.write('(cl:defmethod roslisp-msg-protocol:md5sum ((type (cl:eql \'%s)))'%c)
00594         with Indent(s):
00595             # t2 this should print 'service' instead of 'message' if it's a service request or response
00596             s.write('"Returns md5sum for a message object of type \'%s"'%c)
00597             s.write('"%s")'%md5sum)
00598 
00599 def write_message_definition(s, msg_context, spec):
00600     for c in (message_class(spec), new_message_class(spec)):
00601         s.write('(cl:defmethod roslisp-msg-protocol:message-definition ((type (cl:eql \'%s)))'%c)
00602         with Indent(s):
00603             s.write('"Returns full string definition for message of type \'%s"'%c)
00604             s.write('(cl:format cl:nil "')
00605             definition = genmsg.compute_full_text(msg_context, spec)
00606             lines = definition.split('\n')
00607             for line in lines:
00608                 l = line.replace('\\', '\\\\')
00609                 l = l.replace('"', '\\"')
00610                 l = l.replace('~', '~~')
00611                 s.write('%s~%%'%l, indent=False)
00612             s.write('~%', indent=False)
00613             s.write('"))', indent=False)
00614 
00615 def write_builtin_length(s, f, var='msg'):
00616     if f.base_type in ['int8', 'uint8']:
00617         s.write('1')
00618     elif f.base_type in ['int16', 'uint16']:
00619         s.write('2')
00620     elif f.base_type in ['int32', 'uint32', 'float32']:
00621         s.write('4')
00622     elif f.base_type in ['int64', 'uint64', 'float64', 'duration', 'time']:
00623         s.write('8')
00624     elif f.base_type == 'string':
00625         s.write('4 (cl:length %s)'%var)
00626     elif f.base_type in ['bool', 'byte', 'char']:
00627         s.write('1')
00628     else:
00629         raise ValueError('Unknown: %s', f.base_type)
00630 
00631 def write_serialization_length(s, spec):
00632     c = message_class(spec)
00633     s.write('(cl:defmethod roslisp-msg-protocol:serialization-length ((msg %s))'%c)
00634     with Indent(s):
00635         s.write('(cl:+ 0')
00636         with Indent(s, 3):
00637             for field in spec.parsed_fields():
00638                 slot = '(cl:slot-value msg \'%s)'%field.name
00639                 if field.is_array:
00640                     l = '0' if field.array_len else '4'
00641                     s.write('%s (cl:reduce #\'cl:+ %s :key #\'(cl:lambda (ele) (cl:declare (cl:ignorable ele)) (cl:+ '%(l, slot))
00642                     var = 'ele'
00643                     s.block_next_indent()
00644                 else:
00645                     var = slot
00646                     
00647                 if field.is_builtin:
00648                     write_builtin_length(s, field, var)
00649                 else:
00650                     s.write('(roslisp-msg-protocol:serialization-length %s)'%var)
00651 
00652                 if field.is_array:
00653                     s.write(')))', False)
00654     s.write('))')
00655 
00656 
00657 def write_list_converter(s, spec):
00658     c = message_class(spec)
00659     s.write('(cl:defmethod roslisp-msg-protocol:ros-message-to-list ((msg %s))'%c)
00660     with Indent(s):
00661         s.write('"Converts a ROS message object to a list"')
00662         s.write('(cl:list \'%s'%new_message_class(spec))
00663         with Indent(s):
00664             for f in spec.parsed_fields():
00665                 s.write('(cl:cons \':%s (%s msg))'%(f.name, f.name))
00666     s.write('))')
00667 
00668 def write_constants(s, spec):
00669     if spec.constants:
00670         for cls in (message_class(spec), new_message_class(spec)):
00671             s.write('(cl:defmethod roslisp-msg-protocol:symbol-codes ((msg-type (cl:eql \'%s)))'%cls)
00672             with Indent(s):
00673                 s.write('  "Constants for message type \'%s"'%cls)
00674                 s.write('\'(')
00675                 with Indent(s, indent_first=False):
00676                     for c in spec.constants:
00677                         s.write('(:%s . %s)'%(c.name.upper(), c.val))
00678             s.write(')', False)
00679             s.write(')')
00680 
00681 
00682 def write_srv_component(s, spec, context, parent):
00683     spec.component_type='service'
00684     write_html_include(s, spec)
00685     write_defclass(s, spec)
00686     write_deprecated_readers(s, spec)
00687     write_constants(s, spec)
00688     write_serialize(s, spec)
00689     write_deserialize(s, spec)
00690     write_ros_datatype(s, spec)
00691     write_md5sum(s, context, spec, parent=parent)
00692     write_message_definition(s, context, spec)
00693     write_serialization_length(s, spec)
00694     write_list_converter(s, spec)
00695 
00696 
00697 def write_service_specific_methods(s, spec):
00698     spec.actual_name=spec.short_name
00699     s.write('(cl:defmethod roslisp-msg-protocol:service-request-type ((msg (cl:eql \'%s)))'%spec.short_name)
00700     with Indent(s):
00701         s.write('\'%s)'%new_message_class(spec.request))
00702     s.write('(cl:defmethod roslisp-msg-protocol:service-response-type ((msg (cl:eql \'%s)))'%spec.short_name)
00703     with Indent(s):
00704         s.write('\'%s)'%new_message_class(spec.response))
00705     s.write('(cl:defmethod roslisp-msg-protocol:ros-datatype ((msg (cl:eql \'%s)))'%spec.short_name)
00706     with Indent(s):
00707         s.write('"Returns string type for a service object of type \'%s"'%message_class(spec))
00708         s.write('"%s")'%spec.full_name)
00709 
00710         
00711 def generate_msg(pkg, files, out_dir, search_path):
00712     """
00713     Generate lisp 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 lisp 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     # sort list because listdir is filesystem specific
00741     return sorted([f[:-len(ext)] for f in files])
00742 
00743 def generate_msg_from_spec(msg_context, spec, search_path, output_dir, package):
00744     """
00745     Generate a message
00746     
00747     @param msg_path: The path to the .msg file
00748     @type msg_path: str
00749     """
00750     genmsg.msg_loader.load_depends(msg_context, spec, search_path)
00751     spec.actual_name=spec.short_name
00752     spec.component_type='message'
00753     msgs = msg_list(package, search_path, '.msg')
00754     for m in msgs:
00755         genmsg.load_msg_by_type(msg_context, '%s/%s'%(package, m), search_path)
00756     
00757 
00758     ########################################
00759     # 1. Write the .lisp file
00760     ########################################
00761     
00762     io = StringIO()
00763     s =  IndentedWriter(io)
00764     write_begin(s, spec)
00765     write_html_include(s, spec)
00766     write_defclass(s, spec)
00767     write_deprecated_readers(s, spec)
00768     write_constants(s, spec)
00769     write_serialize(s, spec)
00770     write_deserialize(s, spec)
00771     write_ros_datatype(s, spec)
00772     write_md5sum(s, msg_context, spec)
00773     write_message_definition(s, msg_context, spec)
00774     write_serialization_length(s, spec)
00775     write_list_converter(s, 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.lisp'%(output_dir, spec.short_name), 'w') as f:
00786         f.write(io.getvalue() + "\n")
00787     io.close()
00788 
00789     ########################################
00790     # 2. Write the _package file
00791     # for this message
00792     ########################################
00793 
00794     io = StringIO()
00795     s = IndentedWriter(io)
00796     write_accessor_exports(s, spec)
00797     with open('%s/_package_%s.lisp'%(output_dir, spec.short_name), 'w') as f:
00798         f.write(io.getvalue())
00799     io.close()
00800 
00801     ########################################
00802     # 3. Write the _package.lisp file
00803     # This is being rewritten once per msg
00804     # file, which is inefficient
00805     ########################################
00806 
00807     io = StringIO()
00808     s = IndentedWriter(io)
00809     write_class_exports(s, msgs, package)
00810     with open('%s/_package.lisp'%output_dir, 'w') as f:
00811         f.write(io.getvalue())
00812     io.close()
00813 
00814     ########################################
00815     # 4. Write the .asd file
00816     # This is being written once per msg
00817     # file, which is inefficient
00818     ########################################
00819 
00820     io = StringIO()
00821     s = IndentedWriter(io)
00822     write_asd(s, package, msgs, msg_context)
00823     with open('%s/%s-msg.asd'%(output_dir, package), 'w') as f:
00824         f.write(io.getvalue())
00825     io.close()
00826 
00827 # t0 most of this could probably be refactored into being shared with messages
00828 def generate_srv_from_spec(msg_context, spec, search_path, output_dir, package, path):
00829     "Generate code from .srv file"
00830     genmsg.msg_loader.load_depends(msg_context, spec, search_path)
00831     ext = '.srv'
00832     srv_path = os.path.dirname(path)
00833     srvs = msg_list(package, {package: [srv_path]}, ext)
00834     for srv in srvs:
00835         load_srv_from_file(msg_context, '%s/%s%s'%(srv_path, srv, ext), '%s/%s'%(package, srv))
00836 
00837     ########################################
00838     # 1. Write the .lisp file
00839     ########################################
00840 
00841     io = StringIO()
00842     s = IndentedWriter(io)
00843     write_begin(s, spec, True)
00844     spec.request.actual_name='%s-request'%spec.short_name
00845     spec.response.actual_name='%s-response'%spec.short_name
00846     write_srv_component(s, spec.request, msg_context, spec)
00847     s.newline()
00848     write_srv_component(s, spec.response, msg_context, spec)
00849     write_service_specific_methods(s, spec)
00850     
00851     with open('%s/%s.lisp'%(output_dir, spec.short_name), 'w') as f:
00852         f.write(io.getvalue())
00853     io.close()
00854 
00855     ########################################
00856     # 2. Write the _package file
00857     # for this service
00858     ########################################
00859 
00860     io = StringIO()
00861     s = IndentedWriter(io)
00862     write_accessor_exports(s, spec)
00863     with open('%s/_package_%s.lisp'%(output_dir, spec.short_name), 'w') as f:
00864         f.write(io.getvalue())
00865     io.close()
00866 
00867     ########################################
00868     # 3. Write the _package.lisp file
00869     ########################################
00870 
00871     io = StringIO()
00872     s = IndentedWriter(io)
00873     write_srv_exports(s, srvs, package)
00874     with open('%s/_package.lisp'%output_dir, 'w') as f:
00875         f.write(io.getvalue())
00876     io.close()
00877 
00878     ########################################
00879     # 4. Write the .asd file
00880     ########################################
00881 
00882     io = StringIO()
00883     s = IndentedWriter(io)
00884     write_srv_asd(s, package, srvs, msg_context)
00885     with open('%s/%s-srv.asd'%(output_dir, package), 'w') as f:
00886         f.write(io.getvalue())
00887     io.close()
00888     
00889 


genlisp
Author(s): Bhaskara Marti
autogenerated on Sun Sep 4 2016 03:29:27